ADIS_Projects/ADIS_Csharp/RaspiControl/Application.cs

using NLog.LayoutRenderers;
using RobotLib.Status;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Text;
using System.Text.Json;
using System.Threading.Tasks;

namespace RaspiControl {

    internal class Application {
        private ChallengeFactory? challenge;
        private StateMachine sm;
        private readonly NLog.Logger log;
        private Stopwatch stopwatch;
        private uint PresentCounter;
        public Application() {
            this.log = NLog.LogManager.GetCurrentClassLogger();
            this.sm = new StateMachine();
            this.challenge = new ChallengeFactory();
            this.challenge.PublisherSubscriber.ConnectionStateChanged += this.PublisherSubscriber_ConnectionStateChanged;
            this.challenge.PublisherSubscriber.Connect("localhost", "ADIS", "TEST");
            this.challenge.RobotMobile.Status.StatusChanged += Status_StatusChanged;
            this.challenge.RobotMobile.Status.PresentChanged += Status_PresentChanged;
            Joystick joystick = new Joystick();
            joystick.JoystickChanged += Joystick_JoystickChanged;
            this.stopwatch = new Stopwatch();
            this.PresentCounter = 0;
        }

        public void Status_StatusChanged(object? sender, StatusEventArgs e) {
            try {
                if(e.Status == RoboStatus.Manual) {
                    this.sm.MoveNext(Command.manualMode);
                    this.smHandler();
                }
                if (e.Status == RoboStatus.Auto) {
                    this.sm.MoveNext(Command.autoMode);
                    this.smHandler();
                }
                if(e.Status == RoboStatus.Final) {
                    this.sm.MoveNext(Command.finished);
                    this.smHandler();
                }
                if(e.Status == RoboStatus.Failure) {
                    this.sm.MoveNext(Command.failure);
                    this.smHandler();
                }
            } catch (Exception ex) {
                this.log.Error(ex.Message);
            }
        }

        public void Status_PresentChanged(object? sender, PresentEventArgs e) { 
            if(this.sm.CurrentState == ProcessState.Auto) {
                this.PresentCounter++;
                this.challenge.RobotStationary.SplitFlap.Display(this.PresentCounter.ToString().PadLeft(4,'0'));
            }
        }

        public void Run() {
            bool success = this.challenge.PublisherSubscriber.Connect("localhost", "ADIS", "TEST");
            if (!success) {
                throw new Exception("Could not connect to broker on localhost");
            } else {
                try {
                    this.sm.MoveNext(Command.Connected);
                    this.log.Info($"Current State: {this.sm.CurrentState}");
                } catch (Exception ex) {
                    this.log.Error(ex.Message);
                }
            }
        }

        private void PublisherSubscriber_ConnectionStateChanged(object? sender, bool e) {
            if (e == false) {
                this.log.Error("Connection to MQTT broker lost");
            } else {
                this.log.Info("Connected to MQTT broker");
            }
        }

        private void smHandler() {
            this.log.Info($"Current State: {this.sm.CurrentState}");
            switch (this.sm.CurrentState) {
                case ProcessState.Startup:
                    break;
                case ProcessState.Init:
                    break;
                case ProcessState.Ready:
                    // show sf redy
                    this.challenge.RobotStationary.SplitFlap.Display("REDY");
                    break;
                case ProcessState.StartMoveManual:
                    // mode automatic:false
                    if (this.stopwatch.IsRunning) {
                        this.stopwatch.Stop();
                    }
                    this.stopwatch.Start();
                    this.challenge.RobotStationary.SplitFlap.Display("WALD");
                    this.challenge.RobotMobile.Movement.SetMobilityMode(false);
                    this.challenge.RobotMobile.Movement.AddSpeed(NavigationConstants.SPEED_FORWARD);
                    this.sm.MoveNext(Command.none);
                    break;
                case ProcessState.ResumeMoveManual:
                    this.challenge.RobotMobile.Movement.SetMobilityMode(false);
                    this.challenge.RobotMobile.Movement.AddSpeed(NavigationConstants.SPEED_FORWARD);
                    this.sm.MoveNext(Command.none);
                    break;
                case ProcessState.MoveManual:
                    break;
                case ProcessState.MoveManualF:
                    // move + 100
                    this.challenge.RobotMobile.Movement.AddSpeed(NavigationConstants.SPEED_FORWARD);
                    this.sm.MoveNext(Command.none);
                    break;
                case ProcessState.MoveManualB:
                    // move -100
                    this.challenge.RobotMobile.Movement.AddSpeed(NavigationConstants.SPEED_BACKWARD);
                    this.sm.MoveNext(Command.none);
                    break;
                case ProcessState.MoveManualL:
                    this.challenge.RobotMobile.Movement.Turn(NavigationConstants.TURN_ANGLE_LEFT);
                    this.sm.MoveNext(Command.none);
                    break;
                case ProcessState.MoveManualR:
                    // turn right 30
                    this.challenge.RobotMobile.Movement.Turn(NavigationConstants.TURN_ANGLE_RIGHT);
                    this.sm.MoveNext(Command.none);
                    break;
                case ProcessState.StopManual:
                    // stop
                    this.challenge.RobotMobile.Movement.Stop();
                    break;
                case ProcessState.Auto:
                    this.challenge.RobotStationary.SplitFlap.Display("AUTO");
                    break;
                case ProcessState.Final:
                    this.PresentCounter = 0;
                    this.stopwatch.Stop();
                    long elapsedTime = stopwatch.ElapsedMilliseconds / 1000;
                    this.challenge.RobotStationary.SplitFlap.Display(elapsedTime.ToString().PadLeft(4,'0'));
                    break;
                case ProcessState.Error:
                    //this.PresentCounter = 0;
                    break;
            }
        }

        private void Joystick_JoystickChanged(object sender, JoystickEventArgs e) {
            try {
                switch (e.Button) {
                    case JoystickButton.None:
                        break;
                    case JoystickButton.Left:
                        this.sm.MoveNext(Command.JoystickLeft);
                        break;
                    case JoystickButton.Right:
                        this.sm.MoveNext(Command.JoystickRight);
                        break;
                    case JoystickButton.Up:
                        this.sm.MoveNext(Command.JoystickUp);
                        break;
                    case JoystickButton.Down:
                        this.sm.MoveNext(Command.JoystickDown);
                        break;
                    case JoystickButton.Center:
                        this.sm.MoveNext(Command.JoystickCenter);
                        break;
                }
                if(e.Button != JoystickButton.None) {
                    this.smHandler();
                }
            } catch (Exception ex) {
                this.log.Error(ex.Message);
            }
        }
    }
}