ADIS_Projects/ADIS_Csharp/RaspiControl/Application.cs

using NLog.LayoutRenderers;
using System;
using System.Collections.Generic;
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;
        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");
            Joystick joystick = new Joystick();
            joystick.JoystickChanged += Joystick_JoystickChanged;
        }

        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 {
                this.sm.MoveNext(Command.Connected);
                this.log.Info($"Current State: {this.sm.CurrentState}");
            }
        }

        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
                    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:
                    break;
                case ProcessState.Final:
                    break;
                case ProcessState.Error:
                    break;
            }
        }

        private void Joystick_JoystickChanged(object sender, JoystickEventArgs e) {
            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;
            }
            this.smHandler();
        }
    }
}