From 41df48879d23896af3af49dc4ae896908ffe1dae Mon Sep 17 00:00:00 2001 From: Akram Date: Wed, 5 Jun 2024 11:02:20 +0200 Subject: [PATCH 01/58] Diagramms RL --- .../Application.plantuml | 103 ++++++++ .../DrivingState.plantuml | 105 ++++++++ .../ReinforcementLearning/ErrorState.plantuml | 52 ++++ .../LineSensorsCalibrationState.plantuml | 91 +++++++ .../MotorSpeedCalibrationState.plantuml | 85 ++++++ .../ReinforcementLearning/ReadyState.plantuml | 65 +++++ .../TrainigState.plantuml | 104 ++++++++ .../SystemStates.plantuml | 43 +++ .../MotorSpeedCalibrationState.h | 165 ++++++++++++ .../MotorSpeedCalibrationstate.cpp | 245 ++++++++++++++++++ lib/APPReinforcementLearning/StartupState.cpp | 201 ++++++++++++++ lib/APPReinforcementLearning/StartupState.h | 148 +++++++++++ 12 files changed, 1407 insertions(+) create mode 100644 doc/architecture/uml/LogicalView/ReinforcementLearning/Application.plantuml create mode 100644 doc/architecture/uml/LogicalView/ReinforcementLearning/DrivingState.plantuml create mode 100644 doc/architecture/uml/LogicalView/ReinforcementLearning/ErrorState.plantuml create mode 100644 doc/architecture/uml/LogicalView/ReinforcementLearning/LineSensorsCalibrationState.plantuml create mode 100644 doc/architecture/uml/LogicalView/ReinforcementLearning/MotorSpeedCalibrationState.plantuml create mode 100644 doc/architecture/uml/LogicalView/ReinforcementLearning/ReadyState.plantuml create mode 100644 doc/architecture/uml/LogicalView/ReinforcementLearning/TrainigState.plantuml create mode 100644 doc/architecture/uml/ProcessView/ReinforcementLearning/SystemStates.plantuml create mode 100644 lib/APPReinforcementLearning/MotorSpeedCalibrationState.h create mode 100644 lib/APPReinforcementLearning/MotorSpeedCalibrationstate.cpp create mode 100644 lib/APPReinforcementLearning/StartupState.cpp create mode 100644 lib/APPReinforcementLearning/StartupState.h diff --git a/doc/architecture/uml/LogicalView/ReinforcementLearning/Application.plantuml b/doc/architecture/uml/LogicalView/ReinforcementLearning/Application.plantuml new file mode 100644 index 00000000..0dda04dd --- /dev/null +++ b/doc/architecture/uml/LogicalView/ReinforcementLearning/Application.plantuml @@ -0,0 +1,103 @@ +@startuml + +title Application + +package "Application" as appLayer { + + class App <
> { + + setup() : void + + loop() : void + } + + note left of App + The program entry point. + end note + + class StateMachine <> { + + setState(state : IState*) : void + + getState() : IState* + + process() : void + } + + note left of StateMachine + The state machine executes always one + state exclusive. It is responsible to + enter/leave/process a state. + end note + + interface IState { + + {abstract} entry() : void + + {abstract} process(sm : StateMachine&) : void + + {abstract} exit() : void + } + + note left of IState + Defines the abstract state interface, + which forces every inherited class + to realize it. + end note + + class StartupState <> + class MotorSpeedCalibrationState <> + class LineSensorsCalibrationState <> + class ErrorState <> + class DringState <> + class ReadyState <> + class TrainingState <> + + note bottom of StartupState + The system starts up and shows + the Applikation name on the display. + end note + + note bottom of MotorSpeedCalibrationState + The robot drives with full speed forward + and with full speed backwar to determine + the max speed in steps/s. The slowest + motor is considered! + end note + + note bottom of LineSensorsCalibrationState + The robot turns several times the + line sensors over the track for + calibration. + end note + + note bottom of ErrorState + Error information is shown on display. + Confirmation from operator is requested. + end note + + note bottom of DringState + The robot follows the line. + end note + + note bottom of ReadyState + The robot is stopped and waits for + operator input. + end note + + note bottom of TrainingState + the robot trains to follow the line. + end note +} + +note top of appLayer + Hint: See the application state behaviour + in the corresponding state diagram. +end note + +App *--> StateMachine + +StateMachine o--> "0..1" IState + +IState <|.. StartupState: <> +IState <|.... MotorSpeedCalibrationState: <> +IState <|.. LineSensorsCalibrationState: <> +IState <|.... ErrorState: <> +IState <|.. ReadyState: <> +IState <|.... TrainingState: <> +IState <|.... DringState: <> + + +@enduml \ No newline at end of file diff --git a/doc/architecture/uml/LogicalView/ReinforcementLearning/DrivingState.plantuml b/doc/architecture/uml/LogicalView/ReinforcementLearning/DrivingState.plantuml new file mode 100644 index 00000000..4ab50d28 --- /dev/null +++ b/doc/architecture/uml/LogicalView/ReinforcementLearning/DrivingState.plantuml @@ -0,0 +1,105 @@ +@startuml + +title Driving State + +package "Application" as appLayer { + + class RacingState <> { + + {static} getInstance() : DrivingState + + entry() : void + + process(sm : StateMachine&) : void + + exit() : void + } + + class ReadyState <> + + RacingState .r.> ReadyState: <> + +} + +package "Service" as serviceLayer { + + class SimpleTimer <> { + + start(duration : uint32_t) : void + + restart() : void + + stop() : void + + isTimeout() : bool + } + class DifferentialDrive <> +} + +package "HAL" as hal { + + package "Interfaces" as halInterfaces { + interface IDisplay { + + {abstract} clear() : void + + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void + + {abstract} print(str : const String&) : size_t + + {abstract} print(str : const char[]) : size_t + + {abstract} print(value : uint8_t) : size_t + + {abstract} print(value : uint16_t) : size_t + + {abstract} print(value : uint32_t) : size_t + + {abstract} print(value : int8_t) : size_t + + {abstract} print(value : int16_t) : size_t + + {abstract} print(value : int32_t) : size_t + } + + interface ILineSensors { + + {abstract} init() : void + + {abstract} calibrate() : void + + {abstract} readLine() : int16_t + + {abstract} getSensorValues() : const uint16_t* + + {abstract} isCalibrationSuccessful() : bool + + {abstract} getCalibErrorInfo() const : uint8_t + + {abstract} getNumLineSensors() const : uint8_t + + {abstract} getSensorValueMax() const : uint16_t + } + + interface ILed { + + {abstract} enable(enableIt : bool) : void + } + + interface IButton { + + {abstract} isPressed() : bool + + {abstract} waitForRelease() : void + } + + } + + class Board << namespace >> { + + getDisplay() : IDisplay& + + getLineSensors() : ILineSensors& + + getLedYellow() : ILed& + + getButtonA() : IButton& + } + class WebotsSerialDrv { + + setRxChannel(channelId: int32_t) : void + + setTxChannel(channelId: int32_t ) : void + + print(str: const char[]) : void + + print(value: uint8_t ) : void + + print(value: uint16_t ) : void + + print(value: uint32_t ) : void + + print(value: int8_t ) : void + + print(value: int16_t ) : void + + print(value: int32_t ) : void + + println(str: const char[]) : void + + println(value: uint8_t ) : void + + println(value: uint16_t ) : void + + println(value: uint32_t ) : void + + println(value: int8_t ) : void + + println(value: int16_t ) : void + + println(value: int32_t ) : void + + write( buffer: const uint8_t*, length: size_t ) : size_t + + readBytes( buffer: uint8_t*, length: size_t ) : size_t + } +} + +RacingState *-> SimpleTimer +RacingState ..> DifferentialDrive: <> +RacingState ...> IDisplay: <> +RacingState ...> ILineSensors: <> +RacingState ...> ILed: <> +RacingState ...> Board: <> +RacingState ...>WebotsSerialDrv: <> +RacingState ...>IButton: <> +@enduml \ No newline at end of file diff --git a/doc/architecture/uml/LogicalView/ReinforcementLearning/ErrorState.plantuml b/doc/architecture/uml/LogicalView/ReinforcementLearning/ErrorState.plantuml new file mode 100644 index 00000000..49c0ce7c --- /dev/null +++ b/doc/architecture/uml/LogicalView/ReinforcementLearning/ErrorState.plantuml @@ -0,0 +1,52 @@ +@startuml + +title Error State + +package "Application" as appLayer { + + class ErrorState <> { + + {static} getInstance() : ErrorState + + entry() : void + + process(sm : StateMachine&) : void + + exit() : void + + setErrorMsg(msg : const String&) : void + } + + class StartupState <> + + ErrorState .r.> StartupState: <> +} + +package "HAL" as hal { + + package "Interfaces" as halInterfaces { + interface IDisplay { + + {abstract} clear() : void + + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void + + {abstract} print(str : const String&) : size_t + + {abstract} print(str : const char[]) : size_t + + {abstract} print(value : uint8_t) : size_t + + {abstract} print(value : uint16_t) : size_t + + {abstract} print(value : uint32_t) : size_t + + {abstract} print(value : int8_t) : size_t + + {abstract} print(value : int16_t) : size_t + + {abstract} print(value : int32_t) : size_t + } + + interface IButton { + + {abstract} isPressed() : bool + + {abstract} waitForRelease() : void + } + } + + class Board << namespace >> { + + getDisplay() : IDisplay& + + getButtonA() : IButton& + } +} + +ErrorState ..> Board: <> +ErrorState ..> IDisplay: <> +ErrorState ..> IButton: <> + +@enduml \ No newline at end of file diff --git a/doc/architecture/uml/LogicalView/ReinforcementLearning/LineSensorsCalibrationState.plantuml b/doc/architecture/uml/LogicalView/ReinforcementLearning/LineSensorsCalibrationState.plantuml new file mode 100644 index 00000000..8caa4082 --- /dev/null +++ b/doc/architecture/uml/LogicalView/ReinforcementLearning/LineSensorsCalibrationState.plantuml @@ -0,0 +1,91 @@ +@startuml + +title Line Sensors Calibration State + +package "Application" as appLayer { + + class LineSensorsCalibrationState <> { + + {static} getInstance() : LineSensorsCalibrationState + + entry() : void + + process(sm : StateMachine&) : void + + exit() : void + } + + class ReadyState <> + class ErrorState <> + + LineSensorsCalibrationState ..> ReadyState: <> + LineSensorsCalibrationState ..> ErrorState: <> +} + +package "Service" as serviceLayer { + + class SimpleTimer <> + class RelativeEncoder <> + class DifferentialDrive <><> + class Speedometer <><> + + DifferentialDrive ..> Speedometer: <> +} + +package "HAL" as hal { + + package "Interfaces" as halInterfaces { + interface IDisplay { + + {abstract} clear() : void + + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void + + {abstract} print(str : const String&) : size_t + + {abstract} print(str : const char[]) : size_t + + {abstract} print(value : uint8_t) : size_t + + {abstract} print(value : uint16_t) : size_t + + {abstract} print(value : uint32_t) : size_t + + {abstract} print(value : int8_t) : size_t + + {abstract} print(value : int16_t) : size_t + + {abstract} print(value : int32_t) : size_t + } + + interface IMotors { + + {abstract} setSpeeds(leftSpeed : int16_t, rightSpeed : int16_t) : void + + {abstract} getMaxSpeed() : int16_t + } + + interface ILineSensors { + + {abstract} init() : void + + {abstract} calibrate() : void + + {abstract} readLine() : int16_t + + {abstract} getSensorValues() : const uint16_t* + + {abstract} getNumLineSensors() const : uint8_t + + {abstract} getSensorValueMax() const : uint16_t + } + + interface IEncoders { + + {abstract} getCountsLeft() : int16_t + + {abstract} getCountsRight() : int16_t + + {abstract} getCountsAndResetLeft() : int16_t + + {abstract} getCountsAndResetRight() : int16_t + + {abstract} getResolution() const : uint16_t + } + } + + class Board << namespace >> { + + getDisplay() : IDisplay& + + getMotors() : IMotors& + + getLineSensors() : ILineSensors& + } +} + +appLayer -[hidden]-- serviceLayer +serviceLayer -[hidden]-- hal + +LineSensorsCalibrationState ....> IDisplay: <> +LineSensorsCalibrationState ....> ILineSensors: <> +LineSensorsCalibrationState ....> Board: <> +LineSensorsCalibrationState *--> SimpleTimer +LineSensorsCalibrationState *--> RelativeEncoder +LineSensorsCalibrationState ...> DifferentialDrive: <> + +DifferentialDrive ...> IMotors: <> +Speedometer ..> IEncoders: <> +Speedometer ..> IMotors: <> + +@enduml \ No newline at end of file diff --git a/doc/architecture/uml/LogicalView/ReinforcementLearning/MotorSpeedCalibrationState.plantuml b/doc/architecture/uml/LogicalView/ReinforcementLearning/MotorSpeedCalibrationState.plantuml new file mode 100644 index 00000000..dc6f3d8e --- /dev/null +++ b/doc/architecture/uml/LogicalView/ReinforcementLearning/MotorSpeedCalibrationState.plantuml @@ -0,0 +1,85 @@ +@startuml + +title MotorSpeedCalibration State + +package "Application" as appLayer { + + class MotorSpeedCalibrationState <> { + + {static} getInstance() : MotorSpeedCalibrationState + + entry() : void + + process(sm : StateMachine&) : void + + exit() : void + } + + class LineSensorsCalibrationState <> + class ErrorState <> + + MotorSpeedCalibrationState ..> LineSensorsCalibrationState: <> + MotorSpeedCalibrationState ..> ErrorState: <> +} + +package "Service" as serviceLayer { + + class SimpleTimer <> + class RelativeEncoder <> + class DifferentialDrive <> +} + +package "HAL" as hal { + + package "Interfaces" as halInterfaces { + interface IDisplay { + + {abstract} clear() : void + + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void + + {abstract} print(str : const String&) : size_t + + {abstract} print(str : const char[]) : size_t + + {abstract} print(value : uint8_t) : size_t + + {abstract} print(value : uint16_t) : size_t + + {abstract} print(value : uint32_t) : size_t + + {abstract} print(value : int8_t) : size_t + + {abstract} print(value : int16_t) : size_t + + {abstract} print(value : int32_t) : size_t + } + + interface IMotors { + + {abstract} setSpeeds(leftSpeed : int16_t, rightSpeed : int16_t) : void + + {abstract} getMaxSpeed() : int16_t + } + + interface ILineSensors { + + {abstract} init() : void + + {abstract} calibrate() : void + + {abstract} readLine() : int16_t + + {abstract} getSensorValues() : const uint16_t* + + {abstract} getNumLineSensors() const : uint8_t + + {abstract} getSensorValueMax() const : uint16_t + } + + interface ISettings { + + {abstract} init() : void + + {abstract} getMaxSpeed() : int16_t + + {abstract} setMaxSpeed(maxSpeed : int16_t) : void + } + } + + class Board << namespace >> { + + getDisplay() : IDisplay& + + getMotors() : IMotors& + + getLineSensors() : ILineSensors& + + getSettings() : ISettings& + } +} + +appLayer -[hidden]-- serviceLayer +serviceLayer -[hidden]-- hal + +MotorSpeedCalibrationState ....> IDisplay: <> +MotorSpeedCalibrationState ....> IMotors: <> +MotorSpeedCalibrationState ....> ILineSensors: <> +MotorSpeedCalibrationState ....> ISettings: <> +MotorSpeedCalibrationState ....> Board: <> +MotorSpeedCalibrationState *--> SimpleTimer +MotorSpeedCalibrationState *--> RelativeEncoder +MotorSpeedCalibrationState ...> DifferentialDrive: <> + +@enduml \ No newline at end of file diff --git a/doc/architecture/uml/LogicalView/ReinforcementLearning/ReadyState.plantuml b/doc/architecture/uml/LogicalView/ReinforcementLearning/ReadyState.plantuml new file mode 100644 index 00000000..2690db4e --- /dev/null +++ b/doc/architecture/uml/LogicalView/ReinforcementLearning/ReadyState.plantuml @@ -0,0 +1,65 @@ +@startuml + +title Ready State + +package "Application" as appLayer { + + class ReadyState <> { + + {static} getInstance() : ReadyState + + entry() : void + + process(sm : StateMachine&) : void + + exit() : void + + setLapTIme(lapTime : uint32_t) void + } + + class TrainingState <> + class RacingState <> + ReadyState .r.> RacingState: <> + ReadyState .l.> TrainingState: <> +} + +package "HAL" as hal { + + package "Interfaces" as halInterfaces { + interface IDisplay { + + {abstract} clear() : void + + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void + + {abstract} print(str : const String&) : size_t + + {abstract} print(str : const char[]) : size_t + + {abstract} print(value : uint8_t) : size_t + + {abstract} print(value : uint16_t) : size_t + + {abstract} print(value : uint32_t) : size_t + + {abstract} print(value : int8_t) : size_t + + {abstract} print(value : int16_t) : size_t + + {abstract} print(value : int32_t) : size_t + } + + interface IButton { + + {abstract} isPressed() : bool + + {abstract} waitForRelease() : void + } + + interface ILineSensors { + + {abstract} init() : void + + {abstract} calibrate() : void + + {abstract} readLine() : int16_t + + {abstract} getSensorValues() : const uint16_t* + + {abstract} getNumLineSensors() const : uint8_t + + {abstract} getSensorValueMax() const : uint16_t + } + } + + class Board << namespace >> { + + getDisplay() : IDisplay& + + getButtonA() : IButton& + + getButtonB() : IButton& + + getLineSensors() : ILineSensors& + } +} + +ReadyState ..> IDisplay: <> +ReadyState ..> IButton: <> +ReadyState ..> ILineSensors: <> +ReadyState ..> Board:: <> + +@enduml \ No newline at end of file diff --git a/doc/architecture/uml/LogicalView/ReinforcementLearning/TrainigState.plantuml b/doc/architecture/uml/LogicalView/ReinforcementLearning/TrainigState.plantuml new file mode 100644 index 00000000..9e74ff8e --- /dev/null +++ b/doc/architecture/uml/LogicalView/ReinforcementLearning/TrainigState.plantuml @@ -0,0 +1,104 @@ +@startuml + +title Training State + +package "Application" as appLayer { + + class TrainingState <> { + + {static} getInstance() : TrainingState + + entry() : void + + process(sm : StateMachine&) : void + + exit() : void + } + + class TrainingState <> + + TrainingState .r.> ReadyState: <> + +} + +package "Service" as serviceLayer { + + class SimpleTimer <> { + + start(duration : uint32_t) : void + + restart() : void + + stop() : void + + isTimeout() : bool + } + class DifferentialDrive <> +} + +package "HAL" as hal { + + package "Interfaces" as halInterfaces { + interface IDisplay { + + {abstract} clear() : void + + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void + + {abstract} print(str : const String&) : size_t + + {abstract} print(str : const char[]) : size_t + + {abstract} print(value : uint8_t) : size_t + + {abstract} print(value : uint16_t) : size_t + + {abstract} print(value : uint32_t) : size_t + + {abstract} print(value : int8_t) : size_t + + {abstract} print(value : int16_t) : size_t + + {abstract} print(value : int32_t) : size_t + } + + interface ILineSensors { + + {abstract} init() : void + + {abstract} calibrate() : void + + {abstract} readLine() : int16_t + + {abstract} getSensorValues() : const uint16_t* + + {abstract} isCalibrationSuccessful() : bool + + {abstract} getCalibErrorInfo() const : uint8_t + + {abstract} getNumLineSensors() const : uint8_t + + {abstract} getSensorValueMax() const : uint16_t + } + interface IButton { + + {abstract} isPressed() : bool + + {abstract} waitForRelease() : void + } + + interface ILed { + + {abstract} enable(enableIt : bool) : void + } + + } + + class Board << namespace >> { + + getDisplay() : IDisplay& + + getLineSensors() : ILineSensors& + + getLedYellow() : ILed& + + getButtonA() : IButton& + } + class WebotsSerialDrv { + + setRxChannel(channelId: int32_t) : void + + setTxChannel(channelId: int32_t ) : void + + print(str: const char[]) : void + + print(value: uint8_t ) : void + + print(value: uint16_t ) : void + + print(value: uint32_t ) : void + + print(value: int8_t ) : void + + print(value: int16_t ) : void + + print(value: int32_t ) : void + + println(str: const char[]) : void + + println(value: uint8_t ) : void + + println(value: uint16_t ) : void + + println(value: uint32_t ) : void + + println(value: int8_t ) : void + + println(value: int16_t ) : void + + println(value: int32_t ) : void + + write( buffer: const uint8_t*, length: size_t ) : size_t + + readBytes( buffer: uint8_t*, length: size_t ) : size_t + } +} + +TrainingState *-> SimpleTimer +TrainingState ..> DifferentialDrive: <> +TrainingState ...> IDisplay: <> +TrainingState ...> ILineSensors: <> +TrainingState ...> ILed: <> +TrainingState ...> Board: <> +TrainingState ...>WebotsSerialDrv: <> +TrainingState ...>IButton: <> +@enduml \ No newline at end of file diff --git a/doc/architecture/uml/ProcessView/ReinforcementLearning/SystemStates.plantuml b/doc/architecture/uml/ProcessView/ReinforcementLearning/SystemStates.plantuml new file mode 100644 index 00000000..716e9e3a --- /dev/null +++ b/doc/architecture/uml/ProcessView/ReinforcementLearning/SystemStates.plantuml @@ -0,0 +1,43 @@ +@startuml + +title System States + +state StartupState: /entry Initialize HAL. +state StartupState: /entry Show operator info on LCD. +state StartupState: /do Wait for pushbutton A or B is triggered. + +state MotorSpeedCalibrationState: /entry Show operator info on LCD. +state MotorSpeedCalibrationState: /do Perform calibration. + +state LineSensorsCalibrationState: /entry Show operator info on LCD. +state LineSensorsCalibrationState: /do Perform calibration. + +state ErrorState: /entry Show error info on LCD. +state ErrorState: /do Wait for pushbutton A is triggered. + +state ReadyState: /entry Show operator info on LCD. +state ReadyState: /do Wait for button A or B to be triggered. + +state TrainingState: /entry Show operator info on LCD. +state TrainingState: /entry Start observation timer. +state TrainingState: /do Perform training. +state DrivingState: /exit Stop observation timer. + +state DrivingState: /entry Show operator info on LCD. +state DrivingState: /entry Start observation timer. +state DrivingState: /do Perform driving. +state DrivingState: /exit Stop observation timer. + +[*] --> StartupState: Power up +StartupState --> MotorSpeedCalibrationState: [Pushbutton A triggered] +StartupState --> LineSensorsCalibrationState: [Pushbutton B triggered] and\n[Max. motor speed calib. is available in settings] +MotorSpeedCalibrationState --> LineSensorsCalibrationState: [Calibration finished] +LineSensorsCalibrationState --> ReadyState: [Calibration finished] +LineSensorsCalibrationState --> ErrorState: [Calibration failed] +ReadyState ---> DrivingState: [Pushbutton A triggered] +ReadyState --> TrainingState: [Pushbutton B triggered] +TrainingState --> ReadyState: [Pushbutton A triggered] or\n [End line detected] or\n[Observation timer timeout] +DrivingState ---> ReadyState: [End line detected] or\n[Pushbutton A triggered] or\n[Observation timer timeout] +ErrorState --> StartupState: [Pushbutton A triggered] + +@enduml \ No newline at end of file diff --git a/lib/APPReinforcementLearning/MotorSpeedCalibrationState.h b/lib/APPReinforcementLearning/MotorSpeedCalibrationState.h new file mode 100644 index 00000000..097cf12e --- /dev/null +++ b/lib/APPReinforcementLearning/MotorSpeedCalibrationState.h @@ -0,0 +1,165 @@ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Motor speed calibration state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef MOTOR_SPEED_CALIBRATION_STATE_H +#define MOTOR_SPEED_CALIBRATION_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The motor speed calibration state. */ +class MotorSpeedCalibrationState : public IState +{ +public: + /** + * Get state instance. + * + * @return State instance. + */ + static MotorSpeedCalibrationState& getInstance() + { + static MotorSpeedCalibrationState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + +protected: +private: + /** Calibration phases */ + enum Phase + { + PHASE_1_BACK, /**< Drive with max. speed backwards. */ + PHASE_2_FORWARD, /**< Drive with max. speed forwards. */ + PHASE_3_FINISHED /**< Calibration is finished. */ + }; + + /** + * Duration in ms about to wait, until the calibration drive starts. + */ + static const uint32_t WAIT_TIME = 1000; + + /** + * Calibration drive duration in ms. + * It means how long the robot is driven with max. speed forward/backward. + */ + static const uint32_t CALIB_DURATION = 1000; + + SimpleTimer m_timer; /**< Timer used to wait, until the calibration drive starts and for drive duration. */ + Phase m_phase; /**< Current calibration phase */ + int16_t m_maxSpeedLeft; /**< Max. determined left motor speed [steps/s]. */ + int16_t m_maxSpeedRight; /**< Max. determined right motor speed [steps/s]. */ + RelativeEncoders m_relEncoders; /**< Relative encoders left/right. */ + + /** + * Default constructor. + */ + MotorSpeedCalibrationState() : + m_timer(), + m_phase(PHASE_1_BACK), + m_maxSpeedLeft(0), + m_maxSpeedRight(0), + m_relEncoders(Board::getInstance().getEncoders()) + { + } + + /** + * Default destructor. + */ + ~MotorSpeedCalibrationState() + { + } + + /* Not allowed. */ + MotorSpeedCalibrationState(const MotorSpeedCalibrationState& state); /**< Copy construction of an instance. */ + MotorSpeedCalibrationState& operator=(const MotorSpeedCalibrationState& state); /**< Assignment of an instance. */ + + /** + * Determine the max. motor speed, considering both driving directions. + * There are two steps necessary: + * - Drive full backward and call this method to determine. + * - Drive full forward and call this method to determine. + */ + void determineMaxMotorSpeed(); + + /** + * Finish the calibration and determine next state. + * + * @param[in] sm State machine + */ + void finishCalibration(StateMachine& sm); +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* MOTOR_SPEED_CALIBRATION_STATE_H */ +/** @} */ diff --git a/lib/APPReinforcementLearning/MotorSpeedCalibrationstate.cpp b/lib/APPReinforcementLearning/MotorSpeedCalibrationstate.cpp new file mode 100644 index 00000000..5dec5290 --- /dev/null +++ b/lib/APPReinforcementLearning/MotorSpeedCalibrationstate.cpp @@ -0,0 +1,245 @@ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Motor speed calibration state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "MotorSpeedCalibrationState.h" +#include +#include +#include +#include +#include +#include +#include "LineSensorsCalibrationState.h" +#include "ErrorState.h" +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/** + * Logging source. + */ +LOG_TAG("MSCState"); + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void MotorSpeedCalibrationState::entry() +{ + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + IDisplay& display = Board::getInstance().getDisplay(); + + display.clear(); + display.print("Run"); + display.gotoXY(0, 1); + display.print("MCAL"); + + /* Disable differential drive to avoid any bad influence. */ + diffDrive.disable(); + + /* Setup relative encoders */ + m_relEncoders.clear(); + + /* Set the max. speeds to maximum of datatype, because during calibration + * the max. speed is determined by the lowest motor speed. + */ + m_maxSpeedLeft = INT16_MAX; + m_maxSpeedRight = INT16_MAX; + + /* Wait some time, before starting the calibration drive. */ + m_phase = PHASE_1_BACK; + m_timer.start(WAIT_TIME); +} + +void MotorSpeedCalibrationState::process(StateMachine& sm) +{ + if (true == m_timer.isTimeout()) + { + /* Control motors directly and not via differential drive control, + * because the differential drive control needs first to be updated + * regarding the max. possible motor speed in [steps/s] which is + * determined by this calibration. + */ + IMotors& motors = Board::getInstance().getMotors(); + + switch (m_phase) + { + case PHASE_1_BACK: + /* Drive full back. */ + motors.setSpeeds(-motors.getMaxSpeed(), -motors.getMaxSpeed()); + + m_timer.start(CALIB_DURATION); + m_phase = PHASE_2_FORWARD; + break; + + case PHASE_2_FORWARD: + motors.setSpeeds(0, 0); + determineMaxMotorSpeed(); + + /* Drive full forward. */ + motors.setSpeeds(motors.getMaxSpeed(), motors.getMaxSpeed()); + + m_timer.restart(); + m_phase = PHASE_3_FINISHED; + break; + + case PHASE_3_FINISHED: + motors.setSpeeds(0, 0); + determineMaxMotorSpeed(); + + m_timer.stop(); + finishCalibration(sm); + break; + + default: + break; + } + } +} + +void MotorSpeedCalibrationState::exit() +{ + /* Nothing to do. */ +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +void MotorSpeedCalibrationState::determineMaxMotorSpeed() +{ + int32_t stepsLeft = 0; + int32_t stepsRight = 0; + + /* Determine max. speed backward. */ + stepsLeft = abs(m_relEncoders.getCountsLeft()); + stepsRight = abs(m_relEncoders.getCountsRight()); + + /* Convert number of steps to [steps/s] */ + stepsLeft *= 1000; + stepsLeft /= CALIB_DURATION; + stepsRight *= 1000; + stepsRight /= CALIB_DURATION; + + if (INT16_MAX >= stepsLeft) + { + /* Use lower speed to ensure that motor speed can be reached in both + * directions. + */ + if (stepsLeft < m_maxSpeedLeft) + { + m_maxSpeedLeft = static_cast(stepsLeft); + } + } + + if (INT16_MAX >= stepsRight) + { + /* Use lower speed to ensure that motor speed can be reached in both + * directions. + */ + if (stepsRight < m_maxSpeedRight) + { + m_maxSpeedRight = static_cast(stepsRight); + } + } + + /* Clear relative encoders */ + m_relEncoders.clear(); +} + +void MotorSpeedCalibrationState::finishCalibration(StateMachine& sm) +{ + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + ISettings& settings = Board::getInstance().getSettings(); + + /* Set the lower speed as max. motor speed to ensure that both motors + * can reach the same max. speed. + */ + int16_t maxSpeed = (m_maxSpeedLeft < m_maxSpeedRight) ? m_maxSpeedLeft : m_maxSpeedRight; + + /* Store calibrated max. motor speed in the settings. */ + settings.setMaxSpeed(maxSpeed); + + /* With setting the max. motor speed in [steps/s] the differential drive control + * can now be used. + */ + diffDrive.setMaxMotorSpeed(maxSpeed); + + /* Differential drive can now be used. */ + diffDrive.enable(); + + if (0 == maxSpeed) + { + ErrorState::getInstance().setErrorMsg("EMCAL 0"); + sm.setState(&ErrorState::getInstance()); + } + else + { + int32_t maxSpeed32 = static_cast(maxSpeed) * 1000 / static_cast(RobotConstants::ENCODER_STEPS_PER_M); + + LOG_INFO_VAL("Calibrated max. speed (steps/s): ", maxSpeed); + LOG_INFO_VAL("Calibrated max. speed (mm/s): ", maxSpeed32); + + sm.setState(&LineSensorsCalibrationState::getInstance()); + } +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPReinforcementLearning/StartupState.cpp b/lib/APPReinforcementLearning/StartupState.cpp new file mode 100644 index 00000000..6bc5bee4 --- /dev/null +++ b/lib/APPReinforcementLearning/StartupState.cpp @@ -0,0 +1,201 @@ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Startup state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "StartupState.h" +#include +#include +#include "MotorSpeedCalibrationState.h" +#include "LineSensorsCalibrationState.h" +#include "Sound.h" +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void StartupState::entry() +{ + Board& board = Board::getInstance(); + ISettings& settings = board.getSettings(); + int16_t maxMotorSpeed = settings.getMaxSpeed(); + + /* If max. motor speed calibration value is available, the differential + * drive will be enabled. + */ + if (0 < maxMotorSpeed) + { + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + + /* With setting the max. motor speed in [steps/s] the differential drive control + * can now be used. + */ + diffDrive.setMaxMotorSpeed(maxMotorSpeed); + + /* Differential drive can now be used. */ + diffDrive.enable(); + + m_isMaxMotorSpeedCalibAvailable = true; + } + + showUserInfo(m_userInfoState); +} + +void StartupState::process(StateMachine& sm) +{ + Board& board = Board::getInstance(); + IButton& buttonA = board.getButtonA(); + + /* Start max. motor speed calibration? */ + if (true == buttonA.isPressed()) + { + buttonA.waitForRelease(); + sm.setState(&MotorSpeedCalibrationState::getInstance()); + } + + /* If the max. motor speed calibration is done, it will be possible to + * start the line sensor calibration immediately. + */ + if (true == m_isMaxMotorSpeedCalibAvailable) + { + IButton& buttonB = board.getButtonB(); + + /* Start line sensor calibration? */ + if (true == buttonB.isPressed()) + { + buttonB.waitForRelease(); + sm.setState(&LineSensorsCalibrationState::getInstance()); + } + } + + /* Periodically change the user info on the display. */ + if (true == m_timer.isTimeout()) + { + int8_t next = m_userInfoState + 1; + + if (USER_INFO_COUNT <= next) + { + next = 0; + } + + showUserInfo(static_cast(next)); + m_timer.restart(); + } +} + +void StartupState::exit() +{ + /* Next time start again from begin with the info. */ + m_userInfoState = USER_INFO_TEAM_NAME; +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +void StartupState::showUserInfo(UserInfo next) +{ + Board& board = Board::getInstance(); + IDisplay& display = board.getDisplay(); + ISettings& settings = board.getSettings(); + + display.clear(); + + switch (next) + { + case USER_INFO_TEAM_NAME: + display.print(TEAM_NAME_LINE_1); + display.gotoXY(0, 1); + display.print(TEAM_NAME_LINE_2); + break; + + case USER_INFO_MAX_MOTOR_SPEED: + display.print("maxSpeed"); + display.gotoXY(0, 1); + display.print(settings.getMaxSpeed()); + break; + + case USER_INFO_UI: + display.print("A: MCAL"); + + if (true == m_isMaxMotorSpeedCalibAvailable) + { + display.gotoXY(0, 1); + display.print("B: LCAL"); + } + break; + + case USER_INFO_COUNT: + /* fallthrough */ + default: + display.print("?"); + next = USER_INFO_TEAM_NAME; + break; + } + + m_userInfoState = next; + + m_timer.start(INFO_DURATION); +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPReinforcementLearning/StartupState.h b/lib/APPReinforcementLearning/StartupState.h new file mode 100644 index 00000000..f718292a --- /dev/null +++ b/lib/APPReinforcementLearning/StartupState.h @@ -0,0 +1,148 @@ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Startup state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef STARTUP_STATE_H +#define STARTUP_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The system startup state. */ +class StartupState : public IState +{ +public: + /** + * Get state instance. + * + * @return State instance. + */ + static StartupState& getInstance() + { + static StartupState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + +protected: +private: + /** + * This type defines different kind of information, which will be shown + * to the user in the same order as defined. + */ + enum UserInfo + { + USER_INFO_TEAM_NAME = 0, /**< Show the team name. */ + USER_INFO_MAX_MOTOR_SPEED, /**< Show the max. motor speed. */ + USER_INFO_UI, /**< Show the user interface. */ + USER_INFO_COUNT /**< Number of user infos. */ + }; + + /** + * Duration in ms how long a info on the display shall be shown, until + * the next info appears. + */ + static const uint32_t INFO_DURATION = 2000; + + bool m_isMaxMotorSpeedCalibAvailable; /**< Is max. motor speed calibration value available? */ + SimpleTimer m_timer; /**< Used to show information for a certain time before changing to the next info. */ + UserInfo m_userInfoState; /**< Current user info state. */ + + /** + * Default constructor. + */ + StartupState() : m_isMaxMotorSpeedCalibAvailable(false), m_timer(), m_userInfoState(USER_INFO_TEAM_NAME) + { + } + + /** + * Default destructor. + */ + ~StartupState() + { + } + + /* Not allowed. */ + StartupState(const StartupState& state); /**< Copy construction of an instance. */ + StartupState& operator=(const StartupState& state); /**< Assignment of an instance. */ + + /** + * Show next user info. + * + * @param[in] next Next user info which to show. + */ + void showUserInfo(UserInfo next); +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* STARTUP_STATE_H */ +/** @} */ From e4f8cc2422fd1d73b64e55f6ebbc568950d05197 Mon Sep 17 00:00:00 2001 From: akrambzeo Date: Tue, 2 Jul 2024 15:14:13 +0200 Subject: [PATCH 02/58] Added Serial Webots for Python --- .../controllers/Supervisor/Serial_webots.py | 94 +++++++++++++++++++ 1 file changed, 94 insertions(+) create mode 100644 webots/controllers/Supervisor/Serial_webots.py diff --git a/webots/controllers/Supervisor/Serial_webots.py b/webots/controllers/Supervisor/Serial_webots.py new file mode 100644 index 00000000..693f464b --- /dev/null +++ b/webots/controllers/Supervisor/Serial_webots.py @@ -0,0 +1,94 @@ +################################################################################ +# Imports +################################################################################ +from controller import device +################################################################################ +# Variables +################################################################################ + +################################################################################ +# Classes +################################################################################ + + +class SerialWebots: + """ + Class for Serial Webots Communication + """ + + def __init__(self, Emitter: device, Receiver: device)-> None: + """ SerialWebots Constructor. + + Parameters + ---------- + Emitter : device + Name of Emitter Device + Receiver : device + Name of Receiver Device + + """ + self.m_Emitter = Emitter + self.m_Receiver = Receiver + + + + def write(self, payload : bytearray ) -> int: + """ Sends Data to the Server. + + Parameters + ---------- + payload : bytearray + Payload to send. + + Returns + ---------- + Number of bytes sent + """ + + bytes_sent = 0 + + self.m_Emitter.send(bytes(payload)) + bytes_sent= len(payload) + + return bytes_sent + + def available(self) -> int: + """ Check if there is anything available for reading + + Returns + ---------- + Number of bytes available for reading. + """ + return self.m_Receiver.getQueueLength() + + def read_bytes(self) -> tuple[int, bytearray]: + """ Read a given number of Bytes from Serial. + + Returns + ---------- + Tuple: + - int: Number of bytes received. + - bytearray: Received data. + """ + rcvd_data = b'' + rcvd_data = self.m_Receiver.getBytes() + + return len(rcvd_data), rcvd_data + + + + + + + + + + + +################################################################################ +# Functions +################################################################################ + +################################################################################ +# Main +################################################################################ From 43f462e865f0c1e9add592d62b0aa9bc5d0c657d Mon Sep 17 00:00:00 2001 From: Akram Date: Tue, 9 Jul 2024 10:28:09 +0200 Subject: [PATCH 03/58] Fix issues in functions of Serial_webots class --- .../controllers/Supervisor/Serial_webots.py | 44 ++++++++++++++----- 1 file changed, 32 insertions(+), 12 deletions(-) diff --git a/webots/controllers/Supervisor/Serial_webots.py b/webots/controllers/Supervisor/Serial_webots.py index 693f464b..ffedddea 100644 --- a/webots/controllers/Supervisor/Serial_webots.py +++ b/webots/controllers/Supervisor/Serial_webots.py @@ -13,7 +13,7 @@ class SerialWebots: """ - Class for Serial Webots Communication + Serial Webots Communication Class """ def __init__(self, Emitter: device, Receiver: device)-> None: @@ -29,9 +29,9 @@ def __init__(self, Emitter: device, Receiver: device)-> None: """ self.m_Emitter = Emitter self.m_Receiver = Receiver + self.buffer = bytearray() - def write(self, payload : bytearray ) -> int: """ Sends Data to the Server. @@ -44,9 +44,6 @@ def write(self, payload : bytearray ) -> int: ---------- Number of bytes sent """ - - bytes_sent = 0 - self.m_Emitter.send(bytes(payload)) bytes_sent= len(payload) @@ -57,11 +54,16 @@ def available(self) -> int: Returns ---------- - Number of bytes available for reading. + Number of bytes that are available for reading. + """ - return self.m_Receiver.getQueueLength() + if len(self.buffer) > 0: + return len(self.buffer) + elif self.m_Receiver.getQueueLength() > 0: + return self.m_Receiver.getDataSize() + return 0 - def read_bytes(self) -> tuple[int, bytearray]: + def read_bytes(self, length: int) -> tuple[int, bytearray]: """ Read a given number of Bytes from Serial. Returns @@ -70,10 +72,28 @@ def read_bytes(self) -> tuple[int, bytearray]: - int: Number of bytes received. - bytearray: Received data. """ - rcvd_data = b'' - rcvd_data = self.m_Receiver.getBytes() - - return len(rcvd_data), rcvd_data + + read = 0 + data = bytearray() + + if len(self.buffer) > 0: + read = min(len(self.buffer), length) + data = self.buffer[:read] + self.buffer = self.buffer[read:] + elif self.m_Receiver.getQueueLength() > 0: + receivedData = self.m_Receiver.getBytes() + receivedData_size = self.m_Receiver.getDataSize() + self.m_Receiver.nextPacket() + + if receivedData_size > length: + data = receivedData[:length] + self.buffer = receivedData[length:] + read = length + else: + data = receivedData + read = receivedData_size + + return read, data From ef945a16866132b934b19a1c5543f582b7721d0c Mon Sep 17 00:00:00 2001 From: Akram Date: Tue, 9 Jul 2024 11:11:40 +0200 Subject: [PATCH 04/58] Serial Webots added to Supervisor --- webots/controllers/Supervisor/Supervisor.py | 226 ++++---------------- 1 file changed, 37 insertions(+), 189 deletions(-) diff --git a/webots/controllers/Supervisor/Supervisor.py b/webots/controllers/Supervisor/Supervisor.py index 6e528e82..2a7c72c7 100644 --- a/webots/controllers/Supervisor/Supervisor.py +++ b/webots/controllers/Supervisor/Supervisor.py @@ -1,11 +1,11 @@ """Supervisor controller. Details: https://github.com/cyberbotics/webots/blob/master/docs/guide/supervisor-programming.md """ - -import math import sys +from Serial_webots import SerialWebots +from SerialMuxProt import SerialMuxProt from controller import Supervisor -from robot_observer import RobotObserver + # The PROTO DEF name must be given! ROBOT_NAME = "ROBOT" @@ -13,122 +13,41 @@ # The supervisor receiver name. SUPERVISOR_RX_NAME = "serialComRx" -def serial_com_read(serial_com_rx_device): - """Read from serial communication device. - - Args: - serial_com_rx_device (obj): Serial COM device - - Returns: - Union[str, None]: Received data or None. - """ - rx_data = None - - if serial_com_rx_device.getQueueLength() > 0: - rx_data = serial_com_rx_device.getString() - serial_com_rx_device.nextPacket() - - return rx_data +# The supervisor Emitter name. +SUPERVISOR_TX_NAME = "serialComTx" -def rad_to_deg(angle_rad): - """Convert angle in rad to degree. +# Create the Supervisor instance. +supervisor = Supervisor() - Args: - angle_rad (float): Angle in rad +# Get the time step of the current world. +timestep = int(supervisor.getBasicTimeStep()) - Returns: - float: Angle in degree - """ - return angle_rad * 180 / math.pi - -def convert_angle_to_2pi(angle): - """Convert angle from range [-PI; PI] in rad to [0; 2PI]. - - Args: - angle (float): Angle in rad, range [-PI; PI]. +# Get the Supervisor Receiver Device +supervisor_com_rx = supervisor.getDevice(SUPERVISOR_RX_NAME) - Returns: - float: Angle in rad, range [0; 2PI] - """ - if angle < 0: - angle += 2 * math.pi +# Get the Supervisor Emitter device +supervisor_com_Tx = supervisor.getDevice(SUPERVISOR_TX_NAME) - return angle +# Enable supervisor receiver to receive any message from the robot or other +# devices in the simulation. +if supervisor_com_rx is None: + print(f"No supervisor communication possible, because {SUPERVISOR_RX_NAME} not found.") +else: + supervisor_com_rx.enable(timestep) -def convert_webots_angle_to_ru_angle(webots_angle): - """Convert Webots angle to RadonUlzer angle. +# Get robot node which to observe. +robot_node = supervisor.getFromDef(ROBOT_NAME) - Args: - webots_angle (float): Webots angle in rad, range [-PI; PI]. +# SerialMuxProt Server Instance. +S_client = SerialWebots(supervisor_com_Tx, supervisor_com_rx) +smp_server = SerialMuxProt(10, S_client) - Returns: - float: Angle in rad, range [-2PI; 2PI] - """ - # Radon Ulzer - # Angle [-2PI; 2PI] - # North are 90° (PI/2) - # - # Webots angle [-PI; PI] - webots_angle += math.pi / 2 - - webots_angle_2pi = convert_angle_to_2pi(webots_angle) - - # TODO Handling the negative range. - - return webots_angle_2pi - -def has_position_changed(position, position_old): - """Returns whether the position changed. - - Args: - position (list[float]): Position 1 - position_old (list[float]): Position 2 - - Returns: - bool: If changed, it will return True otherwise False. - """ - has_changed = False - - for idx, value in enumerate(position): - if int(value) != int(position_old[idx]): - has_changed = True - break - - return has_changed - -def has_orientation_changed(orientation, orientation_old): - """Returns whether the orientation changed. - - Args: - orientation (list[float]): Orientation 1 - orientation_old (list[float]): Orientation 2 - - Returns: - bool: If changed, it will return True otherwise False. - """ - has_changed = False - - for idx, value in enumerate(orientation): - if int(rad_to_deg(value)) != int(rad_to_deg(orientation_old[idx])): - has_changed = True - break - - return has_changed +# The PROTO DEF name must be given! +ROBOT_NAME = "ROBOT" -class OdometryData(): # pylint: disable=too-few-public-methods - """Odometry data container. - """ - def __init__(self) -> None: - self.x = 0 - self.y = 0 - self.yaw_angle = 0 - - def reset(self): - """Reset odometry data. - """ - self.x = 0 - self.y = 0 - self.yaw_angle = 0 +def callback_Status(payload: bytearray) -> None: + """ Callback Status Channel """ + print(payload) def main_loop(): """Main loop: @@ -138,91 +57,20 @@ def main_loop(): number: Status """ status = 0 - - # Create the Supervisor instance. - supervisor = Supervisor() - - # Get the time step of the current world. - timestep = int(supervisor.getBasicTimeStep()) - - # Enable supervisor receiver to receive any message from the robot or other - # devices in the simulation. - supervisor_com_rx = supervisor.getDevice(SUPERVISOR_RX_NAME) - - if supervisor_com_rx is None: - print(f"No supervisor communication possible, because {SUPERVISOR_RX_NAME} not found.") - else: - supervisor_com_rx.enable(timestep) - + m_elapsedTimeSinceReset = 0 + # Subscribe To Status Channel + smp_server.subscribe_to_channel("STATUS", callback_Status) + # Get robot node which to observe. robot_node = supervisor.getFromDef(ROBOT_NAME) - if robot_node is None: - print(f"Robot DEF {ROBOT_NAME} not found.") - status = -1 - + status = -1 else: - robot_observer = RobotObserver(robot_node) - - # Set current position and orientation in the map as reference. - robot_observer.set_current_position_as_reference() - robot_observer.set_current_orientation_as_reference() - - robot_position_old = robot_observer.get_rel_position() - robot_orientation_old = robot_observer.get_rel_orientation() - robot_odometry = OdometryData() - while supervisor.step(timestep) != -1: - if supervisor_com_rx is not None: - rx_data = serial_com_read(supervisor_com_rx) - - if rx_data is None: - pass - - else: - command = rx_data.split(',') - - # Reset robot position and orientation? - if command[0] == "RST": - print("RST") - robot_observer.set_current_position_as_reference() - robot_observer.set_current_orientation_as_reference() - robot_odometry.reset() - - # Robot odometry data received? - elif command[0] == "ODO": - robot_odometry.x = int(command[1]) # [mm] - robot_odometry.y = int(command[2]) # [mm] - robot_odometry.yaw_angle = float(command[3]) / 1000.0 # [rad] - - # Unknown command. - else: - print(f"Unknown command: {command[0]}") - - robot_position = robot_observer.get_rel_position() - robot_orientation = robot_observer.get_rel_orientation() - - any_change = has_position_changed(robot_position, robot_position_old) - - if any_change is False: - any_change = has_orientation_changed(robot_orientation, robot_orientation_old) - - if any_change is True: - - robot_yaw_angle = robot_orientation[2] - yaw_ru_angle = convert_webots_angle_to_ru_angle(robot_yaw_angle) - - print(f"{int(robot_position[0])}, ", end="") - print(f"{int(robot_position[1])}, ", end="") - print(f"{int(rad_to_deg(yaw_ru_angle))} / ", end="") - print(f"{robot_odometry.x}, ", end="") - print(f"{robot_odometry.y}, ", end="") - print(f"{int(rad_to_deg(robot_odometry.yaw_angle))}") - - robot_position_old = robot_position - robot_orientation_old = robot_orientation - + m_elapsedTimeSinceReset += timestep + smp_server.process(m_elapsedTimeSinceReset) + return status sys.exit(main_loop()) From 6131885303c5c518c239fea97b908be0dd7fd443 Mon Sep 17 00:00:00 2001 From: Akram Date: Tue, 9 Jul 2024 11:57:09 +0200 Subject: [PATCH 05/58] Copied error state from LineFollower APP without logging --- lib/APPReinforcementLearning/ErrorState.cpp | 131 +++++++++++++++++++ lib/APPReinforcementLearning/ErrorState.h | 134 ++++++++++++++++++++ 2 files changed, 265 insertions(+) create mode 100644 lib/APPReinforcementLearning/ErrorState.cpp create mode 100644 lib/APPReinforcementLearning/ErrorState.h diff --git a/lib/APPReinforcementLearning/ErrorState.cpp b/lib/APPReinforcementLearning/ErrorState.cpp new file mode 100644 index 00000000..2af70585 --- /dev/null +++ b/lib/APPReinforcementLearning/ErrorState.cpp @@ -0,0 +1,131 @@ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Error state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ErrorState.h" +#include +#include +#include "StartupState.h" +#include +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void ErrorState::entry() +{ + IDisplay& display = Board::getInstance().getDisplay(); + + DifferentialDrive::getInstance().disable(); + + display.clear(); + display.print("A: CONT"); + display.gotoXY(0, 1); + + if ('\0' == m_errorMsg[0]) + { + display.print("ERR"); + } + else + { + display.print(m_errorMsg); + } + +} + +void ErrorState::process(StateMachine& sm) +{ + IButton& buttonA = Board::getInstance().getButtonA(); + + /* Restart calibration? */ + if (true == buttonA.isPressed()) + { + buttonA.waitForRelease(); + sm.setState(&StartupState::getInstance()); + } +} + +void ErrorState::exit() +{ + /* Nothing to do. */ +} + +void ErrorState::setErrorMsg(const char* msg) +{ + if (nullptr == msg) + { + m_errorMsg[0] = '\0'; + } + else + { + strncpy(m_errorMsg, msg, ERROR_MSG_SIZE - 1); + m_errorMsg[ERROR_MSG_SIZE - 1] = '\0'; + } +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPReinforcementLearning/ErrorState.h b/lib/APPReinforcementLearning/ErrorState.h new file mode 100644 index 00000000..74095203 --- /dev/null +++ b/lib/APPReinforcementLearning/ErrorState.h @@ -0,0 +1,134 @@ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Error state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef ERROR_STATE_H +#define ERROR_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The system error state. */ +class ErrorState : public IState +{ +public: + /** + * Get state instance. + * + * @return State instance. + */ + static ErrorState& getInstance() + { + static ErrorState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + + /** + * Set error message, which to show on the display. + * + * @param[in] msg Error message + */ + void setErrorMsg(const char* msg); + +protected: +private: + /** + * The error message string size in bytes, which + * includes the terminating character. + */ + static const size_t ERROR_MSG_SIZE = 20; + + char m_errorMsg[ERROR_MSG_SIZE]; /**< Error message, which to show. */ + + /** + * Default constructor. + */ + ErrorState() : m_errorMsg() + { + m_errorMsg[0] = '\0'; + } + + /** + * Default destructor. + */ + ~ErrorState() + { + } + + /* Not allowed. */ + ErrorState(const ErrorState& state); /**< Copy construction of an instance. */ + ErrorState& operator=(const ErrorState& state); /**< Assignment of an instance. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* ERROR_STATE_H */ +/** @} */ From f627421f78c163529fbed06dfc6ba3759852435b Mon Sep 17 00:00:00 2001 From: Akram Date: Thu, 11 Jul 2024 15:32:22 +0200 Subject: [PATCH 06/58] Supervisor supports now serial communication over emitter/receiver. --- webots/protos/Supervisor.proto | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/webots/protos/Supervisor.proto b/webots/protos/Supervisor.proto index cb469f06..77cac08d 100644 --- a/webots/protos/Supervisor.proto +++ b/webots/protos/Supervisor.proto @@ -6,6 +6,7 @@ PROTO Supervisor [ field SFString controller "" field SFBool supervisor FALSE field SFString serialReceiver "serialComRx" + field SFString serialEmitter "serialComTx" ] { Robot { @@ -17,6 +18,10 @@ PROTO Supervisor [ name IS serialReceiver type "serial" } + Emitter { + name IS serialEmitter + type "serial" + } ] } } From 0f651ccdee4bfcc523604c9b960a04663e894d89 Mon Sep 17 00:00:00 2001 From: Akram Date: Fri, 12 Jul 2024 15:26:07 +0200 Subject: [PATCH 07/58] Added SerialMuxChannels --- .../SerialMuxchannels.h | 178 ++++++++++++++++++ 1 file changed, 178 insertions(+) create mode 100644 lib/APPReinforcementLearning/SerialMuxchannels.h diff --git a/lib/APPReinforcementLearning/SerialMuxchannels.h b/lib/APPReinforcementLearning/SerialMuxchannels.h new file mode 100644 index 00000000..c67822ef --- /dev/null +++ b/lib/APPReinforcementLearning/SerialMuxchannels.h @@ -0,0 +1,178 @@ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Channel structure definition for the SerialMuxProt. + * @author Gabryel Reyes + */ + +#ifndef SERIAL_MUX_CHANNELS_H_ +#define SERIAL_MUX_CHANNELS_H_ + +/****************************************************************************** + * Includes + *****************************************************************************/ + +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/** Maximum number of SerialMuxProt Channels. */ +#define MAX_CHANNELS (10U) + +/** Name of Channel to send Commands to. */ +#define COMMAND_CHANNEL_NAME "CMD" + +/** DLC of Command Channel. */ +#define COMMAND_CHANNEL_DLC (sizeof(Command)) + +/** Name of Channel to receive Command Responses from. */ +#define COMMAND_RESPONSE_CHANNEL_NAME "CMD_RSP" + +/** DLC of Command Response Channel. */ +#define COMMAND_RESPONSE_CHANNEL_DLC (sizeof(CommandResponse)) + +/** Name of Channel to send system status to. */ +#define STATUS_CHANNEL_NAME "STATUS" + +/** DLC of Status Channel */ +#define STATUS_CHANNEL_DLC (sizeof(Status)) + +/** Name of Channel to receive Motor Speed Setpoints. */ +#define SPEED_SETPOINT_CHANNEL_NAME "SPEED_SET" + +/** DLC of Speedometer Channel */ +#define SPEED_SETPOINT_CHANNEL_DLC (sizeof(SpeedData)) + +/** Name of the Channel to receive Line Sensor Data from. */ +#define LINE_SENSOR_CHANNEL_NAME "LINE_SENS" + +/** DLC of Line Sensor Channel */ +#define LINE_SENSOR_CHANNEL_DLC (sizeof(LineSensorData)) + +/** Name of channel to send system Mode to */ +#define MODE_CHANNEL_NAME "MODE" + +/** DLC of Mode Channel */ +#define MODE_CHANNEL_DLC (sizeof(MODE)) + + + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** SerialMuxProt Server with fixed template argument. */ +typedef SerialMuxProtServer SMPServer; + +/** Channel payload constants. */ +namespace SMPChannelPayload +{ + /** Remote control commands. */ + typedef enum : uint8_t + { + CMD_ID_IDLE = 0, /**< Nothing to do. */ + CMD_ID_REINIT_BOARD, /**< Re-initialize the board. Required for webots simulation. */ + CMD_ID_GET_MAX_SPEED, /**< Get maximum speed. */ + + } CmdId; /**< Command ID */ + + /** Remote control command responses. */ + typedef enum : uint8_t + { + RSP_ID_OK = 0, /**< Command successful executed. */ + RSP_ID_PENDING, /**< Command is pending. */ + RSP_ID_ERROR /**< Command failed. */ + + } RspId; /**< Response ID */ + /** Status flags. */ + typedef enum : uint8_t + { + NOT_DONE = 0, /**< End Line Not Detected. */ + DONE /**< End Line Detected. */ + + } Status; /**< Status flag */ + + typedef enum : uint8_t + { + DRIVING_MODE = 0, /**< Driving Mode Selected. */ + TRAINING_MODE /**< Training Mode Selected. */ + + } Mode; /**< Status flag */ + +} +/** Struct of the "Command" channel payload. */ +typedef struct _Command +{ + SMPChannelPayload::CmdId commandId; /**< Command ID */ + +} __attribute__((packed)) Command; + +/** Struct of the "Command Response" channel payload. */ +typedef struct _CommandResponse +{ + SMPChannelPayload::RspId responseId; /**< Response to the command */ + + /** Response Payload. */ + union + { + int16_t maxMotorSpeed; /**< Max speed [steps/s]. */ + }; +} __attribute__((packed)) CommandResponse; + +/** Struct of the "Speed" channel payload. */ +typedef struct _SpeedData +{ + int16_t left; /**< Left motor speed [steps/s] */ + int16_t right; /**< Right motor speed [steps/s] */ +} __attribute__((packed)) SpeedData; + +/** Struct of the "Mode" channel payload. */ +typedef struct _Mode +{ + SMPChannelPayload::Mode mode; /**< Mode */ +} __attribute__((packed)) Mode; + +/** Struct of the "Status" channel payload. */ +typedef struct _Status +{ + SMPChannelPayload::Status status; /**< Status */ +} __attribute__((packed)) Status; + +/** Struct of the "Line Sensor" channel payload. */ +typedef struct _LineSensorData +{ + uint16_t lineSensorData[5U]; /**< Line sensor data [digits] normalized to max 1000 digits. */ +} __attribute__((packed)) LineSensorData; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* SERIAL_MUX_CHANNELS_H_ */ \ No newline at end of file From 0071bc40e24cd8dbd632815b6c8f44d0d5770641 Mon Sep 17 00:00:00 2001 From: Akram Date: Mon, 15 Jul 2024 17:32:32 +0200 Subject: [PATCH 08/58] Aded Reinforcement Learning Application, Data exchange between supervisor and APP with the use of SerialMuxprot --- lib/APPReinforcementLearning/App.cpp | 235 +++++++ lib/APPReinforcementLearning/App.h | 137 ++++ lib/APPReinforcementLearning/DrivingState.cpp | 152 ++++ lib/APPReinforcementLearning/DrivingState.h | 171 +++++ lib/APPReinforcementLearning/IBoard.h | 205 ++++++ .../LineSensorsCalibrationState.cpp | 214 ++++++ .../LineSensorsCalibrationState.h | 161 +++++ lib/APPReinforcementLearning/ReadyState.cpp | 163 +++++ lib/APPReinforcementLearning/ReadyState.h | 168 +++++ .../SerialMuxchannels.h | 6 +- lib/APPReinforcementLearning/StartupState.cpp | 2 - platformio.ini | 94 ++- .../controllers/Supervisor/SerialMuxProt.py | 648 ++++++++++++++++++ 13 files changed, 2349 insertions(+), 7 deletions(-) create mode 100644 lib/APPReinforcementLearning/App.cpp create mode 100644 lib/APPReinforcementLearning/App.h create mode 100644 lib/APPReinforcementLearning/DrivingState.cpp create mode 100644 lib/APPReinforcementLearning/DrivingState.h create mode 100644 lib/APPReinforcementLearning/IBoard.h create mode 100644 lib/APPReinforcementLearning/LineSensorsCalibrationState.cpp create mode 100644 lib/APPReinforcementLearning/LineSensorsCalibrationState.h create mode 100644 lib/APPReinforcementLearning/ReadyState.cpp create mode 100644 lib/APPReinforcementLearning/ReadyState.h create mode 100644 webots/controllers/Supervisor/SerialMuxProt.py diff --git a/lib/APPReinforcementLearning/App.cpp b/lib/APPReinforcementLearning/App.cpp new file mode 100644 index 00000000..3b9aff48 --- /dev/null +++ b/lib/APPReinforcementLearning/App.cpp @@ -0,0 +1,235 @@ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief LineFollower application with Reinforcement Learning + * @author Akram Bziouech + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "App.h" +#include "DrivingState.h" +#include "StartupState.h" +#include +#include +#include +#include "ErrorState.h" +#include +#include +#include +#include +#include +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + + +/****************************************************************************** + * Prototypes + *****************************************************************************/ +static void App_motorSpeedSetpointsChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData); + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + + +void App::setup() +{ + Serial.begin(SERIAL_BAUDRATE); + /* Initialize HAL */ + Board::getInstance().init(); + Logging::disable(); + if (false == setupSerialMuxProt()) + { + ErrorState::getInstance().setErrorMsg("SMP=0"); + m_systemStateMachine.setState(&ErrorState::getInstance()); + } + else + { + /* Setup the state machine with the first state. */ + m_systemStateMachine.setState(&StartupState::getInstance()); + m_statusTimer.start(SEND_STATUS_TIMER_INTERVAL); + m_sendLineSensorsDataInterval.start(SEND_LINE_SENSORS_DATA_PERIOD); + /* Setup the periodically processing of robot control. */ + m_controlInterval.start(DIFFERENTIAL_DRIVE_CONTROL_PERIOD); + } + +#ifdef DEBUG_ODOMETRY + /* Reset supervisor which set its observed position and orientation to 0. */ + Board::getInstance().getSender().send("RST"); +#endif /* DEBUG_ODOMETRY */ + + /* Surprise the audience. */ + Sound::playMelody(Sound::MELODY_WELCOME); +} + +void App::loop() +{ + Board::getInstance().process(); + Speedometer::getInstance().process(); + + if (true == m_controlInterval.isTimeout()) + { + /* The differential drive control needs the measured speed of the + * left and right wheel. Therefore it shall be processed after + * the speedometer. + */ + DifferentialDrive::getInstance().process(DIFFERENTIAL_DRIVE_CONTROL_PERIOD); + + /* The odometry unit needs to detect motor speed changes to be able to + * calculate correct values. Therefore it shall be processed right after + * the differential drive control. + */ + Odometry::getInstance().process(); + +#ifdef DEBUG_ODOMETRY + { + Odometry& odo = Odometry::getInstance(); + int32_t posX = 0; + int32_t posY = 0; + int32_t orientation = odo.getOrientation(); + const size_t BUFFER_SIZE = 128U; + char buffer[BUFFER_SIZE]; + + odo.getPosition(posX, posY); + + snprintf(buffer, BUFFER_SIZE, "ODO,%d,%d,%d", posX, posY, orientation); + + Board::getInstance().getSender().send(buffer); + } +#endif /* DEBUG_ODOMETRY */ + + m_controlInterval.restart(); + } + + if ((true == m_statusTimer.isTimeout()) && (true == m_smpServer.isSynced())&& (&DrivingState::getInstance() == m_systemStateMachine.getState())) + { + Status payload = {SMPChannelPayload::Status::NOT_DONE}; + ILineSensors& lineSensors = Board::getInstance().getLineSensors(); + uint8_t maxLineSensors = lineSensors.getNumLineSensors(); + const uint16_t* lineSensorValues = lineSensors.getSensorValues(); + bool m_isTrackLost = DrivingState::getInstance().isNoLineDetected(lineSensorValues, maxLineSensors); + if (DrivingState::getInstance().isAbortRequired(m_isTrackLost) == true) + { + payload = {SMPChannelPayload::Status::DONE}; + m_systemStateMachine.setState(&ReadyState::getInstance()); + } + (void)m_smpServer.sendData(m_serialMuxProtChannelIdStatus, &payload, sizeof(payload)); + m_statusTimer.restart(); + } + /* Send periodically line sensor data. */ + if (true == m_sendLineSensorsDataInterval.isTimeout() && (&DrivingState::getInstance() == m_systemStateMachine.getState()) ) + { + sendLineSensorsData(); + + m_sendLineSensorsDataInterval.restart(); + } + + if (&ReadyState::getInstance() == m_systemStateMachine.getState()) + { + Mode payload = {SMPChannelPayload::Mode::TRAINING_MODE}; + if(ReadyState::getInstance().selectedMode() == 0) + { + payload = {SMPChannelPayload::Mode::DRIVING_MODE}; + } + (void)m_smpServer.sendData(m_serialMuxProtChannelIdMode, &payload, sizeof(payload)); + } + + + m_smpServer.process(millis()); + + m_systemStateMachine.process(); +} + + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +bool App::setupSerialMuxProt() +{ + bool isSuccessful = false; + m_smpServer.subscribeToChannel(SPEED_SETPOINT_CHANNEL_NAME, App_motorSpeedSetpointsChannelCallback); + /* Channel creation. */ + m_serialMuxProtChannelIdStatus = m_smpServer.createChannel(STATUS_CHANNEL_NAME, STATUS_CHANNEL_DLC); + m_serialMuxProtChannelIdLineSensors = m_smpServer.createChannel(LINE_SENSOR_CHANNEL_NAME, LINE_SENSOR_CHANNEL_DLC); + m_serialMuxProtChannelIdMode = m_smpServer.createChannel(MODE_CHANNEL_NAME, MODE_CHANNEL_DLC); + + + /* Channels succesfully created? */ + if ((0U != m_serialMuxProtChannelIdStatus) && (0U != m_serialMuxProtChannelIdLineSensors) && (0U != m_serialMuxProtChannelIdMode)) + { + isSuccessful = true; + } + + return isSuccessful; +} + +void App::sendLineSensorsData() const +{ + ILineSensors& lineSensors = Board::getInstance().getLineSensors(); + uint8_t maxLineSensors = lineSensors.getNumLineSensors(); + const uint16_t* lineSensorValues = lineSensors.getSensorValues(); + uint8_t lineSensorIdx = 0U; + LineSensorData payload; + + if (LINE_SENSOR_CHANNEL_DLC == maxLineSensors * sizeof(uint16_t)) + { + while (maxLineSensors > lineSensorIdx) + { + payload.lineSensorData[lineSensorIdx] = lineSensorValues[lineSensorIdx]; + + ++lineSensorIdx; + } + } + + (void)m_smpServer.sendData(m_serialMuxProtChannelIdLineSensors, &payload, sizeof(payload)); +} + + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ + +void App_motorSpeedSetpointsChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData) +{ + + (void)userData; + if ((nullptr != payload) && (SPEED_SETPOINT_CHANNEL_DLC == payloadSize)) + { + StateMachine m_systemStateMachine; + const SpeedData* motorSpeedData = reinterpret_cast(payload); + DrivingState::getInstance().setTargetSpeeds(motorSpeedData->left, motorSpeedData->right); + if((motorSpeedData->left == 0) && (motorSpeedData->right == 0)) + { + m_systemStateMachine.setState(&StartupState::getInstance()); + printf("StartupState"); + } + } +} \ No newline at end of file diff --git a/lib/APPReinforcementLearning/App.h b/lib/APPReinforcementLearning/App.h new file mode 100644 index 00000000..193347ed --- /dev/null +++ b/lib/APPReinforcementLearning/App.h @@ -0,0 +1,137 @@ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief LineFollower application + * @author Akram Bziouech + * + * @addtogroup Application + * + * @{ + */ + +#ifndef APP_H +#define APP_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "SerialMuxChannels.h" +#include +#include +#include + + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The line follower application. */ +class App +{ +public: + /** + * Construct the line follower application. + */ + App() : + m_systemStateMachine(), + m_controlInterval(), + m_serialMuxProtChannelIdStatus(0U), + m_serialMuxProtChannelIdLineSensors(0U), + m_serialMuxProtChannelIdMode(0U), + m_statusTimer(), + m_sendLineSensorsDataInterval(), + m_smpServer(Serial, nullptr) + { + } + + /** + * Destroy the line follower application. + */ + ~App() + { + } + + /** + * Setup the application. + */ + void setup(); + + /** + * Process the application periodically. + */ + void loop(); + +private: + /** Differential drive control period in ms. */ + static const uint32_t DIFFERENTIAL_DRIVE_CONTROL_PERIOD = 5U; + + /** Baudrate for Serial Communication */ + static const uint32_t SERIAL_BAUDRATE = 115200U; + + /** The system state machine. */ + StateMachine m_systemStateMachine; + + /** Timer used for differential drive control processing. */ + SimpleTimer m_controlInterval; + + /** + * Timer for sending system status to DCS. + */ + SimpleTimer m_statusTimer; + + /** Timer used for sending data periodically. */ + SimpleTimer m_sendLineSensorsDataInterval; + + /** Send status timer interval in ms. */ + static const uint32_t SEND_STATUS_TIMER_INTERVAL = 24U; + + /** Sending Data period in ms. */ + static const uint32_t SEND_LINE_SENSORS_DATA_PERIOD = 24U; + + /** SerialMuxProt Channel id for sending system status. */ + uint8_t m_serialMuxProtChannelIdStatus; + + /** SerialMuxProt Channel id for sending LineSensors. */ + uint8_t m_serialMuxProtChannelIdLineSensors; + + /** SerialMuxProt Channel id for sending Mode Selected. */ + uint8_t m_serialMuxProtChannelIdMode; + + /** SerialMuxProt Server Instance. */ + SMPServer m_smpServer; + + + /** + * Setup the SerialMuxProt channels. + * + * @return If successful returns true, otherwise false. + */ + bool setupSerialMuxProt(); + + /** + * Send line sensors data via SerialMuxProt. + */ + void sendLineSensorsData() const; + + + /* Not allowed. */ + App(const App& app); /**< Copy construction of an instance. */ + App& operator=(const App& app); /**< Assignment of an instance. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* APP_H */ +/** @} */ diff --git a/lib/APPReinforcementLearning/DrivingState.cpp b/lib/APPReinforcementLearning/DrivingState.cpp new file mode 100644 index 00000000..2bdefdef --- /dev/null +++ b/lib/APPReinforcementLearning/DrivingState.cpp @@ -0,0 +1,152 @@ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Driving state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "DrivingState.h" +#include +#include +#include +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/* Initialize the required sensor IDs to be generic. */ +const uint8_t DrivingState::SENSOR_ID_MOST_LEFT = 0U; +const uint8_t DrivingState::SENSOR_ID_MIDDLE = Board::getInstance().getLineSensors().getNumLineSensors() / 2U; +const uint8_t DrivingState::SENSOR_ID_MOST_RIGHT = Board::getInstance().getLineSensors().getNumLineSensors() - 1U; +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void DrivingState::entry() +{ + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + + diffDrive.setLinearSpeed(0, 0); + diffDrive.enable(); +} + +void DrivingState::process(StateMachine& sm) +{ + /* Nothing to do. */ + (void)sm; +} + +void DrivingState::exit() +{ + + /* Stop motors. */ + DifferentialDrive::getInstance().setLinearSpeed(0, 0); + DifferentialDrive::getInstance().disable(); +} + +void DrivingState::setTargetSpeeds(int16_t leftMotor, int16_t rightMotor) +{ + DifferentialDrive::getInstance().setLinearSpeed(leftMotor, rightMotor); +} + + +bool DrivingState::isNoLineDetected(const uint16_t* lineSensorValues, uint8_t length) const +{ + bool isDetected = true; + uint8_t idx = SENSOR_ID_MOST_RIGHT; + + /* + * + * + + + + + + * L M R + */ + for (idx = SENSOR_ID_MOST_LEFT; idx <= SENSOR_ID_MOST_RIGHT; ++idx) + { + if (LINE_SENSOR_ON_TRACK_MIN_VALUE <= lineSensorValues[idx]) + { + isDetected = false; + break; + } + } + + return isDetected; +} + +bool DrivingState::isAbortRequired(bool m_isTrackLost) +{ + bool isAbort = false; + + /* If track is lost over a certain distance, abort driving. */ + if (true == m_isTrackLost) + { + /* Max. distance driven, but track still not found? */ + if (MAX_DISTANCE < Odometry::getInstance().getMileageCenter()) + { + isAbort = true; + } + } + return isAbort; +} + + + + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPReinforcementLearning/DrivingState.h b/lib/APPReinforcementLearning/DrivingState.h new file mode 100644 index 00000000..d8599bae --- /dev/null +++ b/lib/APPReinforcementLearning/DrivingState.h @@ -0,0 +1,171 @@ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Driving state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef DRIVING_STATE_H +#define DRIVING_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The system driving state. */ +class DrivingState : public IState +{ +public: + /** + * Get state instance. + * + * @return State instance. + */ + static DrivingState& getInstance() + { + static DrivingState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + + /** + * Set target motor speeds. + * + * @param[in] leftMotor Left motor speed. [steps/s] + * @param[in] rightMotor Right motor speed. [steps/s] + */ + void setTargetSpeeds(int16_t leftMotor, int16_t rightMotor); + + /** + * Is no line detected? + * + * @param[in] lineSensorValues The line sensor values as array. + * @param[in] length The number of line sensor values. + * + * @return If no line is detected, it will return true otherwise false. + */ + + bool isNoLineDetected(const uint16_t* lineSensorValues, uint8_t length) const; + /** + * Check the abort conditions while driving the challenge. + * + * @return If abort is required, it will return true otherwise false. + */ + bool isAbortRequired(bool m_isTrackLost); + +protected: +private: + /** + * The line sensor threshold (normalized) used to detect the track. + * The track is detected in case the received value is greater or equal than + * the threshold. + */ + static const uint16_t LINE_SENSOR_ON_TRACK_MIN_VALUE = 200U; + + /** Max. distance in mm after a lost track must be found again. */ + static const uint32_t MAX_DISTANCE = 200; + /** + * ID of most left sensor. + */ + static const uint8_t SENSOR_ID_MOST_LEFT; + + /** + * ID of most right sensor. + */ + static const uint8_t SENSOR_ID_MIDDLE; + + /** + * ID of middle sensor. + */ + static const uint8_t SENSOR_ID_MOST_RIGHT; + + /** + * Default constructor. + */ + DrivingState() : IState() + { + } + + /** + * Default destructor. + */ + ~DrivingState() + { + } + + /* Not allowed. */ + DrivingState(const DrivingState& state); /**< Copy construction of an instance. */ + DrivingState& operator=(const DrivingState& state); /**< Assignment of an instance. */ + + + +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* DRIVING_STATE_H */ +/** @} */ diff --git a/lib/APPReinforcementLearning/IBoard.h b/lib/APPReinforcementLearning/IBoard.h new file mode 100644 index 00000000..68c58215 --- /dev/null +++ b/lib/APPReinforcementLearning/IBoard.h @@ -0,0 +1,205 @@ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Board interface, which abstracts the physical board + * @author Andreas Merkle + * + * @addtogroup HALInterfaces + * + * @{ + */ + +#ifndef IBOARD_H +#define IBOARD_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef DEBUG_ODOMETRY +#include +#endif /* DEBUG_ODOMETRY */ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** + * Abstracts the physical board interface. + */ +class IBoard +{ +public: + + /** + * Destroys the board interface. + */ + virtual ~IBoard() + { + } + + /** + * Initialize the hardware. + */ + virtual void init() = 0; + + /** + * Get button A driver. + * + * @return Button A driver. + */ + virtual IButton& getButtonA() = 0; + + /** + * Get button B driver. + * + * @return Button B driver. + */ + virtual IButton& getButtonB() = 0; + + /** + * Get button C driver. + * + * @return Button C driver. + */ + virtual IButton& getButtonC() = 0; + + /** + * Get buzzer driver. + * + * @return Buzzer driver. + */ + virtual IBuzzer& getBuzzer() = 0; + + /** + * Get LCD driver. + * + * @return LCD driver. + */ + virtual IDisplay& getDisplay() = 0; + + /** + * Get encoders driver. + * + * @return Encoders driver. + */ + virtual IEncoders& getEncoders() = 0; + + /** + * Get line sensors driver. + * + * @return Line sensor driver. + */ + virtual ILineSensors& getLineSensors() = 0; + + /** + * Get motor driver. + * + * @return Motor driver. + */ + virtual IMotors& getMotors() = 0; + + /** + * Get red LED driver. + * + * @return Red LED driver. + */ + virtual ILed& getRedLed() = 0; + + /** + * Get yellow LED driver. + * + * @return Yellow LED driver. + */ + virtual ILed& getYellowLed() = 0; + + /** + * Get green LED driver. + * + * @return Green LED driver. + */ + virtual ILed& getGreenLed() = 0; + + /** + * Get the settings. + * + * @return Settings + */ + virtual ISettings& getSettings() = 0; + +#ifdef DEBUG_ODOMETRY + + /** + * Get the sender driver, used to send data to the webots supervisor. + * + * @return Sender driver + */ + virtual ISender& getSender() = 0; + +#endif /* DEBUG_ODOMETRY */ + + /** + * Process actuators and sensors. + */ + virtual void process() = 0; + +protected: + + /** + * Constructs the board interface. + */ + IBoard() + { + } + +private: + +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* IBOARD_H */ +/** @} */ diff --git a/lib/APPReinforcementLearning/LineSensorsCalibrationState.cpp b/lib/APPReinforcementLearning/LineSensorsCalibrationState.cpp new file mode 100644 index 00000000..378d0998 --- /dev/null +++ b/lib/APPReinforcementLearning/LineSensorsCalibrationState.cpp @@ -0,0 +1,214 @@ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Line sensors calibration state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "LineSensorsCalibrationState.h" +#include +#include +#include +#include +#include +#include "ReadyState.h" +#include "ErrorState.h" +#include "DrivingState.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void LineSensorsCalibrationState::entry() +{ + IDisplay& display = Board::getInstance().getDisplay(); + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + Odometry& odometry = Odometry::getInstance(); + ILineSensors& lineSensors = Board::getInstance().getLineSensors(); + + display.clear(); + display.print("Run"); + display.gotoXY(0, 1); + display.print("LCAL"); + + /* Prepare calibration drive. */ + m_calibrationSpeed = diffDrive.getMaxMotorSpeed() / 4; + m_orientation = odometry.getOrientation(); + + /* Mandatory for each new calibration. */ + lineSensors.resetCalibration(); + + /* Wait some time, before starting the calibration drive. */ + m_phase = PHASE_1_WAIT; + m_timer.start(WAIT_TIME); +} + +void LineSensorsCalibrationState::process(StateMachine& sm) +{ + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + + switch (m_phase) + { + case PHASE_1_WAIT: + if (true == m_timer.isTimeout()) + { + m_phase = PHASE_2_TURN_LEFT; + diffDrive.setLinearSpeed(-m_calibrationSpeed, m_calibrationSpeed); + } + break; + + case PHASE_2_TURN_LEFT: + if (true == turnAndCalibrate(CALIB_ANGLE, true)) + { + m_phase = PHASE_3_TURN_RIGHT; + diffDrive.setLinearSpeed(m_calibrationSpeed, -m_calibrationSpeed); + } + break; + + case PHASE_3_TURN_RIGHT: + if (true == turnAndCalibrate(-CALIB_ANGLE, false)) + { + m_phase = PHASE_4_TURN_ORIG; + diffDrive.setLinearSpeed(-m_calibrationSpeed, m_calibrationSpeed); + } + break; + + case PHASE_4_TURN_ORIG: + if (true == turnAndCalibrate(0, true)) + { + m_phase = PHASE_5_FINISHED; + diffDrive.setLinearSpeed(0, 0); + finishCalibration(sm); + } + break; + + case PHASE_5_FINISHED: + /* fallthrough */ + default: + break; + } +} + +void LineSensorsCalibrationState::exit() +{ + m_timer.stop(); +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +bool LineSensorsCalibrationState::turnAndCalibrate(int32_t calibAlpha, bool isGreaterEqual) +{ + ILineSensors& lineSensors = Board::getInstance().getLineSensors(); + Odometry& odometry = Odometry::getInstance(); + int32_t alpha = odometry.getOrientation() - m_orientation; /* [mrad] */ + bool isSuccesful = false; + + /* Continously calibrate the line sensors. */ + lineSensors.calibrate(); + + /* Is the goal that the current angle shall be lower or equal than the destination calibration angle? */ + if (false == isGreaterEqual) + { + /* Is alpha lower or equal than the destination calibration angle? */ + if (calibAlpha >= alpha) + { + isSuccesful = true; + } + } + else + { + /* Is alpha greater or equal than the destination calibration angle? */ + if (calibAlpha <= alpha) + { + isSuccesful = true; + } + } + + return isSuccesful; +} + +void LineSensorsCalibrationState::finishCalibration(StateMachine& sm) +{ + ILineSensors& lineSensors = Board::getInstance().getLineSensors(); + + if (false == lineSensors.isCalibrationSuccessful()) + { + char str[10]; + char valueStr[10]; + + Util::uintToStr(valueStr, sizeof(valueStr), lineSensors.getCalibErrorInfo()); + + strncpy(str, "ELCAL ", sizeof(str) - 1); + str[sizeof(str) - 1] = '\0'; + + strncat(str, valueStr, sizeof(str) - strlen(valueStr) - 1); + + ErrorState::getInstance().setErrorMsg(str); + sm.setState(&ErrorState::getInstance()); + } + else + { + sm.setState(&ReadyState::getInstance()); + } +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPReinforcementLearning/LineSensorsCalibrationState.h b/lib/APPReinforcementLearning/LineSensorsCalibrationState.h new file mode 100644 index 00000000..564e7508 --- /dev/null +++ b/lib/APPReinforcementLearning/LineSensorsCalibrationState.h @@ -0,0 +1,161 @@ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Line sensors calibration state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef LINE_SENSORS_CALIBRATION_STATE_H +#define LINE_SENSORS_CALIBRATION_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The line sensors calibration state. */ +class LineSensorsCalibrationState : public IState +{ +public: + /** + * Get state instance. + * + * @return State instance. + */ + static LineSensorsCalibrationState& getInstance() + { + static LineSensorsCalibrationState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + +protected: +private: + /** Calibration phases */ + enum Phase + { + PHASE_1_WAIT = 0, /**< Wait a short time before starting the calibration. */ + PHASE_2_TURN_LEFT, /**< Turn line sensors left over the line. */ + PHASE_3_TURN_RIGHT, /**< Turn line sensor right over the line. */ + PHASE_4_TURN_ORIG, /**< Turn back to origin. */ + PHASE_5_FINISHED /**< Calibration is finished. */ + }; + + /** + * Duration in ms about to wait, until the calibration drive starts. + */ + static const uint32_t WAIT_TIME = 1000; + + /** + * Calibration turn angle in mrad (corresponds to 72°). + */ + static const int32_t CALIB_ANGLE = (FP_2PI() / 5); + + SimpleTimer m_timer; /**< Timer used to wait, until the calibration drive starts. */ + Phase m_phase; /**< Current calibration phase */ + int16_t m_calibrationSpeed; /**< Speed in steps/s for the calibration drive. */ + int32_t m_orientation; /**< Orientation at the begin of the calibration in [mrad]. */ + + /** + * Default constructor. + */ + LineSensorsCalibrationState() : m_timer(), m_phase(PHASE_1_WAIT), m_calibrationSpeed(0), m_orientation(0) + { + } + + /** + * Default destructor. + */ + ~LineSensorsCalibrationState() + { + } + + /* Not allowed. */ + LineSensorsCalibrationState(const LineSensorsCalibrationState& state); /**< Copy construction of an instance. */ + LineSensorsCalibrationState& operator=(const LineSensorsCalibrationState& state); /**< Assignment of an instance. */ + + /** + * Turn and calibrate the line sensors. + * + * @param[in] calibAlpha Destination calibration angle in [mrad] + * @param[in] isGreaterEqual Configure true if angle shall be greater or equal than the destination. + * + * @return If destination angle reached, it will return true otherwise false. + */ + bool turnAndCalibrate(int32_t calibAlpha, bool isGreaterEqual); + + /** + * Finish the calibration and determine next state. + * + * @param[in] sm State machine + */ + void finishCalibration(StateMachine& sm); +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* LINE_SENSORS_CALIBRATION_STATE_H */ +/** @} */ diff --git a/lib/APPReinforcementLearning/ReadyState.cpp b/lib/APPReinforcementLearning/ReadyState.cpp new file mode 100644 index 00000000..8aa9190f --- /dev/null +++ b/lib/APPReinforcementLearning/ReadyState.cpp @@ -0,0 +1,163 @@ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Ready state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ReadyState.h" +#include +#include +#include +#include "DrivingState.h" +#include "ErrorState.h" +#include +#include +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void ReadyState::entry() +{ + IDisplay& display = Board::getInstance().getDisplay(); + const int32_t SENSOR_VALUE_OUT_PERIOD = 1000; /* ms */ + const int32_t Mode_Selected_period = 1000; /* ms*/ + display.clear(); + display.print("A: TMD"); + display.gotoXY(0, 1); + display.print("B: DMD"); + + if (true == m_isLapTimeAvailable) + { + display.gotoXY(0, 2); + display.print(m_lapTime); + display.print("ms"); + } + + /* The line sensor value shall be output on console cyclic. */ + m_timer.start(SENSOR_VALUE_OUT_PERIOD); + + m_ModeTimeoutTimer.start(Mode_Selected_period); +} + +void ReadyState::process(StateMachine& sm) +{ + IButton& buttonA = Board::getInstance().getButtonA(); + IButton& buttonB = Board::getInstance().getButtonB(); + uint8_t selectedMode = 0; + /* Shall the driving mode be released? */ + if (true == buttonA.isPressed()) + { + buttonA.waitForRelease(); + myMode = DRIVING_MODE; + sm.setState(&DrivingState::getInstance()); + } + /* Shall the Training mode be released? */ + else if ((true == buttonB.isPressed()) ) + { + buttonB.waitForRelease(); + myMode = TRAINING_MODE; + sm.setState(&DrivingState::getInstance()); + } + else if (true == m_ModeTimeoutTimer.isTimeout()) + { + myMode = TRAINING_MODE; + sm.setState(&DrivingState::getInstance()); + + } + else + { + /* Nothing to do. */ + ; + } + +} + +void ReadyState::exit() +{ + m_timer.stop(); + m_isLapTimeAvailable = false; +} + +void ReadyState::setLapTime(uint32_t lapTime) +{ + m_isLapTimeAvailable = true; + m_lapTime = lapTime; +} + +uint8_t ReadyState::selectedMode() +{ + return static_cast(myMode); +} + + + + + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ + /*if (true == m_smpServer.isSynced()) + {*/ \ No newline at end of file diff --git a/lib/APPReinforcementLearning/ReadyState.h b/lib/APPReinforcementLearning/ReadyState.h new file mode 100644 index 00000000..f2920025 --- /dev/null +++ b/lib/APPReinforcementLearning/ReadyState.h @@ -0,0 +1,168 @@ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Ready state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef READY_STATE_H +#define READY_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The system ready state. */ +class ReadyState : public IState +{ +public: + + /** + * Get state instance. + * + * @return State instance. + */ + static ReadyState& getInstance() + { + static ReadyState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + + /** + * Set lap time, which to show on the display. + * + * @param[in] lapTime Lap time in ms + */ + void setLapTime(uint32_t lapTime); + + /** + * Returns the mode selection + * + * @return It returns 1 if DrivingMode is selected or 2 if TrainingMode is selected or 0 if nothing is selected and there is no timeout.. + */ + + uint8_t selectedMode( ); + /* Re-initialize the board. This is required for the webots simulation in + * case the world is reset by a supervisor without restarting the RadonUlzer + * controller executable. + */ + void reinit_Board(); + +protected: +private: + /** + * the mode that can be selected. + */ + /**< Timer used for cyclic debug output. */ + SimpleTimer m_timer; + + /** Timeout timer for the selected mode. */ + SimpleTimer m_ModeTimeoutTimer; + + /** The system state machine. */ + StateMachine m_systemStateMachine; + + /**< Is set (true), if a lap time is available. */ + bool m_isLapTimeAvailable; + + /**< Lap time in ms of the last successful driven round. */ + uint32_t m_lapTime; + + enum Mode_: uint8_t + { + DRIVING_MODE = 0, /**< Driving Mode Selected. */ + TRAINING_MODE /**< Training Mode Selected. */ + }; + + Mode_ myMode; + + + ReadyState() : + m_timer(), + m_isLapTimeAvailable(false), + m_lapTime(0), + myMode(TRAINING_MODE) + { + } + + /** + * Default destructor. + */ + ~ReadyState() + { + } + + /* Not allowed. */ + ReadyState(const ReadyState& state); /**< Copy construction of an instance. */ + ReadyState& operator=(const ReadyState& state); /**< Assignment of an instance. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* READY_STATE_H */ +/** @} */ diff --git a/lib/APPReinforcementLearning/SerialMuxchannels.h b/lib/APPReinforcementLearning/SerialMuxchannels.h index c67822ef..f4068122 100644 --- a/lib/APPReinforcementLearning/SerialMuxchannels.h +++ b/lib/APPReinforcementLearning/SerialMuxchannels.h @@ -80,7 +80,7 @@ #define MODE_CHANNEL_NAME "MODE" /** DLC of Mode Channel */ -#define MODE_CHANNEL_DLC (sizeof(MODE)) +#define MODE_CHANNEL_DLC (sizeof(Mode)) @@ -98,8 +98,8 @@ namespace SMPChannelPayload typedef enum : uint8_t { CMD_ID_IDLE = 0, /**< Nothing to do. */ - CMD_ID_REINIT_BOARD, /**< Re-initialize the board. Required for webots simulation. */ - CMD_ID_GET_MAX_SPEED, /**< Get maximum speed. */ + CMD_ID_SET_INIT_POS, /**< Set initial position. */ + CMD_ID_GET_MAX_SPEED /**< Get maximum speed. */ } CmdId; /**< Command ID */ diff --git a/lib/APPReinforcementLearning/StartupState.cpp b/lib/APPReinforcementLearning/StartupState.cpp index 6bc5bee4..7c1a7377 100644 --- a/lib/APPReinforcementLearning/StartupState.cpp +++ b/lib/APPReinforcementLearning/StartupState.cpp @@ -87,7 +87,6 @@ void StartupState::entry() m_isMaxMotorSpeedCalibAvailable = true; } - showUserInfo(m_userInfoState); } @@ -171,7 +170,6 @@ void StartupState::showUserInfo(UserInfo next) case USER_INFO_UI: display.print("A: MCAL"); - if (true == m_isMaxMotorSpeedCalibAvailable) { display.gotoXY(0, 1); diff --git a/platformio.ini b/platformio.ini index c0526b04..9d83d1c4 100644 --- a/platformio.ini +++ b/platformio.ini @@ -16,13 +16,14 @@ ;default_envs = CalibTarget ;default_envs = CalibSim ;default_envs = ConvoyLeaderTarget -default_envs = ConvoyLeaderSim +;default_envs = ConvoyLeaderSim ;default_envs = LineFollowerTarget ;default_envs = LineFollowerSim ;default_envs = RemoteControlTarget ;default_envs = RemoteControlSim ;default_envs = SensorFusionTarget ;default_envs = SensorFusionSim +;default_envs = ReinforcementLearning ;default_envs = TestSim ; ***************************************************************************** @@ -157,6 +158,7 @@ lib_ignore = APPRemoteControl APPSensorFusion APPTest + APPReinforcementLearning ; ***************************************************************************** ; Calibration application specific HAL for target @@ -203,6 +205,7 @@ lib_ignore = APPLineFollower APPRemoteControl APPSensorFusion + APPReinforcementLearning APPTest ; ***************************************************************************** @@ -250,6 +253,7 @@ lib_ignore = APPConvoyLeader APPRemoteControl APPSensorFusion + APPReinforcementLearning APPTest ; ***************************************************************************** @@ -282,6 +286,54 @@ lib_ignore = extra_scripts = ${hal:Sim.extra_scripts} +; ***************************************************************************** +; Reinforcement Learning for Line follower application +; ***************************************************************************** +[app:ReinforcementLearning] +build_flags = + ${common.build_flags} + -DLOG_DEBUG_ENABLE +lib_deps = + APPReinforcementLearning + Service +lib_ignore = + APPCalib + APPConvoyLeader + APPRemoteControl + APPSensorFusion + APPLineFollower + APPTest + +; ***************************************************************************** +; Line follower application specific HAL for target +; ***************************************************************************** +[hal_app:ReinforcementLearningTarget] +extends = hal:Target +build_flags = + ${hal:Target.build_flags} +lib_deps = + ${hal:Target.lib_deps} + HALLineFollowerTarget +lib_ignore = + ${hal:Target.lib_ignore} +extra_scripts = + ${hal:Target.extra_scripts} + +; ***************************************************************************** +; Line follower application specific HAL for simulation +; ***************************************************************************** +[hal_app:ReinforcementLearningSim] +extends = hal:Sim +build_flags = + ${hal:Sim.build_flags} +lib_deps = + ${hal:Sim.lib_deps} + HALReinforcementLearningSim +lib_ignore = + ${hal:Sim.lib_ignore} +extra_scripts = + ${hal:Sim.extra_scripts} + ; ***************************************************************************** ; Remote control application ; ***************************************************************************** @@ -296,6 +348,7 @@ lib_ignore = APPCalib APPConvoyLeader APPLineFollower + APPReinforcementLearning APPSensorFusion APPTest @@ -343,6 +396,7 @@ lib_ignore = APPCalib APPLineFollower APPConvoyLeader + APPReinforcementLearning APPRemoteControl APPTest @@ -388,7 +442,7 @@ lib_deps = lib_ignore = APPCalib APPConvoyLeader - APPLineFollower + APPReinforcementLearning APPRemoteControl APPSensorFusion @@ -511,6 +565,42 @@ lib_ignore = extra_scripts = ${hal_app:LineFollowerSim.extra_scripts} + +; ***************************************************************************** +; Reinforcement Learning for Line follower application on target +; ***************************************************************************** +[env:ReinforcementLearningTarget] +extends = hal_app:ReinforcementLearningTarget, app:ReinforcementLearning, static_check_configuration +build_flags = + ${hal_app:ReinforcementLearningTarget.build_flags} + ${app:ReinforcementLearning.build_flags} +lib_deps = + ${hal_app:ReinforcementLearningTarget.lib_deps} + ${app:ReinforcementLearning.lib_deps} +lib_ignore = + ${hal_app:ReinforcementLearningTarget.lib_ignore} + ${app:ReinforcementLearning.lib_ignore} +extra_scripts = + ${hal_app:ReinforcementLearningTarget.extra_scripts} + +; ***************************************************************************** +; Reinforcement Learning for Line follower application on simulation +; ***************************************************************************** +[env:ReinforcementLearningSim] +extends = hal_app:ReinforcementLearningSim, app:ReinforcementLearning, static_check_configuration +build_flags = + ${hal_app:ReinforcementLearningSim.build_flags} + ${app:ReinforcementLearning.build_flags} +lib_deps = + ${hal_app:ReinforcementLearningSim.lib_deps} + ${app:ReinforcementLearning.lib_deps} +lib_ignore = + ${hal_app:ReinforcementLearningSim.lib_ignore} + ${app:ReinforcementLearning.lib_ignore} +extra_scripts = + ${hal_app:ReinforcementLearningSim.extra_scripts} + + ; ***************************************************************************** ; Remote control application on target ; ***************************************************************************** diff --git a/webots/controllers/Supervisor/SerialMuxProt.py b/webots/controllers/Supervisor/SerialMuxProt.py new file mode 100644 index 00000000..8c9deae2 --- /dev/null +++ b/webots/controllers/Supervisor/SerialMuxProt.py @@ -0,0 +1,648 @@ +""" Serial Multiplexer Protocol (SerialMuxProt) for lightweight communication. """ + +# MIT License +# +# Copyright (c) 2023 Gabryel Reyes +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. +# + + +################################################################################ +# Imports +################################################################################ + +from dataclasses import dataclass +from struct import Struct +from Serial_webots import SerialWebots + +################################################################################ +# Variables +################################################################################ + +################################################################################ +# Classes +################################################################################ + + +@dataclass(frozen=True) +class SerialMuxProtConstants: + """ Container Data class for SerialMuxProt Constants """ + + CHANNEL_LEN = 1 # Channel Field Length in Bytes + DLC_LEN = 1 # DLC Field Length in Bytes + CHECKSUM_LEN = 1 # Checksum Field Length in Bytes + HEADER_LEN = CHANNEL_LEN + DLC_LEN + \ + CHECKSUM_LEN # Length of Complete Header Field + MAX_DATA_LEN = 32 # Data Field Length in Bytes + MAX_FRAME_LEN = HEADER_LEN + MAX_DATA_LEN # Total Frame Length in Bytes + CHANNEL_NAME_MAX_LEN = 10 # Max length of channel name + # Available Bytes in Control Channel Payload for data. + CONTROL_CHANNEL_PAYLOAD_DATA_LENGTH = 4 + CONTROL_CHANNEL_CMD_BYTE_LENGTH = 1 # Lenght of Command in Bytes + CONTROL_CHANNEL_NUMBER = 0 # Number of Control Channel. + CONTROL_CHANNEL_PAYLOAD_LENGTH = CHANNEL_NAME_MAX_LEN + \ + CONTROL_CHANNEL_PAYLOAD_DATA_LENGTH + \ + CONTROL_CHANNEL_CMD_BYTE_LENGTH + \ + CHANNEL_LEN # DLC of Heartbeat Command. + # Index of the Command Byte of the Control Channel + CONTROL_CHANNEL_COMMAND_INDEX = 0 + # Index of the start of the payload of the Control Channel + CONTROL_CHANNEL_PAYLOAD_INDEX = 1 + HEATBEAT_PERIOD_SYNCED = 5000 # Period of Heartbeat when Synced. + HEATBEAT_PERIOD_UNSYNCED = 1000 # Period of Heartbeat when Unsynced + # Max number of attempts at receiving a Frame before resetting RX Buffer + MAX_RX_ATTEMPTS = MAX_FRAME_LEN + + @dataclass + class Commands(): + """ Enumeration of Commands of Control Channel. """ + SYNC = 0 + SYNC_RSP = 1 + SCRB = 2 + SCRB_RSP = 3 + + +@dataclass +class Channel(): + """ Channel Definition """ + + def __init__(self, channel_name: str = "", channel_dlc: int = 0, channel_callback=None) -> None: + self.name = channel_name + self.dlc = channel_dlc + self.callback = channel_callback + + +class Frame(): + """ Frame Defintion """ + + def __init__(self) -> None: + self.raw = bytearray(SerialMuxProtConstants.MAX_FRAME_LEN) + self.channel = 0 + self.dlc = 0 + self.checksum = 0 + self.payload = bytearray(SerialMuxProtConstants.MAX_DATA_LEN) + + def unpack_header(self): + """ Unpack/parse channel number and checksum from raw frame """ + header_packer = Struct(">3B") + self.channel, self.dlc, self.checksum = header_packer.unpack_from( + self.raw) + + def unpack_payload(self): + """ Unpack/parse payload from raw frame """ + self.payload = self.raw[3:] + + def pack_frame(self): + """ Pack header and payload into raw array""" + self.raw[0] = self.channel + self.raw[1] = self.dlc + self.raw[2] = self.checksum + self.raw[3:] = self.payload + + +@dataclass +class ChannelArrays: + """ Container Class for Channel Arrays and their counters """ + + def __init__(self, max_configured_channels: int) -> None: + self.number_of_rx_channels = 0 + self.number_of_tx_channels = 0 + self.number_of_pending_channels = 0 + self.rx_channels = [Channel() for x in range(max_configured_channels)] + self.tx_channels = [Channel() for x in range(max_configured_channels)] + self.pending_channels = [Channel() + for x in range(max_configured_channels)] + + +@dataclass +class SyncData: + """ Container Dataclass for Synchronization Data. """ + + def __init__(self) -> None: + self.is_synced = False + self.last_sync_response = 0 + self.last_sync_command = 0 + + +@dataclass +class RxData: + """ Container Dataclass for Receive Data and counters. """ + + def __init__(self) -> None: + self.received_bytes = 0 + self.rx_attempts = 0 + self.receive_frame = Frame() + + +class SerialMuxProt: + """ SerialMuxProt Server """ + + def __init__(self, max_configured_channels: int, stream: SerialWebots) -> None: + self.__max_configured_channels = max_configured_channels + self.__stream = stream + self.__rx_data = RxData() + self.__sync_data = SyncData() + self.__channels = ChannelArrays(max_configured_channels) + + def process(self, current_timestamp: int) -> None: + """Manage the Server functions. + Call this function cyclic. + + Parameters + ---------- + current_timestamp : int + Time in milliseconds. + + """ + + # Periodic Heartbeat. + self.__heartbeat(current_timestamp) + + # Process RX data. + self.__process_rx() + + def send_data(self, channel_name: str, payload: bytearray) -> bool: + """Send a frame with the selected bytes. + + Parameters: + ---------- + channel_name : str + Channel to send frame to. + payload : bytearray + Byte buffer to be sent. + + Returns: + -------- + If payload succesfully sent, returns true. Otherwise, false. + """ + is_sent = False + channel_number = self.get_tx_channel_number(channel_name) + + if (SerialMuxProtConstants.CONTROL_CHANNEL_NUMBER != channel_number) and\ + (self.__sync_data.is_synced is True): + is_sent = self.__send(channel_number, payload) + + return is_sent + + def is_synced(self) -> bool: + """ Returns current Sync state of the SerialMuxProt Server. """ + return self.__sync_data.is_synced + + def get_number_rx_channels(self) -> int: + """Get the number of configured RX channels.""" + return self.__channels.number_of_rx_channels + + def get_number_tx_channels(self) -> int: + """Get the number of configured TX channels.""" + return self.__channels.number_of_tx_channels + + def create_channel(self, name: str, dlc: int) -> int: + """Creates a new TX Channel on the server. + + Parameters: + ---------- + name : str + Name of the channel. It will not be checked if the name already exists. + dlc : int + Length of the payload of this channel. + + Returns: + -------- + The channel number if succesfully created, or 0 if not able to create new channel. + """ + name_length = len(name) + idx = 0 + + if (0 != name_length) and\ + (SerialMuxProtConstants.CHANNEL_NAME_MAX_LEN >= name_length) and\ + (0 != dlc) and\ + (SerialMuxProtConstants.MAX_DATA_LEN >= dlc) and\ + (self.__max_configured_channels > self.__channels.number_of_tx_channels): + + # Create Channel + self.__channels.tx_channels[self.__channels.number_of_tx_channels] = Channel( + name, dlc) + + # Increase Channel Counter + self.__channels.number_of_tx_channels += 1 + + # Provide Channel Number. Could be summarized with operation above. + idx = self.__channels.number_of_tx_channels + + return idx + + def subscribe_to_channel(self, name: str, callback) -> None: + """ Suscribe to a Channel to receive the incoming data. + + Parameters: + ---------- + name : str + Name of the Channel to suscribe to. + callback : function + Callback to return the incoming data. + """ + + if (SerialMuxProtConstants.CHANNEL_NAME_MAX_LEN >= len(name)) and\ + (callback is not None) and\ + (self.__max_configured_channels > self.__channels.number_of_pending_channels): + + # Save Name and Callback for channel creation after response + self.__channels.pending_channels[self.__channels.number_of_pending_channels] = Channel( + channel_name=name, channel_dlc=0, channel_callback=callback) + # Increase Channel Counter + self.__channels.number_of_pending_channels += 1 + + def get_tx_channel_number(self, channel_name: str) -> int: + """Get Number of a TX channel by its name. + + Parameters: + ----------- + channel_name : str + Name of channel + + Returns: + -------- + Number of the Channel, or 0 if not channel with the name is present. + """ + + channel_number = 0 + for idx in range(self.__max_configured_channels): + if self.__channels.tx_channels[idx].name == channel_name: + channel_number = idx + 1 + break + + return channel_number + + def __heartbeat(self, current_timestamp: int) -> None: + """ Periodic heartbeat. + Sends SYNC Command depending on the current Sync state. + + Parameters + ---------- + current_timestamp : int + Time in milliseconds. + + """ + heartbeat_period = SerialMuxProtConstants.HEATBEAT_PERIOD_UNSYNCED + + if self.__sync_data.is_synced is True: + heartbeat_period = SerialMuxProtConstants.HEATBEAT_PERIOD_SYNCED + if (current_timestamp - self.__sync_data.last_sync_command) >= heartbeat_period: + + # Timeout + if self.__sync_data.last_sync_command != self.__sync_data.last_sync_response: + self.__sync_data.is_synced = False + + # Pack big-endian uint32 + packer = Struct(">L") + timestamp_array = packer.pack(current_timestamp) + + # Send SYNC Command + command = bytearray() + command.append(SerialMuxProtConstants.Commands.SYNC) + command.extend(timestamp_array) + + # Pad array if necessary + command = command.ljust( + SerialMuxProtConstants.CONTROL_CHANNEL_PAYLOAD_LENGTH, b'\x00') + if self.__send(SerialMuxProtConstants.CONTROL_CHANNEL_NUMBER, command) is True: + self.__sync_data.last_sync_command = current_timestamp + + def __process_rx(self) -> None: + """ Receive and process RX Data. """ + + expected_bytes = 0 + dlc = 0 + # Determine how many bytes to read. + if SerialMuxProtConstants.HEADER_LEN > self.__rx_data.received_bytes: + # Header must be read. + expected_bytes = SerialMuxProtConstants.HEADER_LEN - self.__rx_data.received_bytes + else: + # Header has been read. Get DLC of Rx Channel using header. + self.__rx_data.receive_frame.unpack_header() + # dlc = self.__get_channel_dlc(self.__receive_frame.channel, False) + dlc = self.__rx_data.receive_frame.dlc + # DLC = 0 means that the channel does not exist. + if (0 != dlc) and (SerialMuxProtConstants.MAX_RX_ATTEMPTS >= self.__rx_data.rx_attempts): + remaining_payload_bytes = self.__rx_data.received_bytes - \ + SerialMuxProtConstants.HEADER_LEN + expected_bytes = dlc - remaining_payload_bytes + + self.__rx_data.rx_attempts += 1 + # Are we expecting to read anything? + if 0 != expected_bytes: + # Read the required amount of bytes, if available. + if self.__stream.available() >= expected_bytes: + rcvd, data = self.__stream.read_bytes(expected_bytes) + self.__rx_data.receive_frame.raw[self.__rx_data.received_bytes:] = data + self.__rx_data.received_bytes += rcvd + + # Frame has been received. + if (0 != dlc) and ((SerialMuxProtConstants.HEADER_LEN + dlc) == self.__rx_data.received_bytes): + # Check validity + if self.__is_frame_valid(self.__rx_data.receive_frame) is True: + channel_array_index = self.__rx_data.receive_frame.channel - 1 + self.__rx_data.receive_frame.unpack_payload() + + + # Differenciate between Control and Data Channels. + if SerialMuxProtConstants.CONTROL_CHANNEL_NUMBER == self.__rx_data.receive_frame.channel: + self.__callback_control_channel( + self.__rx_data.receive_frame.payload) + elif self.__channels.rx_channels[channel_array_index].callback is not None: + self.__channels.rx_channels[channel_array_index].callback( + self.__rx_data.receive_frame.payload) + + # Frame received. Cleaning! + self.__clear_local_buffers() + else: + # Invalid Header. Delete Frame! + self.__clear_local_buffers() + + def __process_subscriptions(self) -> None: + """ Subscribe to any pending Channels if synced to server. """ + + # If synced and a channel is pending + if (self.__sync_data.is_synced is True) and\ + (0 < self.__channels.number_of_pending_channels): + # Channel Iterator + for pending_channel in self.__channels.pending_channels: + + # Channel is pending + if pending_channel.callback is not None: + + # Subscribe to Channel + request = bytearray( + SerialMuxProtConstants.CONTROL_CHANNEL_PAYLOAD_LENGTH) + request[0] = SerialMuxProtConstants.Commands.SCRB + request[6:] = bytearray(pending_channel.name, 'ascii') + + # Pad array if necessary + request = request.ljust( + SerialMuxProtConstants.CONTROL_CHANNEL_PAYLOAD_LENGTH, b'\x00') + + if self.__send(SerialMuxProtConstants.CONTROL_CHANNEL_NUMBER, request) is False: + # Fall out of sync if failed to send. + self.__sync_data.is_synced = False + break + + def __clear_local_buffers(self) -> None: + """ Clear Local RX Buffers """ + self.__rx_data.receive_frame = Frame() + self.__rx_data.received_bytes = 0 + self.__rx_data.rx_attempts = 0 + + def __get_tx_channel_dlc(self, channel_number: int) -> int: + """ Get the Payload Length of a channel. + + Parameters + ---------- + channel_number : int + Channel number to check. + + is_tx_channel : bool + Is the Channel a TX Channel? If false, will return value for an RX Channel instead. + + Returns + ------- + DLC of the channel, or 0 if channel is not found. + """ + dlc = 0 + + if SerialMuxProtConstants.CONTROL_CHANNEL_NUMBER == channel_number: + dlc = SerialMuxProtConstants.CONTROL_CHANNEL_PAYLOAD_LENGTH + else: + if self.__max_configured_channels >= channel_number: + channel_idx = channel_number - 1 + dlc = self.__channels.tx_channels[channel_idx].dlc + + return dlc + + def __is_frame_valid(self, frame: Frame) -> bool: + """ Check if a Frame is valid using its checksum. + + Parameters + ---------- + frame : Frame + Frame to be checked + + Returns: + True if the Frame's checksum is correct. Otherwise, False. + """ + + return self.__checksum(frame.raw) == frame.checksum + + def __checksum(self, raw_frame: bytearray) -> int: + """ + Performs simple checksum of the Frame to confirm its validity. + + Parameters: + ---------- + raw_frame : Frame + Frame to calculate checksum for + + Returns: + ------- + Checksum value + """ + newsum = raw_frame[0] + raw_frame[1] + + for idx in range(3, len(raw_frame)): + newsum += raw_frame[idx] + + newsum = newsum % 255 + + return newsum + + def __send(self, channel_number: int, payload: bytearray) -> bool: + """ Send a frame with the selected bytes. + + Parameters: + ---------- + channel_number : int + Channel to send frame to. + payload : bytearray + Payload to send + + Returns: + -------- + If payload succesfully sent, returns True. Otherwise, False. + """ + + frame_sent = False + channel_dlc = self.__get_tx_channel_dlc(channel_number) + + if (len(payload) == channel_dlc) and \ + ((self.__sync_data.is_synced is True) or + (SerialMuxProtConstants.CONTROL_CHANNEL_NUMBER == channel_number)): + written_bytes = 0 + new_frame = Frame() + new_frame.channel = channel_number + new_frame.dlc = channel_dlc + new_frame.payload = payload + new_frame.pack_frame() # Pack Header and payload in Frame + new_frame.checksum = self.__checksum(new_frame.raw) + new_frame.pack_frame() # Pack Checksum in Frame + + try: + written_bytes = self.__stream.write(new_frame.raw) + except Exception as excpt: # pylint: disable=broad-exception-caught + print(excpt) + + if written_bytes == (SerialMuxProtConstants.HEADER_LEN + channel_dlc): + frame_sent = True + + return frame_sent + + def __cmd_sync(self, payload: bytearray) -> None: + """ Control Channel Command: SYNC + + Parameters: + ----------- + payload : bytearray + Command Data of received frame + """ + response = bytearray( + SerialMuxProtConstants.CONTROL_CHANNEL_PAYLOAD_LENGTH) + response[SerialMuxProtConstants.CONTROL_CHANNEL_COMMAND_INDEX] = SerialMuxProtConstants.Commands.SYNC_RSP + response[SerialMuxProtConstants.CONTROL_CHANNEL_PAYLOAD_INDEX:] = payload + + self.__send(SerialMuxProtConstants.CONTROL_CHANNEL_NUMBER, response) + + def __cmd_sync_rsp(self, payload: bytearray) -> None: + """ Control Channel Command: SYNC_RSP + + Parameters: + ----------- + payload : bytearray + Command Data of received frame + """ + + # Parse big-endian uint32 + unpacker = Struct(">L") + rcvd_timestamp = unpacker.unpack_from(payload)[0] + + if self.__sync_data.last_sync_command == rcvd_timestamp: + self.__sync_data.last_sync_response = self.__sync_data.last_sync_command + self.__sync_data.is_synced = True + # Process pending Subscriptions + self.__process_subscriptions() + + else: + self.__sync_data.is_synced = False + + def __cmd_scrb(self, payload: bytearray) -> None: + """ Control Channel Command: SCRB + + Parameters: + ----------- + payload : bytearray + Command Data of received frame + """ + + response = bytearray( + SerialMuxProtConstants.CONTROL_CHANNEL_PAYLOAD_LENGTH) + response[SerialMuxProtConstants.CONTROL_CHANNEL_COMMAND_INDEX] = SerialMuxProtConstants.Commands.SCRB_RSP + + # Parse name + channel_name = str(payload[5:], "ascii").strip('\x00') + response[5] = self.get_tx_channel_number(channel_name) + # Name is always sent back. + response[6:] = bytearray(channel_name, 'ascii') + + # Pad array if necessary + response = response.ljust( + SerialMuxProtConstants.CONTROL_CHANNEL_PAYLOAD_LENGTH, b'\x00') + if self.__send(SerialMuxProtConstants.CONTROL_CHANNEL_NUMBER, response) is False: + # Fall out of sync if failed to send. + self.__sync_data.is_synced = False + + def __cmd_scrb_rsp(self, payload: bytearray) -> None: + """ Control Channel Command: SCRB + + Parameters: + ----------- + payload : bytearray + Command Data of received frame + """ + + # Parse payload + channel_number = payload[4] + channel_name = str(payload[5:], "ascii").strip('\x00') + + if (self.__max_configured_channels >= channel_number) and \ + (0 < self.__channels.number_of_pending_channels): + + # Channel Iterator + for potential_channel in self.__channels.pending_channels: + # Check if a SCRB is pending and is the correct channel + if (potential_channel.callback is not None) and\ + (potential_channel.name == channel_name): + # Channel is found in the server + if 0 != channel_number: + channel_array_index = channel_number - 1 + + if self.__channels.rx_channels[channel_array_index].callback is None: + self.__channels.number_of_rx_channels += 1 + + self.__channels.rx_channels[channel_array_index] = Channel( + channel_name, 0, potential_channel.callback) + + # Channel is no longer pending + potential_channel = Channel() + + # Decrease Channel Counter + self.__channels.number_of_pending_channels -= 1 + + # Break out of iterator + break + + def __callback_control_channel(self, payload: bytearray) -> None: + """ Callback for the Control Channel + + Parameters: + ----------- + payload : bytearray + Payload of received frame + """ + if len(payload) != SerialMuxProtConstants.CONTROL_CHANNEL_PAYLOAD_LENGTH: + return + + cmd_byte = payload[SerialMuxProtConstants.CONTROL_CHANNEL_COMMAND_INDEX] + cmd_data = payload[SerialMuxProtConstants.CONTROL_CHANNEL_PAYLOAD_INDEX:] + + # Switch not available until Python 3.10 + # Find command handler + if SerialMuxProtConstants.Commands.SYNC == cmd_byte: + self.__cmd_sync(cmd_data) + elif SerialMuxProtConstants.Commands.SYNC_RSP == cmd_byte: + self.__cmd_sync_rsp(cmd_data) + elif SerialMuxProtConstants.Commands.SCRB == cmd_byte: + self.__cmd_scrb(cmd_data) + elif SerialMuxProtConstants.Commands.SCRB_RSP == cmd_byte: + self.__cmd_scrb_rsp(cmd_data) + +################################################################################ +# Functions +################################################################################ + +################################################################################ +# Main +################################################################################ From f0f254d80461aad66b955b5b1bd6729e7bb60f02 Mon Sep 17 00:00:00 2001 From: Akram Date: Tue, 16 Jul 2024 10:43:26 +0200 Subject: [PATCH 09/58] HALSIM added to ReinforcementLearning APP --- lib/HALReinforcementLearningSim/Board.cpp | 134 ++++++++ lib/HALReinforcementLearningSim/Board.h | 374 ++++++++++++++++++++++ 2 files changed, 508 insertions(+) create mode 100644 lib/HALReinforcementLearningSim/Board.cpp create mode 100644 lib/HALReinforcementLearningSim/Board.h diff --git a/lib/HALReinforcementLearningSim/Board.cpp b/lib/HALReinforcementLearningSim/Board.cpp new file mode 100644 index 00000000..d2516f1c --- /dev/null +++ b/lib/HALReinforcementLearningSim/Board.cpp @@ -0,0 +1,134 @@ +/* MIT License + * + * Copyright (c) 2019 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief The simulation robot board realization. + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void Board::init() +{ + m_encoders.init(); + m_lineSensors.init(); + m_motors.init(); + m_settings.init(); +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +Board::Board() : + IBoard(), + m_robot(), + m_WebotsSerialDrv(m_robot.getEmitter(RobotDeviceNames::EMITTER_NAME_SERIAL), + m_robot.getReceiver(RobotDeviceNames::RECEIVER_NAME_SERIAL)), + m_simTime(m_robot), + m_keyboard(m_simTime, m_robot.getKeyboard()), + m_buttonA(m_keyboard), + m_buttonB(m_keyboard), + m_buttonC(m_keyboard), + m_buzzer(m_robot.getSpeaker(RobotDeviceNames::SPEAKER_NAME)), + m_display(m_robot.getDisplay(RobotDeviceNames::DISPLAY_NAME)), + m_encoders(m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME), + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)), + m_lineSensors( + m_robot.getEmitter(RobotDeviceNames::EMITTER_0_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_1_NAME), + m_robot.getEmitter(RobotDeviceNames::EMITTER_2_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_3_NAME), + m_robot.getEmitter(RobotDeviceNames::EMITTER_4_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)), + m_motors(m_robot.getMotor(RobotDeviceNames::LEFT_MOTOR_NAME), m_robot.getMotor(RobotDeviceNames::RIGHT_MOTOR_NAME)), + m_ledRed(m_robot.getLED(RobotDeviceNames::LED_RED_NAME)), + m_ledYellow(m_robot.getLED(RobotDeviceNames::LED_YELLOW_NAME)), + m_ledGreen(m_robot.getLED(RobotDeviceNames::LED_GREEN_NAME)), + m_settings() +#ifdef DEBUG_ODOMETRY + , + m_sender(m_robot.getEmitter(SENDER_NAME)) +#endif /* DEBUG_ODOMETRY */ +{ +} + +void Board::enableSimulationDevices() +{ + const int timeStep = m_simTime.getTimeStep(); + + m_robot.getKeyboard()->enable(timeStep); + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME)->enable(timeStep); + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)->enable(timeStep); + m_robot.getReceiver(RobotDeviceNames::RECEIVER_NAME_SERIAL)->enable(timeStep); +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ \ No newline at end of file diff --git a/lib/HALReinforcementLearningSim/Board.h b/lib/HALReinforcementLearningSim/Board.h new file mode 100644 index 00000000..993f8f89 --- /dev/null +++ b/lib/HALReinforcementLearningSim/Board.h @@ -0,0 +1,374 @@ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief The simulation robot board realization. + * @author Andreas Merkle + * + * @addtogroup HALSim + * + * @{ + */ +#ifndef BOARD_H +#define BOARD_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#ifdef DEBUG_ODOMETRY +#include +#endif /* DEBUG_ODOMETRY */ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** + * The concrete simulation robot board. + */ +class Board : public IBoard +{ +public: + /** + * Get board instance. + * + * @return Board instance + */ + static Board& getInstance() + { + static Board instance; /* idiom */ + + return instance; + } + + /** + * Initialize the hardware. + */ + void init() final; + + /** + * Get button A driver. + * + * @return Button A driver. + */ + IButton& getButtonA() final + { + return m_buttonA; + } + + /** + * Get button B driver. + * + * @return Button B driver. + */ + IButton& getButtonB() final + { + return m_buttonB; + } + + /** + * Get button C driver. + * + * @return Button C driver. + */ + IButton& getButtonC() final + { + return m_buttonC; + } + + /** + * Get buzzer driver. + * + * @return Buzzer driver. + */ + IBuzzer& getBuzzer() final + { + return m_buzzer; + } + + /** + * Get LCD driver. + * + * @return LCD driver. + */ + IDisplay& getDisplay() final + { + return m_display; + } + + /** + * Get encoders. + * + * @return Encoders driver. + */ + IEncoders& getEncoders() final + { + return m_encoders; + } + + /** + * Get line sensors driver. + * + * @return Line sensor driver. + */ + ILineSensors& getLineSensors() final + { + return m_lineSensors; + } + + /** + * Get motor driver. + * + * @return Motor driver. + */ + IMotors& getMotors() final + { + return m_motors; + } + + /** + * Get red LED driver. + * + * @return Red LED driver. + */ + ILed& getRedLed() final + { + return m_ledRed; + } + + /** + * Get yellow LED driver. + * + * @return Yellow LED driver. + */ + ILed& getYellowLed() final + { + return m_ledYellow; + } + + /** + * Get green LED driver. + * + * @return Green LED driver. + */ + ILed& getGreenLed() final + { + return m_ledGreen; + } + + /** + * Get the settings. + * + * @return Settings + */ + ISettings& getSettings() final + { + return m_settings; + } + + +#ifdef DEBUG_ODOMETRY + + /** + * Get the sender driver, used to send data to the webots supervisor. + * + * @return Sender driver + */ + ISender& getSender() final + { + return m_sender; + } + +#endif /* DEBUG_ODOMETRY */ + + /** + * Process actuators and sensors. + */ + void process() final + { + m_buzzer.process(); + } + WebotsSerialDrv* getSimSerial() + { + + return &m_WebotsSerialDrv; + + } + + +private: + + /** Simulated roboter instance. */ + webots::Robot m_robot; + + /** Simulation time handler */ + SimTime m_simTime; + + /** Own keyboard that wraps the webots keyboard. */ + Keyboard m_keyboard; + + /** Button A driver */ + ButtonA m_buttonA; + + /** Button B driver */ + ButtonB m_buttonB; + + /** Button C driver */ + ButtonC m_buttonC; + + /** Buzzer driver */ + Buzzer m_buzzer; + + /** Display driver */ + Display m_display; + + /** Encoders driver */ + Encoders m_encoders; + + /** Line sensors driver */ + LineSensors m_lineSensors; + + /** Motors driver */ + Motors m_motors; + + /** Red LED driver */ + LedRed m_ledRed; + + /** Red LED driver */ + LedYellow m_ledYellow; + + /** Red LED driver */ + LedGreen m_ledGreen; + + /** Settings */ + Settings m_settings; + + WebotsSerialDrv m_WebotsSerialDrv; + + + +#ifdef DEBUG_ODOMETRY + + /** Sender driver for supervisor communication. */ + Sender m_sender; + +#endif /* DEBUG_ODOMETRY */ + + /** + * Constructs the concrete board. + */ + Board(); + + /** + * Destroys the concrete board. + */ + ~Board() + { + } + /** + * Get the simulation time handler. + * + * @return Simulation time handler + */ + SimTime& getSimTime() + { + return m_simTime; + } + + /** + * Get the keyboard instance of the simulation. + * + * @return The keyboard. + */ + Keyboard& getKeyboard() + { + return m_keyboard; + } + + /** + * Get the simulation serial driver, which is connected within Webots. + * + * @return If serial driver is available, it will return a pointer to it, otherwise nullptr. + */ + + + + /** + * Enable all simulation devices. + * It is called by the main entry only. + * Devices must be enabled before they can be used, and a simulation step must be performed before the application + * initialization. + */ + void enableSimulationDevices(); + + /** + * The main entry needs access to the simulation robot instance. + * But all other application parts shall have no access, which is + * solved by this friend. + * + * @param[in] argc Number of arguments + * @param[in] argv Arguments + * + * @return Exit code + */ + friend int main(int argc, char** argv); +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* BOARD_H */ +/** @} */ From 602d8116d584708bd0f76523955b01c6cbf62078 Mon Sep 17 00:00:00 2001 From: Akram Date: Tue, 16 Jul 2024 11:17:23 +0200 Subject: [PATCH 10/58] Update Supervisor to receive Line Sensors Values and Selected Mode --- webots/controllers/Supervisor/Supervisor.py | 59 +++++++++++++++------ 1 file changed, 44 insertions(+), 15 deletions(-) diff --git a/webots/controllers/Supervisor/Supervisor.py b/webots/controllers/Supervisor/Supervisor.py index 2a7c72c7..de543cdd 100644 --- a/webots/controllers/Supervisor/Supervisor.py +++ b/webots/controllers/Supervisor/Supervisor.py @@ -5,6 +5,10 @@ from Serial_webots import SerialWebots from SerialMuxProt import SerialMuxProt from controller import Supervisor +from SerialMuxChannels import * + + + # The PROTO DEF name must be given! @@ -40,15 +44,40 @@ # SerialMuxProt Server Instance. S_client = SerialWebots(supervisor_com_Tx, supervisor_com_rx) -smp_server = SerialMuxProt(10, S_client) +smp_server = SerialMuxProt(10, S_client) # The PROTO DEF name must be given! ROBOT_NAME = "ROBOT" -def callback_Status(payload: bytearray) -> None: +m_serialMuxProtChannelIdSPEED_SET = smp_server.create_channel("SPEED_SET", 4) + +def callback_Status(payload: bytearray) -> int: """ Callback Status Channel """ - print(payload) + if payload[0] == 1: + print("End-Start-Line Detected") + smp_server.send_data("SPEED_SET", struct.pack('2H', 0, 0)) # Stop motors + + +def callback_LineSensors(payload: bytearray)-> None: + """ Callback LINE_SENS Channel""" + sensor_data = struct.unpack('5H', payload) + for idx in range (5): + print(f"Sensor[{idx}] = {sensor_data[idx]}") + smp_server.send_data("SPEED_SET", struct.pack('2H', 1000, 1000)) # Stop motors + +def callback_Mode(payload: bytearray)-> None: + """ Callback MODE Channel""" + print("Akram") + train_mode = payload[0] + if train_mode: + print("Train Mode Selected") + else: + print("Driving Mode Selected") +smp_server.subscribe_to_channel("STATUS", callback_Status) +smp_server.subscribe_to_channel("LINE_SENS",callback_LineSensors) +smp_server.subscribe_to_channel("MODE",callback_Mode) + def main_loop(): """Main loop: - Perform simulation steps until Webots is stopping the controller- @@ -58,19 +87,19 @@ def main_loop(): """ status = 0 m_elapsedTimeSinceReset = 0 - # Subscribe To Status Channel - smp_server.subscribe_to_channel("STATUS", callback_Status) - - # Get robot node which to observe. - robot_node = supervisor.getFromDef(ROBOT_NAME) - if robot_node is None: - print(f"Robot DEF {ROBOT_NAME} not found.") - status = -1 + + if m_serialMuxProtChannelIdSPEED_SET == 0: + print(f"Channel SPEED_SET not created.") else: - while supervisor.step(timestep) != -1: - m_elapsedTimeSinceReset += timestep - smp_server.process(m_elapsedTimeSinceReset) - + # Get robot node which to observe. + robot_node = supervisor.getFromDef(ROBOT_NAME) + if robot_node is None: + print(f"Robot DEF {ROBOT_NAME} not found.") + status = -1 + else: + while supervisor.step(timestep) != -1: + m_elapsedTimeSinceReset += timestep + smp_server.process(m_elapsedTimeSinceReset) return status sys.exit(main_loop()) From fc8a04845887dc1bfd31ad3d28e5396e6d5dbb9d Mon Sep 17 00:00:00 2001 From: Akram Date: Tue, 16 Jul 2024 12:13:02 +0200 Subject: [PATCH 11/58] Update import to struct and clean up print comment --- webots/controllers/Supervisor/Supervisor.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/webots/controllers/Supervisor/Supervisor.py b/webots/controllers/Supervisor/Supervisor.py index de543cdd..64c34974 100644 --- a/webots/controllers/Supervisor/Supervisor.py +++ b/webots/controllers/Supervisor/Supervisor.py @@ -5,7 +5,7 @@ from Serial_webots import SerialWebots from SerialMuxProt import SerialMuxProt from controller import Supervisor -from SerialMuxChannels import * +import struct @@ -67,7 +67,6 @@ def callback_LineSensors(payload: bytearray)-> None: def callback_Mode(payload: bytearray)-> None: """ Callback MODE Channel""" - print("Akram") train_mode = payload[0] if train_mode: print("Train Mode Selected") From 432e90ce77027d203ebeab78106d9ba11775f24d Mon Sep 17 00:00:00 2001 From: Akram Date: Tue, 16 Jul 2024 12:15:49 +0200 Subject: [PATCH 12/58] Update import to Struct and Clean up print Comment --- webots/controllers/Supervisor/Supervisor.py | 4 ---- 1 file changed, 4 deletions(-) diff --git a/webots/controllers/Supervisor/Supervisor.py b/webots/controllers/Supervisor/Supervisor.py index 64c34974..26143179 100644 --- a/webots/controllers/Supervisor/Supervisor.py +++ b/webots/controllers/Supervisor/Supervisor.py @@ -7,10 +7,6 @@ from controller import Supervisor import struct - - - - # The PROTO DEF name must be given! ROBOT_NAME = "ROBOT" From 9a724bdf4ff5c5775322a47145e536222ab952ed Mon Sep 17 00:00:00 2001 From: Akram Date: Wed, 17 Jul 2024 15:59:46 +0200 Subject: [PATCH 13/58] Deleted the isNoLineDetected function --- lib/APPReinforcementLearning/DrivingState.cpp | 45 ++++--------------- lib/APPReinforcementLearning/DrivingState.h | 41 +++-------------- 2 files changed, 15 insertions(+), 71 deletions(-) diff --git a/lib/APPReinforcementLearning/DrivingState.cpp b/lib/APPReinforcementLearning/DrivingState.cpp index 2bdefdef..908732a1 100644 --- a/lib/APPReinforcementLearning/DrivingState.cpp +++ b/lib/APPReinforcementLearning/DrivingState.cpp @@ -58,10 +58,6 @@ * Local Variables *****************************************************************************/ -/* Initialize the required sensor IDs to be generic. */ -const uint8_t DrivingState::SENSOR_ID_MOST_LEFT = 0U; -const uint8_t DrivingState::SENSOR_ID_MIDDLE = Board::getInstance().getLineSensors().getNumLineSensors() / 2U; -const uint8_t DrivingState::SENSOR_ID_MOST_RIGHT = Board::getInstance().getLineSensors().getNumLineSensors() - 1U; /****************************************************************************** * Public Methods *****************************************************************************/ @@ -69,9 +65,9 @@ const uint8_t DrivingState::SENSOR_ID_MOST_RIGHT = Board::getInstance().getLineS void DrivingState::entry() { DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - diffDrive.setLinearSpeed(0, 0); diffDrive.enable(); + m_observationTimer.start(OBSERVATION_DURATION); } void DrivingState::process(StateMachine& sm) @@ -82,10 +78,10 @@ void DrivingState::process(StateMachine& sm) void DrivingState::exit() { - /* Stop motors. */ DifferentialDrive::getInstance().setLinearSpeed(0, 0); DifferentialDrive::getInstance().disable(); + m_observationTimer.stop(); } void DrivingState::setTargetSpeeds(int16_t leftMotor, int16_t rightMotor) @@ -93,41 +89,13 @@ void DrivingState::setTargetSpeeds(int16_t leftMotor, int16_t rightMotor) DifferentialDrive::getInstance().setLinearSpeed(leftMotor, rightMotor); } - -bool DrivingState::isNoLineDetected(const uint16_t* lineSensorValues, uint8_t length) const -{ - bool isDetected = true; - uint8_t idx = SENSOR_ID_MOST_RIGHT; - - /* - * - * + + + + + - * L M R - */ - for (idx = SENSOR_ID_MOST_LEFT; idx <= SENSOR_ID_MOST_RIGHT; ++idx) - { - if (LINE_SENSOR_ON_TRACK_MIN_VALUE <= lineSensorValues[idx]) - { - isDetected = false; - break; - } - } - - return isDetected; -} - -bool DrivingState::isAbortRequired(bool m_isTrackLost) +bool DrivingState::isAbortRequired() { bool isAbort = false; - /* If track is lost over a certain distance, abort driving. */ - if (true == m_isTrackLost) + if (true == m_observationTimer.isTimeout()) { - /* Max. distance driven, but track still not found? */ - if (MAX_DISTANCE < Odometry::getInstance().getMileageCenter()) - { isAbort = true; - } } return isAbort; } @@ -142,7 +110,10 @@ bool DrivingState::isAbortRequired(bool m_isTrackLost) /****************************************************************************** * Private Methods *****************************************************************************/ - +DrivingState::DrivingState() : + m_observationTimer() +{ +} /****************************************************************************** * External Functions *****************************************************************************/ diff --git a/lib/APPReinforcementLearning/DrivingState.h b/lib/APPReinforcementLearning/DrivingState.h index d8599bae..3d39e696 100644 --- a/lib/APPReinforcementLearning/DrivingState.h +++ b/lib/APPReinforcementLearning/DrivingState.h @@ -98,55 +98,28 @@ class DrivingState : public IState */ void setTargetSpeeds(int16_t leftMotor, int16_t rightMotor); - /** - * Is no line detected? - * - * @param[in] lineSensorValues The line sensor values as array. - * @param[in] length The number of line sensor values. - * - * @return If no line is detected, it will return true otherwise false. - */ - - bool isNoLineDetected(const uint16_t* lineSensorValues, uint8_t length) const; /** * Check the abort conditions while driving the challenge. * * @return If abort is required, it will return true otherwise false. */ - bool isAbortRequired(bool m_isTrackLost); + bool isAbortRequired(); protected: private: - /** - * The line sensor threshold (normalized) used to detect the track. - * The track is detected in case the received value is greater or equal than - * the threshold. - */ - static const uint16_t LINE_SENSOR_ON_TRACK_MIN_VALUE = 200U; - /** Max. distance in mm after a lost track must be found again. */ - static const uint32_t MAX_DISTANCE = 200; - /** - * ID of most left sensor. - */ - static const uint8_t SENSOR_ID_MOST_LEFT; - /** - * ID of most right sensor. - */ - static const uint8_t SENSOR_ID_MIDDLE; + /** Observation duration in ms. This is the max. time within the robot must be finished its drive. */ + static const uint32_t OBSERVATION_DURATION = 3000000; - /** - * ID of middle sensor. - */ - static const uint8_t SENSOR_ID_MOST_RIGHT; + /**< Observation timer to observe the max. time per challenge. */ + + SimpleTimer m_observationTimer; /** * Default constructor. */ - DrivingState() : IState() - { - } + DrivingState(); /** * Default destructor. From f8a7eabf1dcd14cf3d8a9c7273ddcb702f6c41c5 Mon Sep 17 00:00:00 2001 From: Akram Date: Wed, 17 Jul 2024 16:02:30 +0200 Subject: [PATCH 14/58] Updated the Ready state: Robot now drives until the Start-Stop line is crossed. --- lib/APPReinforcementLearning/ReadyState.cpp | 94 ++++++++++++--------- lib/APPReinforcementLearning/ReadyState.h | 88 ++++++++++++++----- 2 files changed, 121 insertions(+), 61 deletions(-) diff --git a/lib/APPReinforcementLearning/ReadyState.cpp b/lib/APPReinforcementLearning/ReadyState.cpp index 8aa9190f..1cc8ed2d 100644 --- a/lib/APPReinforcementLearning/ReadyState.cpp +++ b/lib/APPReinforcementLearning/ReadyState.cpp @@ -61,7 +61,11 @@ /****************************************************************************** * Local Variables *****************************************************************************/ - +const uint16_t ReadyState::SENSOR_VALUE_MAX = Board::getInstance().getLineSensors().getSensorValueMax(); +/* Initialize the required sensor IDs to be generic. */ +const uint8_t ReadyState::SENSOR_ID_MOST_LEFT = 0U; +const uint8_t ReadyState::SENSOR_ID_MIDDLE = Board::getInstance().getLineSensors().getNumLineSensors() / 2U; +const uint8_t ReadyState::SENSOR_ID_MOST_RIGHT = Board::getInstance().getLineSensors().getNumLineSensors() - 1U; /****************************************************************************** * Public Methods @@ -70,8 +74,6 @@ void ReadyState::entry() { IDisplay& display = Board::getInstance().getDisplay(); - const int32_t SENSOR_VALUE_OUT_PERIOD = 1000; /* ms */ - const int32_t Mode_Selected_period = 1000; /* ms*/ display.clear(); display.print("A: TMD"); display.gotoXY(0, 1); @@ -83,49 +85,53 @@ void ReadyState::entry() display.print(m_lapTime); display.print("ms"); } - - /* The line sensor value shall be output on console cyclic. */ - m_timer.start(SENSOR_VALUE_OUT_PERIOD); - - m_ModeTimeoutTimer.start(Mode_Selected_period); + m_ModeTimeoutTimer.start(mode_selected_period); } void ReadyState::process(StateMachine& sm) { IButton& buttonA = Board::getInstance().getButtonA(); IButton& buttonB = Board::getInstance().getButtonB(); - uint8_t selectedMode = 0; + ILineSensors& lineSensors = Board::getInstance().getLineSensors(); + uint8_t maxLineSensors = lineSensors.getNumLineSensors(); + const uint16_t* lineSensorValues = lineSensors.getSensorValues(); /* Shall the driving mode be released? */ if (true == buttonA.isPressed()) { buttonA.waitForRelease(); - myMode = DRIVING_MODE; - sm.setState(&DrivingState::getInstance()); + m_mode = DRIVING_MODE; } /* Shall the Training mode be released? */ else if ((true == buttonB.isPressed()) ) { buttonB.waitForRelease(); - myMode = TRAINING_MODE; - sm.setState(&DrivingState::getInstance()); + m_mode = TRAINING_MODE; } - else if (true == m_ModeTimeoutTimer.isTimeout()) + else if (true == m_ModeTimeoutTimer.isTimeout() && (m_mode == IDLE)) { - myMode = TRAINING_MODE; - sm.setState(&DrivingState::getInstance()); - + m_mode = TRAINING_MODE; } else { /* Nothing to do. */ ; + } + + DriveUntilStartLinecross(); + + if ((isStartStopLineDetected(lineSensorValues, maxLineSensors) == false) && (LastStatus == true)) + { + sm.setState(&DrivingState::getInstance()); + } + else + { + LastStatus = isStartStopLineDetected(lineSensorValues, maxLineSensors); } } void ReadyState::exit() { - m_timer.stop(); m_isLapTimeAvailable = false; } @@ -135,29 +141,39 @@ void ReadyState::setLapTime(uint32_t lapTime) m_lapTime = lapTime; } -uint8_t ReadyState::selectedMode() +uint8_t ReadyState::setSelectedMode() { - return static_cast(myMode); + return (m_mode); } +void ReadyState :: DriveUntilStartLinecross() +{ + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + int16_t top_speed = 2000; + int16_t leftMotor = top_speed / 2U; + int16_t rightMotor = top_speed / 2U; + diffDrive.setLinearSpeed(leftMotor, rightMotor); +} +bool ReadyState::isStartStopLineDetected(const uint16_t* lineSensorValues, uint8_t length) const +{ + bool isDetected = false; + const uint32_t LINE_MAX_30 = (SENSOR_VALUE_MAX * 3U) / 10U; /* 30 % of max. value */ + const uint32_t LINE_MAX_70 = (SENSOR_VALUE_MAX * 7U) / 10U; /* 70 % of max. value */ + + /* + * === = === + * + + + + + + * L M R + */ + if ((LINE_MAX_30 <= lineSensorValues[SENSOR_ID_MOST_LEFT]) && + (LINE_MAX_70 > lineSensorValues[SENSOR_ID_MIDDLE - 1U]) && + (LINE_MAX_70 <= lineSensorValues[SENSOR_ID_MIDDLE]) && + (LINE_MAX_70 > lineSensorValues[SENSOR_ID_MIDDLE + 1U]) && + (LINE_MAX_30 <= lineSensorValues[SENSOR_ID_MOST_RIGHT])) + { + isDetected = true; + } - - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ - /*if (true == m_smpServer.isSynced()) - {*/ \ No newline at end of file + return isDetected; +} diff --git a/lib/APPReinforcementLearning/ReadyState.h b/lib/APPReinforcementLearning/ReadyState.h index f2920025..6b76ee4f 100644 --- a/lib/APPReinforcementLearning/ReadyState.h +++ b/lib/APPReinforcementLearning/ReadyState.h @@ -99,25 +99,50 @@ class ReadyState : public IState void setLapTime(uint32_t lapTime); /** - * Returns the mode selection + * Set the selected mode. * - * @return It returns 1 if DrivingMode is selected or 2 if TrainingMode is selected or 0 if nothing is selected and there is no timeout.. + * @return It returns 1 if DrivingMode is selected or 2 if TrainingMode is selected. */ - uint8_t selectedMode( ); - /* Re-initialize the board. This is required for the webots simulation in - * case the world is reset by a supervisor without restarting the RadonUlzer - * controller executable. - */ - void reinit_Board(); + uint8_t setSelectedMode( ); protected: private: + + /** Duration of the selected mode in ms. This is the maximum time to select a mode. */ + static const uint32_t mode_selected_period = 500U; + + /** + * The line sensor threshold (normalized) used to detect the track. + * The track is detected in case the received value is greater or equal than + * the threshold. + */ + static const uint16_t LINE_SENSOR_ON_TRACK_MIN_VALUE = 200U; + + /** + * ID of most left sensor. + */ + static const uint8_t SENSOR_ID_MOST_LEFT; + /** - * the mode that can be selected. + * ID of middle sensor. */ - /**< Timer used for cyclic debug output. */ - SimpleTimer m_timer; + static const uint8_t SENSOR_ID_MIDDLE; + + /** + * ID of most right sensor. + */ + static const uint8_t SENSOR_ID_MOST_RIGHT; + + /** + * The max. normalized value of a sensor in digits. + */ + static const uint16_t SENSOR_VALUE_MAX; + + /** + * last Line Status + */ + bool LastStatus ; /** Timeout timer for the selected mode. */ SimpleTimer m_ModeTimeoutTimer; @@ -129,22 +154,25 @@ class ReadyState : public IState bool m_isLapTimeAvailable; /**< Lap time in ms of the last successful driven round. */ - uint32_t m_lapTime; - - enum Mode_: uint8_t + uint32_t m_lapTime; + + /** + * The mode that can be selected. + */ + enum mode: uint8_t { - DRIVING_MODE = 0, /**< Driving Mode Selected. */ - TRAINING_MODE /**< Training Mode Selected. */ + IDLE = 0, /**< No mode has been selected*/ + DRIVING_MODE, /**< Driving Mode Selected. */ + TRAINING_MODE /**< Training Mode Selected. */ }; - - Mode_ myMode; - + + mode m_mode; ReadyState() : - m_timer(), m_isLapTimeAvailable(false), m_lapTime(0), - myMode(TRAINING_MODE) + LastStatus(false), + m_mode(IDLE) { } @@ -158,6 +186,22 @@ class ReadyState : public IState /* Not allowed. */ ReadyState(const ReadyState& state); /**< Copy construction of an instance. */ ReadyState& operator=(const ReadyState& state); /**< Assignment of an instance. */ + + /* + * The robot moves from its current position + * until it crosses and leaves the start line. + */ + void DriveUntilStartLinecross(); + + /** + * Is the start/stop line detected? + * + * @param[in] lineSensorValues The line sensor values as array. + * @param[in] length The number of line sensor values. + * + * @return If start/stop line detected, it will return true otherwise false. + */ + bool isStartStopLineDetected(const uint16_t* lineSensorValues, uint8_t length) const; }; /****************************************************************************** @@ -165,4 +209,4 @@ class ReadyState : public IState *****************************************************************************/ #endif /* READY_STATE_H */ -/** @} */ +/** @} */ \ No newline at end of file From 7bc202139a5085a2de8fad66c16cbe7f8597db3c Mon Sep 17 00:00:00 2001 From: Akram Date: Wed, 17 Jul 2024 16:08:16 +0200 Subject: [PATCH 15/58] Updated supervisor logic: Supervisor now decides to stop motors if no line is detected for a specified time. --- lib/APPReinforcementLearning/App.cpp | 23 ++++++------ lib/APPReinforcementLearning/App.h | 4 +-- webots/controllers/Supervisor/Supervisor.py | 39 +++++++++++++++------ 3 files changed, 41 insertions(+), 25 deletions(-) diff --git a/lib/APPReinforcementLearning/App.cpp b/lib/APPReinforcementLearning/App.cpp index 3b9aff48..67879584 100644 --- a/lib/APPReinforcementLearning/App.cpp +++ b/lib/APPReinforcementLearning/App.cpp @@ -119,18 +119,14 @@ void App::loop() m_controlInterval.restart(); } - + if ((true == m_statusTimer.isTimeout()) && (true == m_smpServer.isSynced())&& (&DrivingState::getInstance() == m_systemStateMachine.getState())) { Status payload = {SMPChannelPayload::Status::NOT_DONE}; - ILineSensors& lineSensors = Board::getInstance().getLineSensors(); - uint8_t maxLineSensors = lineSensors.getNumLineSensors(); - const uint16_t* lineSensorValues = lineSensors.getSensorValues(); - bool m_isTrackLost = DrivingState::getInstance().isNoLineDetected(lineSensorValues, maxLineSensors); - if (DrivingState::getInstance().isAbortRequired(m_isTrackLost) == true) + + if (DrivingState::getInstance().isAbortRequired() == true) { payload = {SMPChannelPayload::Status::DONE}; - m_systemStateMachine.setState(&ReadyState::getInstance()); } (void)m_smpServer.sendData(m_serialMuxProtChannelIdStatus, &payload, sizeof(payload)); m_statusTimer.restart(); @@ -142,11 +138,12 @@ void App::loop() m_sendLineSensorsDataInterval.restart(); } + /* Send Mode selected to The Supervisor. */ + if (&ReadyState::getInstance() == m_systemStateMachine.getState() && (ReadyState::getInstance().setSelectedMode() != 0)) + { + Mode payload = {SMPChannelPayload::Mode::TRAINING_MODE}; - if (&ReadyState::getInstance() == m_systemStateMachine.getState()) - { - Mode payload = {SMPChannelPayload::Mode::TRAINING_MODE}; - if(ReadyState::getInstance().selectedMode() == 0) + if(ReadyState::getInstance().setSelectedMode() <= 1) { payload = {SMPChannelPayload::Mode::DRIVING_MODE}; } @@ -228,8 +225,8 @@ void App_motorSpeedSetpointsChannelCallback(const uint8_t* payload, const uint8_ DrivingState::getInstance().setTargetSpeeds(motorSpeedData->left, motorSpeedData->right); if((motorSpeedData->left == 0) && (motorSpeedData->right == 0)) { - m_systemStateMachine.setState(&StartupState::getInstance()); - printf("StartupState"); + ErrorState::getInstance().setErrorMsg("LNF"); + m_systemStateMachine.setState(&ErrorState::getInstance()); } } } \ No newline at end of file diff --git a/lib/APPReinforcementLearning/App.h b/lib/APPReinforcementLearning/App.h index 193347ed..7ba95bd2 100644 --- a/lib/APPReinforcementLearning/App.h +++ b/lib/APPReinforcementLearning/App.h @@ -93,10 +93,10 @@ class App SimpleTimer m_sendLineSensorsDataInterval; /** Send status timer interval in ms. */ - static const uint32_t SEND_STATUS_TIMER_INTERVAL = 24U; + static const uint32_t SEND_STATUS_TIMER_INTERVAL = 1000U; /** Sending Data period in ms. */ - static const uint32_t SEND_LINE_SENSORS_DATA_PERIOD = 24U; + static const uint32_t SEND_LINE_SENSORS_DATA_PERIOD = 20; /** SerialMuxProt Channel id for sending system status. */ uint8_t m_serialMuxProtChannelIdStatus; diff --git a/webots/controllers/Supervisor/Supervisor.py b/webots/controllers/Supervisor/Supervisor.py index 26143179..0dda736b 100644 --- a/webots/controllers/Supervisor/Supervisor.py +++ b/webots/controllers/Supervisor/Supervisor.py @@ -45,29 +45,47 @@ # The PROTO DEF name must be given! ROBOT_NAME = "ROBOT" -m_serialMuxProtChannelIdSPEED_SET = smp_server.create_channel("SPEED_SET", 4) +#DLC of SPEED_SET Channel +SPEED_SET_DLC = 4 + +m_serialMuxProtChannelIdSPEED_SET = smp_server.create_channel("SPEED_SET", SPEED_SET_DLC) + +# Counter for the number of times no line has been detected +noLineDetectionCount = 0 + +# The line sensor threshold +LINE_SENSOR_ON_TRACK_MIN_VALUE = 200 def callback_Status(payload: bytearray) -> int: """ Callback Status Channel """ if payload[0] == 1: - print("End-Start-Line Detected") + print("the max. time within the robot must be finished its drive is Done") smp_server.send_data("SPEED_SET", struct.pack('2H', 0, 0)) # Stop motors def callback_LineSensors(payload: bytearray)-> None: """ Callback LINE_SENS Channel""" + global noLineDetectionCount,LINE_SENSOR_ON_TRACK_MIN_VALUE sensor_data = struct.unpack('5H', payload) for idx in range (5): - print(f"Sensor[{idx}] = {sensor_data[idx]}") - smp_server.send_data("SPEED_SET", struct.pack('2H', 1000, 1000)) # Stop motors - + print(f"Sensor[{idx}] = {sensor_data[idx]}") + if all(value == 0 for value in sensor_data): + noLineDetectionCount += 1 + else: + noLineDetectionCount = 0 + + if noLineDetectionCount >= 20 or all(value >= LINE_SENSOR_ON_TRACK_MIN_VALUE for value in sensor_data): + smp_server.send_data("SPEED_SET", struct.pack('2H', 0, 0)) # Stop motors, maximum NO Line Detection Counter reached + else: + smp_server.send_data("SPEED_SET", struct.pack('2H', 1000, 1000)) + def callback_Mode(payload: bytearray)-> None: """ Callback MODE Channel""" - train_mode = payload[0] - if train_mode: - print("Train Mode Selected") + driving_mode = payload[0] + if driving_mode: + print("Driving Mode Selected") else: - print("Driving Mode Selected") + print("Train Mode Selected") smp_server.subscribe_to_channel("STATUS", callback_Status) smp_server.subscribe_to_channel("LINE_SENS",callback_LineSensors) @@ -84,7 +102,7 @@ def main_loop(): m_elapsedTimeSinceReset = 0 if m_serialMuxProtChannelIdSPEED_SET == 0: - print(f"Channel SPEED_SET not created.") + print("Channel SPEED_SET not created.") else: # Get robot node which to observe. robot_node = supervisor.getFromDef(ROBOT_NAME) @@ -98,3 +116,4 @@ def main_loop(): return status sys.exit(main_loop()) + From 1007013e983d7070da7287a33d5218519faf4ff7 Mon Sep 17 00:00:00 2001 From: Akram Date: Wed, 17 Jul 2024 16:44:15 +0200 Subject: [PATCH 16/58] Added serial RX/TX channels configuration for communication with the robot in the supervisor. --- webots/controllers/Supervisor/Supervisor.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/webots/controllers/Supervisor/Supervisor.py b/webots/controllers/Supervisor/Supervisor.py index 0dda736b..aeb47eb9 100644 --- a/webots/controllers/Supervisor/Supervisor.py +++ b/webots/controllers/Supervisor/Supervisor.py @@ -28,6 +28,10 @@ # Get the Supervisor Emitter device supervisor_com_Tx = supervisor.getDevice(SUPERVISOR_TX_NAME) +# Set serial rx/tx channels for communication with the Roboter. +supervisor_com_rx.setChannel(2) +supervisor_com_Tx.setChannel(1) + # Enable supervisor receiver to receive any message from the robot or other # devices in the simulation. if supervisor_com_rx is None: From bac84b6d3de63510d6f01c898b4896a675354d9a Mon Sep 17 00:00:00 2001 From: Akram Date: Wed, 17 Jul 2024 17:08:14 +0200 Subject: [PATCH 17/58] Removed duplicate declaration of Robot_Name in Supervisor. --- webots/controllers/Supervisor/Supervisor.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/webots/controllers/Supervisor/Supervisor.py b/webots/controllers/Supervisor/Supervisor.py index aeb47eb9..2e87450e 100644 --- a/webots/controllers/Supervisor/Supervisor.py +++ b/webots/controllers/Supervisor/Supervisor.py @@ -46,9 +46,6 @@ S_client = SerialWebots(supervisor_com_Tx, supervisor_com_rx) smp_server = SerialMuxProt(10, S_client) -# The PROTO DEF name must be given! -ROBOT_NAME = "ROBOT" - #DLC of SPEED_SET Channel SPEED_SET_DLC = 4 From 80d9a5f89f9cc1447d46a70a9679e43df436c371 Mon Sep 17 00:00:00 2001 From: Akram Date: Thu, 18 Jul 2024 17:23:31 +0200 Subject: [PATCH 18/58] Created a new communication channel to handle the reinitialization of the state machine in the Ready State and the position and orientation of the robot. --- lib/APPReinforcementLearning/App.cpp | 65 ++++++++++++++----- lib/APPReinforcementLearning/App.h | 20 ++++-- lib/APPReinforcementLearning/DrivingState.cpp | 4 -- lib/APPReinforcementLearning/ReadyState.cpp | 27 +++++--- lib/APPReinforcementLearning/ReadyState.h | 4 +- .../SerialMuxchannels.h | 51 +-------------- webots/controllers/Supervisor/Supervisor.py | 49 ++++++++++---- 7 files changed, 124 insertions(+), 96 deletions(-) diff --git a/lib/APPReinforcementLearning/App.cpp b/lib/APPReinforcementLearning/App.cpp index 67879584..7c6a33cb 100644 --- a/lib/APPReinforcementLearning/App.cpp +++ b/lib/APPReinforcementLearning/App.cpp @@ -42,6 +42,9 @@ *****************************************************************************/ static void App_motorSpeedSetpointsChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData); +/* Initialization of the static variable*/ +App* App::appInstance = nullptr; + /****************************************************************************** * Local Variables *****************************************************************************/ @@ -139,15 +142,15 @@ void App::loop() m_sendLineSensorsDataInterval.restart(); } /* Send Mode selected to The Supervisor. */ - if (&ReadyState::getInstance() == m_systemStateMachine.getState() && (ReadyState::getInstance().setSelectedMode() != 0)) + if (&ReadyState::getInstance() == m_systemStateMachine.getState() && (!m_sentmode)) { - Mode payload = {SMPChannelPayload::Mode::TRAINING_MODE}; - - if(ReadyState::getInstance().setSelectedMode() <= 1) + int modeSelection = ReadyState::getInstance().setSelectedMode(); + if(modeSelection > 0) { - payload = {SMPChannelPayload::Mode::DRIVING_MODE}; + SMPChannelPayload::Mode payload = (modeSelection == 1) ? SMPChannelPayload::Mode::DRIVING_MODE : SMPChannelPayload::Mode::TRAINING_MODE; + (void)m_smpServer.sendData(m_serialMuxProtChannelIdMode, &payload, sizeof(payload)); + m_sentmode = true; } - (void)m_smpServer.sendData(m_serialMuxProtChannelIdMode, &payload, sizeof(payload)); } @@ -169,11 +172,12 @@ bool App::setupSerialMuxProt() { bool isSuccessful = false; m_smpServer.subscribeToChannel(SPEED_SETPOINT_CHANNEL_NAME, App_motorSpeedSetpointsChannelCallback); + m_smpServer.subscribeToChannel(STATUS_CHANNEL_NAME,App_statusChannelCallback); /* Channel creation. */ m_serialMuxProtChannelIdStatus = m_smpServer.createChannel(STATUS_CHANNEL_NAME, STATUS_CHANNEL_DLC); m_serialMuxProtChannelIdLineSensors = m_smpServer.createChannel(LINE_SENSOR_CHANNEL_NAME, LINE_SENSOR_CHANNEL_DLC); m_serialMuxProtChannelIdMode = m_smpServer.createChannel(MODE_CHANNEL_NAME, MODE_CHANNEL_DLC); - + /* Channels succesfully created? */ if ((0U != m_serialMuxProtChannelIdStatus) && (0U != m_serialMuxProtChannelIdLineSensors) && (0U != m_serialMuxProtChannelIdMode)) @@ -205,6 +209,25 @@ void App::sendLineSensorsData() const (void)m_smpServer.sendData(m_serialMuxProtChannelIdLineSensors, &payload, sizeof(payload)); } +void App::systemStatusCallback(SMPChannelPayload::Status status) +{ + switch (status) + { + case SMPChannelPayload::NOT_DONE: + /* Nothing to do. All good. */ + break; + case SMPChannelPayload::DONE: + Odometry::getInstance().clearPosition(); + Odometry::getInstance().clearMileage(); + m_systemStateMachine.setState(&ReadyState::getInstance()); + m_sentmode = false; + + break; + + default: + break; + } +} /****************************************************************************** * External Functions @@ -213,20 +236,30 @@ void App::sendLineSensorsData() const /****************************************************************************** * Local Functions *****************************************************************************/ +/** + * Receives current status of the DCS over SerialMuxProt channel. + * + * @param[in] payload Status of the DCS. + * @param[in] payloadSize Size of the Status Flag + * @param[in] userData Instance of App class. + */ +void App::App_statusChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData) +{ + + if ((nullptr != payload) && (STATUS_CHANNEL_DLC == payloadSize) && (nullptr != App::appInstance)) + { + const Status* currentStatus = reinterpret_cast(payload); + App::appInstance->systemStatusCallback(currentStatus->status); + } +} void App_motorSpeedSetpointsChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData) { - - (void)userData; if ((nullptr != payload) && (SPEED_SETPOINT_CHANNEL_DLC == payloadSize)) - { - StateMachine m_systemStateMachine; + { const SpeedData* motorSpeedData = reinterpret_cast(payload); + //printf("Payload %d", motorSpeedData->left); DrivingState::getInstance().setTargetSpeeds(motorSpeedData->left, motorSpeedData->right); - if((motorSpeedData->left == 0) && (motorSpeedData->right == 0)) - { - ErrorState::getInstance().setErrorMsg("LNF"); - m_systemStateMachine.setState(&ErrorState::getInstance()); - } + } } \ No newline at end of file diff --git a/lib/APPReinforcementLearning/App.h b/lib/APPReinforcementLearning/App.h index 7ba95bd2..32b40730 100644 --- a/lib/APPReinforcementLearning/App.h +++ b/lib/APPReinforcementLearning/App.h @@ -50,8 +50,10 @@ class App m_serialMuxProtChannelIdMode(0U), m_statusTimer(), m_sendLineSensorsDataInterval(), + m_sentmode(false), m_smpServer(Serial, nullptr) { + appInstance = this; } /** @@ -70,7 +72,12 @@ class App * Process the application periodically. */ void loop(); - + /** + * System Status callback. + * + * @param[in] status System status + */ + void systemStatusCallback(SMPChannelPayload::Status status); private: /** Differential drive control period in ms. */ static const uint32_t DIFFERENTIAL_DRIVE_CONTROL_PERIOD = 5U; @@ -78,6 +85,9 @@ class App /** Baudrate for Serial Communication */ static const uint32_t SERIAL_BAUDRATE = 115200U; + /** Saving the APP instance */ + static App* appInstance; + /** The system state machine. */ StateMachine m_systemStateMachine; @@ -96,7 +106,7 @@ class App static const uint32_t SEND_STATUS_TIMER_INTERVAL = 1000U; /** Sending Data period in ms. */ - static const uint32_t SEND_LINE_SENSORS_DATA_PERIOD = 20; + static const uint32_t SEND_LINE_SENSORS_DATA_PERIOD = 20U; /** SerialMuxProt Channel id for sending system status. */ uint8_t m_serialMuxProtChannelIdStatus; @@ -110,7 +120,9 @@ class App /** SerialMuxProt Server Instance. */ SMPServer m_smpServer; - + /* Ensue that the mode is only sent once*/ + bool m_sentmode; + /** * Setup the SerialMuxProt channels. * @@ -123,7 +135,7 @@ class App */ void sendLineSensorsData() const; - + static void App_statusChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData); /* Not allowed. */ App(const App& app); /**< Copy construction of an instance. */ App& operator=(const App& app); /**< Assignment of an instance. */ diff --git a/lib/APPReinforcementLearning/DrivingState.cpp b/lib/APPReinforcementLearning/DrivingState.cpp index 908732a1..635e91c7 100644 --- a/lib/APPReinforcementLearning/DrivingState.cpp +++ b/lib/APPReinforcementLearning/DrivingState.cpp @@ -66,7 +66,6 @@ void DrivingState::entry() { DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); diffDrive.setLinearSpeed(0, 0); - diffDrive.enable(); m_observationTimer.start(OBSERVATION_DURATION); } @@ -78,9 +77,6 @@ void DrivingState::process(StateMachine& sm) void DrivingState::exit() { - /* Stop motors. */ - DifferentialDrive::getInstance().setLinearSpeed(0, 0); - DifferentialDrive::getInstance().disable(); m_observationTimer.stop(); } diff --git a/lib/APPReinforcementLearning/ReadyState.cpp b/lib/APPReinforcementLearning/ReadyState.cpp index 1cc8ed2d..5b79418b 100644 --- a/lib/APPReinforcementLearning/ReadyState.cpp +++ b/lib/APPReinforcementLearning/ReadyState.cpp @@ -73,19 +73,23 @@ const uint8_t ReadyState::SENSOR_ID_MOST_RIGHT = Board::getInstance().getLineSen void ReadyState::entry() { - IDisplay& display = Board::getInstance().getDisplay(); + IDisplay& display = Board::getInstance().getDisplay(); display.clear(); display.print("A: TMD"); display.gotoXY(0, 1); display.print("B: DMD"); + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + diffDrive.setLinearSpeed(0, 0); + LastStatus = false; + if (true == m_isLapTimeAvailable) { display.gotoXY(0, 2); display.print(m_lapTime); display.print("ms"); } - m_ModeTimeoutTimer.start(mode_selected_period); + m_modeTimeoutTimer.start(mode_selected_period); } void ReadyState::process(StateMachine& sm) @@ -94,7 +98,8 @@ void ReadyState::process(StateMachine& sm) IButton& buttonB = Board::getInstance().getButtonB(); ILineSensors& lineSensors = Board::getInstance().getLineSensors(); uint8_t maxLineSensors = lineSensors.getNumLineSensors(); - const uint16_t* lineSensorValues = lineSensors.getSensorValues(); + const uint16_t* lineSensorValues = lineSensors.getSensorValues(); + m_mode = IDLE; /* Shall the driving mode be released? */ if (true == buttonA.isPressed()) { @@ -107,26 +112,27 @@ void ReadyState::process(StateMachine& sm) buttonB.waitForRelease(); m_mode = TRAINING_MODE; } - else if (true == m_ModeTimeoutTimer.isTimeout() && (m_mode == IDLE)) + else if (true == m_modeTimeoutTimer.isTimeout() && (m_mode == IDLE)) { - m_mode = TRAINING_MODE; + m_mode = TRAINING_MODE; + m_modeTimeoutTimer.restart(); } else { /* Nothing to do. */ ; } - - DriveUntilStartLinecross(); - + /**Drive forward until START LINE is crossed */ + DriveUntilStartLinecross(); if ((isStartStopLineDetected(lineSensorValues, maxLineSensors) == false) && (LastStatus == true)) { sm.setState(&DrivingState::getInstance()); } - else + else { LastStatus = isStartStopLineDetected(lineSensorValues, maxLineSensors); - } + } + } @@ -146,6 +152,7 @@ uint8_t ReadyState::setSelectedMode() return (m_mode); } +/**Drive forward until START LINE is crossed */ void ReadyState :: DriveUntilStartLinecross() { DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); diff --git a/lib/APPReinforcementLearning/ReadyState.h b/lib/APPReinforcementLearning/ReadyState.h index 6b76ee4f..dced9196 100644 --- a/lib/APPReinforcementLearning/ReadyState.h +++ b/lib/APPReinforcementLearning/ReadyState.h @@ -145,7 +145,7 @@ class ReadyState : public IState bool LastStatus ; /** Timeout timer for the selected mode. */ - SimpleTimer m_ModeTimeoutTimer; + SimpleTimer m_modeTimeoutTimer; /** The system state machine. */ StateMachine m_systemStateMachine; @@ -165,11 +165,11 @@ class ReadyState : public IState DRIVING_MODE, /**< Driving Mode Selected. */ TRAINING_MODE /**< Training Mode Selected. */ }; - mode m_mode; ReadyState() : m_isLapTimeAvailable(false), + m_modeTimeoutTimer(), m_lapTime(0), LastStatus(false), m_mode(IDLE) diff --git a/lib/APPReinforcementLearning/SerialMuxchannels.h b/lib/APPReinforcementLearning/SerialMuxchannels.h index f4068122..239aa823 100644 --- a/lib/APPReinforcementLearning/SerialMuxchannels.h +++ b/lib/APPReinforcementLearning/SerialMuxchannels.h @@ -46,18 +46,6 @@ /** Maximum number of SerialMuxProt Channels. */ #define MAX_CHANNELS (10U) -/** Name of Channel to send Commands to. */ -#define COMMAND_CHANNEL_NAME "CMD" - -/** DLC of Command Channel. */ -#define COMMAND_CHANNEL_DLC (sizeof(Command)) - -/** Name of Channel to receive Command Responses from. */ -#define COMMAND_RESPONSE_CHANNEL_NAME "CMD_RSP" - -/** DLC of Command Response Channel. */ -#define COMMAND_RESPONSE_CHANNEL_DLC (sizeof(CommandResponse)) - /** Name of Channel to send system status to. */ #define STATUS_CHANNEL_NAME "STATUS" @@ -94,23 +82,7 @@ typedef SerialMuxProtServer SMPServer; /** Channel payload constants. */ namespace SMPChannelPayload { - /** Remote control commands. */ - typedef enum : uint8_t - { - CMD_ID_IDLE = 0, /**< Nothing to do. */ - CMD_ID_SET_INIT_POS, /**< Set initial position. */ - CMD_ID_GET_MAX_SPEED /**< Get maximum speed. */ - - } CmdId; /**< Command ID */ - - /** Remote control command responses. */ - typedef enum : uint8_t - { - RSP_ID_OK = 0, /**< Command successful executed. */ - RSP_ID_PENDING, /**< Command is pending. */ - RSP_ID_ERROR /**< Command failed. */ - } RspId; /**< Response ID */ /** Status flags. */ typedef enum : uint8_t { @@ -120,31 +92,14 @@ namespace SMPChannelPayload } Status; /**< Status flag */ typedef enum : uint8_t - { - DRIVING_MODE = 0, /**< Driving Mode Selected. */ - TRAINING_MODE /**< Training Mode Selected. */ + { + TRAINING_MODE = 0, /**< Driving Mode Selected. */ + DRIVING_MODE /**< Training Mode Selected. */ } Mode; /**< Status flag */ } -/** Struct of the "Command" channel payload. */ -typedef struct _Command -{ - SMPChannelPayload::CmdId commandId; /**< Command ID */ -} __attribute__((packed)) Command; - -/** Struct of the "Command Response" channel payload. */ -typedef struct _CommandResponse -{ - SMPChannelPayload::RspId responseId; /**< Response to the command */ - - /** Response Payload. */ - union - { - int16_t maxMotorSpeed; /**< Max speed [steps/s]. */ - }; -} __attribute__((packed)) CommandResponse; /** Struct of the "Speed" channel payload. */ typedef struct _SpeedData diff --git a/webots/controllers/Supervisor/Supervisor.py b/webots/controllers/Supervisor/Supervisor.py index 2e87450e..4e1bf0ff 100644 --- a/webots/controllers/Supervisor/Supervisor.py +++ b/webots/controllers/Supervisor/Supervisor.py @@ -48,35 +48,57 @@ #DLC of SPEED_SET Channel SPEED_SET_DLC = 4 - -m_serialMuxProtChannelIdSPEED_SET = smp_server.create_channel("SPEED_SET", SPEED_SET_DLC) +#DLC of STATUS Channel +STATUS_DLC = 1 # Counter for the number of times no line has been detected noLineDetectionCount = 0 -# The line sensor threshold -LINE_SENSOR_ON_TRACK_MIN_VALUE = 200 +# The line sensor threshold. +Line_Sensor_On_Track_Min_Value = 200 + +# The counter of how much it has been reset. +Reset_Count = 0 + +def reinitialize (): + """ Re-initialization of position and orientation of The ROBOT """ + global robot_node,Reset_Count + trans_field = robot_node.getField("translation") + rot_field = robot_node.getField("rotation") + intial_position =[-0.24713614078815466, -0.04863962992854465, 0.013994298332013683] + initial_orientation =[-1.0564747468923541e-06, 8.746699709178704e-07, 0.9999999999990595, 1.5880805820884731] + trans_field.setSFVec3f(intial_position) + rot_field.setSFRotation(initial_orientation) + Reset_Count = 0 def callback_Status(payload: bytearray) -> int: """ Callback Status Channel """ if payload[0] == 1: print("the max. time within the robot must be finished its drive is Done") smp_server.send_data("SPEED_SET", struct.pack('2H', 0, 0)) # Stop motors + smp_server.send_data("STATUS",struct.pack('B', 1)) + reinitialize() def callback_LineSensors(payload: bytearray)-> None: """ Callback LINE_SENS Channel""" - global noLineDetectionCount,LINE_SENSOR_ON_TRACK_MIN_VALUE + global noLineDetectionCount, Line_Sensor_On_Track_Min_Value, Reset_Count sensor_data = struct.unpack('5H', payload) for idx in range (5): - print(f"Sensor[{idx}] = {sensor_data[idx]}") + if idx == 4: + print(f"Sensor[{idx}] = {sensor_data[idx]}") + else : + print(f"Sensor[{idx}] = {sensor_data[idx]}, ", end="") + if all(value == 0 for value in sensor_data): noLineDetectionCount += 1 else: noLineDetectionCount = 0 - if noLineDetectionCount >= 20 or all(value >= LINE_SENSOR_ON_TRACK_MIN_VALUE for value in sensor_data): - smp_server.send_data("SPEED_SET", struct.pack('2H', 0, 0)) # Stop motors, maximum NO Line Detection Counter reached + if noLineDetectionCount >= 20 or all(value >= Line_Sensor_On_Track_Min_Value for value in sensor_data): + smp_server.send_data("SPEED_SET", struct.pack('2H', 0, 0)) # Stop motors, maximum NO Line Detection Counter reached + smp_server.send_data("STATUS",struct.pack('B', 1)) # SEND STATUS DONE + Reset_Count += 1 else: smp_server.send_data("SPEED_SET", struct.pack('2H', 1000, 1000)) @@ -86,8 +108,11 @@ def callback_Mode(payload: bytearray)-> None: if driving_mode: print("Driving Mode Selected") else: - print("Train Mode Selected") + print("Train Mode Selected") + +m_serialMuxProtChannelIdSPEED_SET = smp_server.create_channel("SPEED_SET", SPEED_SET_DLC) +m_serialMuxProtChannelIdSTATUS = smp_server.create_channel("STATUS", STATUS_DLC) smp_server.subscribe_to_channel("STATUS", callback_Status) smp_server.subscribe_to_channel("LINE_SENS",callback_LineSensors) smp_server.subscribe_to_channel("MODE",callback_Mode) @@ -102,11 +127,9 @@ def main_loop(): status = 0 m_elapsedTimeSinceReset = 0 - if m_serialMuxProtChannelIdSPEED_SET == 0: + if m_serialMuxProtChannelIdSPEED_SET == 0 or m_serialMuxProtChannelIdSTATUS == 0 : print("Channel SPEED_SET not created.") else: - # Get robot node which to observe. - robot_node = supervisor.getFromDef(ROBOT_NAME) if robot_node is None: print(f"Robot DEF {ROBOT_NAME} not found.") status = -1 @@ -114,6 +137,8 @@ def main_loop(): while supervisor.step(timestep) != -1: m_elapsedTimeSinceReset += timestep smp_server.process(m_elapsedTimeSinceReset) + if Reset_Count != 0: + reinitialize() return status sys.exit(main_loop()) From c3c65304d391394ef67f15776895113e3f27fa4a Mon Sep 17 00:00:00 2001 From: Akram bziouech <146352842+akrambzeo@users.noreply.github.com> Date: Fri, 19 Jul 2024 10:54:57 +0200 Subject: [PATCH 19/58] Remove unnecessary file --- .../controllers/Supervisor/SerialMuxProt.py | 648 ------------------ 1 file changed, 648 deletions(-) delete mode 100644 webots/controllers/Supervisor/SerialMuxProt.py diff --git a/webots/controllers/Supervisor/SerialMuxProt.py b/webots/controllers/Supervisor/SerialMuxProt.py deleted file mode 100644 index 8c9deae2..00000000 --- a/webots/controllers/Supervisor/SerialMuxProt.py +++ /dev/null @@ -1,648 +0,0 @@ -""" Serial Multiplexer Protocol (SerialMuxProt) for lightweight communication. """ - -# MIT License -# -# Copyright (c) 2023 Gabryel Reyes -# -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in all -# copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS -# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -# SOFTWARE. -# - - -################################################################################ -# Imports -################################################################################ - -from dataclasses import dataclass -from struct import Struct -from Serial_webots import SerialWebots - -################################################################################ -# Variables -################################################################################ - -################################################################################ -# Classes -################################################################################ - - -@dataclass(frozen=True) -class SerialMuxProtConstants: - """ Container Data class for SerialMuxProt Constants """ - - CHANNEL_LEN = 1 # Channel Field Length in Bytes - DLC_LEN = 1 # DLC Field Length in Bytes - CHECKSUM_LEN = 1 # Checksum Field Length in Bytes - HEADER_LEN = CHANNEL_LEN + DLC_LEN + \ - CHECKSUM_LEN # Length of Complete Header Field - MAX_DATA_LEN = 32 # Data Field Length in Bytes - MAX_FRAME_LEN = HEADER_LEN + MAX_DATA_LEN # Total Frame Length in Bytes - CHANNEL_NAME_MAX_LEN = 10 # Max length of channel name - # Available Bytes in Control Channel Payload for data. - CONTROL_CHANNEL_PAYLOAD_DATA_LENGTH = 4 - CONTROL_CHANNEL_CMD_BYTE_LENGTH = 1 # Lenght of Command in Bytes - CONTROL_CHANNEL_NUMBER = 0 # Number of Control Channel. - CONTROL_CHANNEL_PAYLOAD_LENGTH = CHANNEL_NAME_MAX_LEN + \ - CONTROL_CHANNEL_PAYLOAD_DATA_LENGTH + \ - CONTROL_CHANNEL_CMD_BYTE_LENGTH + \ - CHANNEL_LEN # DLC of Heartbeat Command. - # Index of the Command Byte of the Control Channel - CONTROL_CHANNEL_COMMAND_INDEX = 0 - # Index of the start of the payload of the Control Channel - CONTROL_CHANNEL_PAYLOAD_INDEX = 1 - HEATBEAT_PERIOD_SYNCED = 5000 # Period of Heartbeat when Synced. - HEATBEAT_PERIOD_UNSYNCED = 1000 # Period of Heartbeat when Unsynced - # Max number of attempts at receiving a Frame before resetting RX Buffer - MAX_RX_ATTEMPTS = MAX_FRAME_LEN - - @dataclass - class Commands(): - """ Enumeration of Commands of Control Channel. """ - SYNC = 0 - SYNC_RSP = 1 - SCRB = 2 - SCRB_RSP = 3 - - -@dataclass -class Channel(): - """ Channel Definition """ - - def __init__(self, channel_name: str = "", channel_dlc: int = 0, channel_callback=None) -> None: - self.name = channel_name - self.dlc = channel_dlc - self.callback = channel_callback - - -class Frame(): - """ Frame Defintion """ - - def __init__(self) -> None: - self.raw = bytearray(SerialMuxProtConstants.MAX_FRAME_LEN) - self.channel = 0 - self.dlc = 0 - self.checksum = 0 - self.payload = bytearray(SerialMuxProtConstants.MAX_DATA_LEN) - - def unpack_header(self): - """ Unpack/parse channel number and checksum from raw frame """ - header_packer = Struct(">3B") - self.channel, self.dlc, self.checksum = header_packer.unpack_from( - self.raw) - - def unpack_payload(self): - """ Unpack/parse payload from raw frame """ - self.payload = self.raw[3:] - - def pack_frame(self): - """ Pack header and payload into raw array""" - self.raw[0] = self.channel - self.raw[1] = self.dlc - self.raw[2] = self.checksum - self.raw[3:] = self.payload - - -@dataclass -class ChannelArrays: - """ Container Class for Channel Arrays and their counters """ - - def __init__(self, max_configured_channels: int) -> None: - self.number_of_rx_channels = 0 - self.number_of_tx_channels = 0 - self.number_of_pending_channels = 0 - self.rx_channels = [Channel() for x in range(max_configured_channels)] - self.tx_channels = [Channel() for x in range(max_configured_channels)] - self.pending_channels = [Channel() - for x in range(max_configured_channels)] - - -@dataclass -class SyncData: - """ Container Dataclass for Synchronization Data. """ - - def __init__(self) -> None: - self.is_synced = False - self.last_sync_response = 0 - self.last_sync_command = 0 - - -@dataclass -class RxData: - """ Container Dataclass for Receive Data and counters. """ - - def __init__(self) -> None: - self.received_bytes = 0 - self.rx_attempts = 0 - self.receive_frame = Frame() - - -class SerialMuxProt: - """ SerialMuxProt Server """ - - def __init__(self, max_configured_channels: int, stream: SerialWebots) -> None: - self.__max_configured_channels = max_configured_channels - self.__stream = stream - self.__rx_data = RxData() - self.__sync_data = SyncData() - self.__channels = ChannelArrays(max_configured_channels) - - def process(self, current_timestamp: int) -> None: - """Manage the Server functions. - Call this function cyclic. - - Parameters - ---------- - current_timestamp : int - Time in milliseconds. - - """ - - # Periodic Heartbeat. - self.__heartbeat(current_timestamp) - - # Process RX data. - self.__process_rx() - - def send_data(self, channel_name: str, payload: bytearray) -> bool: - """Send a frame with the selected bytes. - - Parameters: - ---------- - channel_name : str - Channel to send frame to. - payload : bytearray - Byte buffer to be sent. - - Returns: - -------- - If payload succesfully sent, returns true. Otherwise, false. - """ - is_sent = False - channel_number = self.get_tx_channel_number(channel_name) - - if (SerialMuxProtConstants.CONTROL_CHANNEL_NUMBER != channel_number) and\ - (self.__sync_data.is_synced is True): - is_sent = self.__send(channel_number, payload) - - return is_sent - - def is_synced(self) -> bool: - """ Returns current Sync state of the SerialMuxProt Server. """ - return self.__sync_data.is_synced - - def get_number_rx_channels(self) -> int: - """Get the number of configured RX channels.""" - return self.__channels.number_of_rx_channels - - def get_number_tx_channels(self) -> int: - """Get the number of configured TX channels.""" - return self.__channels.number_of_tx_channels - - def create_channel(self, name: str, dlc: int) -> int: - """Creates a new TX Channel on the server. - - Parameters: - ---------- - name : str - Name of the channel. It will not be checked if the name already exists. - dlc : int - Length of the payload of this channel. - - Returns: - -------- - The channel number if succesfully created, or 0 if not able to create new channel. - """ - name_length = len(name) - idx = 0 - - if (0 != name_length) and\ - (SerialMuxProtConstants.CHANNEL_NAME_MAX_LEN >= name_length) and\ - (0 != dlc) and\ - (SerialMuxProtConstants.MAX_DATA_LEN >= dlc) and\ - (self.__max_configured_channels > self.__channels.number_of_tx_channels): - - # Create Channel - self.__channels.tx_channels[self.__channels.number_of_tx_channels] = Channel( - name, dlc) - - # Increase Channel Counter - self.__channels.number_of_tx_channels += 1 - - # Provide Channel Number. Could be summarized with operation above. - idx = self.__channels.number_of_tx_channels - - return idx - - def subscribe_to_channel(self, name: str, callback) -> None: - """ Suscribe to a Channel to receive the incoming data. - - Parameters: - ---------- - name : str - Name of the Channel to suscribe to. - callback : function - Callback to return the incoming data. - """ - - if (SerialMuxProtConstants.CHANNEL_NAME_MAX_LEN >= len(name)) and\ - (callback is not None) and\ - (self.__max_configured_channels > self.__channels.number_of_pending_channels): - - # Save Name and Callback for channel creation after response - self.__channels.pending_channels[self.__channels.number_of_pending_channels] = Channel( - channel_name=name, channel_dlc=0, channel_callback=callback) - # Increase Channel Counter - self.__channels.number_of_pending_channels += 1 - - def get_tx_channel_number(self, channel_name: str) -> int: - """Get Number of a TX channel by its name. - - Parameters: - ----------- - channel_name : str - Name of channel - - Returns: - -------- - Number of the Channel, or 0 if not channel with the name is present. - """ - - channel_number = 0 - for idx in range(self.__max_configured_channels): - if self.__channels.tx_channels[idx].name == channel_name: - channel_number = idx + 1 - break - - return channel_number - - def __heartbeat(self, current_timestamp: int) -> None: - """ Periodic heartbeat. - Sends SYNC Command depending on the current Sync state. - - Parameters - ---------- - current_timestamp : int - Time in milliseconds. - - """ - heartbeat_period = SerialMuxProtConstants.HEATBEAT_PERIOD_UNSYNCED - - if self.__sync_data.is_synced is True: - heartbeat_period = SerialMuxProtConstants.HEATBEAT_PERIOD_SYNCED - if (current_timestamp - self.__sync_data.last_sync_command) >= heartbeat_period: - - # Timeout - if self.__sync_data.last_sync_command != self.__sync_data.last_sync_response: - self.__sync_data.is_synced = False - - # Pack big-endian uint32 - packer = Struct(">L") - timestamp_array = packer.pack(current_timestamp) - - # Send SYNC Command - command = bytearray() - command.append(SerialMuxProtConstants.Commands.SYNC) - command.extend(timestamp_array) - - # Pad array if necessary - command = command.ljust( - SerialMuxProtConstants.CONTROL_CHANNEL_PAYLOAD_LENGTH, b'\x00') - if self.__send(SerialMuxProtConstants.CONTROL_CHANNEL_NUMBER, command) is True: - self.__sync_data.last_sync_command = current_timestamp - - def __process_rx(self) -> None: - """ Receive and process RX Data. """ - - expected_bytes = 0 - dlc = 0 - # Determine how many bytes to read. - if SerialMuxProtConstants.HEADER_LEN > self.__rx_data.received_bytes: - # Header must be read. - expected_bytes = SerialMuxProtConstants.HEADER_LEN - self.__rx_data.received_bytes - else: - # Header has been read. Get DLC of Rx Channel using header. - self.__rx_data.receive_frame.unpack_header() - # dlc = self.__get_channel_dlc(self.__receive_frame.channel, False) - dlc = self.__rx_data.receive_frame.dlc - # DLC = 0 means that the channel does not exist. - if (0 != dlc) and (SerialMuxProtConstants.MAX_RX_ATTEMPTS >= self.__rx_data.rx_attempts): - remaining_payload_bytes = self.__rx_data.received_bytes - \ - SerialMuxProtConstants.HEADER_LEN - expected_bytes = dlc - remaining_payload_bytes - - self.__rx_data.rx_attempts += 1 - # Are we expecting to read anything? - if 0 != expected_bytes: - # Read the required amount of bytes, if available. - if self.__stream.available() >= expected_bytes: - rcvd, data = self.__stream.read_bytes(expected_bytes) - self.__rx_data.receive_frame.raw[self.__rx_data.received_bytes:] = data - self.__rx_data.received_bytes += rcvd - - # Frame has been received. - if (0 != dlc) and ((SerialMuxProtConstants.HEADER_LEN + dlc) == self.__rx_data.received_bytes): - # Check validity - if self.__is_frame_valid(self.__rx_data.receive_frame) is True: - channel_array_index = self.__rx_data.receive_frame.channel - 1 - self.__rx_data.receive_frame.unpack_payload() - - - # Differenciate between Control and Data Channels. - if SerialMuxProtConstants.CONTROL_CHANNEL_NUMBER == self.__rx_data.receive_frame.channel: - self.__callback_control_channel( - self.__rx_data.receive_frame.payload) - elif self.__channels.rx_channels[channel_array_index].callback is not None: - self.__channels.rx_channels[channel_array_index].callback( - self.__rx_data.receive_frame.payload) - - # Frame received. Cleaning! - self.__clear_local_buffers() - else: - # Invalid Header. Delete Frame! - self.__clear_local_buffers() - - def __process_subscriptions(self) -> None: - """ Subscribe to any pending Channels if synced to server. """ - - # If synced and a channel is pending - if (self.__sync_data.is_synced is True) and\ - (0 < self.__channels.number_of_pending_channels): - # Channel Iterator - for pending_channel in self.__channels.pending_channels: - - # Channel is pending - if pending_channel.callback is not None: - - # Subscribe to Channel - request = bytearray( - SerialMuxProtConstants.CONTROL_CHANNEL_PAYLOAD_LENGTH) - request[0] = SerialMuxProtConstants.Commands.SCRB - request[6:] = bytearray(pending_channel.name, 'ascii') - - # Pad array if necessary - request = request.ljust( - SerialMuxProtConstants.CONTROL_CHANNEL_PAYLOAD_LENGTH, b'\x00') - - if self.__send(SerialMuxProtConstants.CONTROL_CHANNEL_NUMBER, request) is False: - # Fall out of sync if failed to send. - self.__sync_data.is_synced = False - break - - def __clear_local_buffers(self) -> None: - """ Clear Local RX Buffers """ - self.__rx_data.receive_frame = Frame() - self.__rx_data.received_bytes = 0 - self.__rx_data.rx_attempts = 0 - - def __get_tx_channel_dlc(self, channel_number: int) -> int: - """ Get the Payload Length of a channel. - - Parameters - ---------- - channel_number : int - Channel number to check. - - is_tx_channel : bool - Is the Channel a TX Channel? If false, will return value for an RX Channel instead. - - Returns - ------- - DLC of the channel, or 0 if channel is not found. - """ - dlc = 0 - - if SerialMuxProtConstants.CONTROL_CHANNEL_NUMBER == channel_number: - dlc = SerialMuxProtConstants.CONTROL_CHANNEL_PAYLOAD_LENGTH - else: - if self.__max_configured_channels >= channel_number: - channel_idx = channel_number - 1 - dlc = self.__channels.tx_channels[channel_idx].dlc - - return dlc - - def __is_frame_valid(self, frame: Frame) -> bool: - """ Check if a Frame is valid using its checksum. - - Parameters - ---------- - frame : Frame - Frame to be checked - - Returns: - True if the Frame's checksum is correct. Otherwise, False. - """ - - return self.__checksum(frame.raw) == frame.checksum - - def __checksum(self, raw_frame: bytearray) -> int: - """ - Performs simple checksum of the Frame to confirm its validity. - - Parameters: - ---------- - raw_frame : Frame - Frame to calculate checksum for - - Returns: - ------- - Checksum value - """ - newsum = raw_frame[0] + raw_frame[1] - - for idx in range(3, len(raw_frame)): - newsum += raw_frame[idx] - - newsum = newsum % 255 - - return newsum - - def __send(self, channel_number: int, payload: bytearray) -> bool: - """ Send a frame with the selected bytes. - - Parameters: - ---------- - channel_number : int - Channel to send frame to. - payload : bytearray - Payload to send - - Returns: - -------- - If payload succesfully sent, returns True. Otherwise, False. - """ - - frame_sent = False - channel_dlc = self.__get_tx_channel_dlc(channel_number) - - if (len(payload) == channel_dlc) and \ - ((self.__sync_data.is_synced is True) or - (SerialMuxProtConstants.CONTROL_CHANNEL_NUMBER == channel_number)): - written_bytes = 0 - new_frame = Frame() - new_frame.channel = channel_number - new_frame.dlc = channel_dlc - new_frame.payload = payload - new_frame.pack_frame() # Pack Header and payload in Frame - new_frame.checksum = self.__checksum(new_frame.raw) - new_frame.pack_frame() # Pack Checksum in Frame - - try: - written_bytes = self.__stream.write(new_frame.raw) - except Exception as excpt: # pylint: disable=broad-exception-caught - print(excpt) - - if written_bytes == (SerialMuxProtConstants.HEADER_LEN + channel_dlc): - frame_sent = True - - return frame_sent - - def __cmd_sync(self, payload: bytearray) -> None: - """ Control Channel Command: SYNC - - Parameters: - ----------- - payload : bytearray - Command Data of received frame - """ - response = bytearray( - SerialMuxProtConstants.CONTROL_CHANNEL_PAYLOAD_LENGTH) - response[SerialMuxProtConstants.CONTROL_CHANNEL_COMMAND_INDEX] = SerialMuxProtConstants.Commands.SYNC_RSP - response[SerialMuxProtConstants.CONTROL_CHANNEL_PAYLOAD_INDEX:] = payload - - self.__send(SerialMuxProtConstants.CONTROL_CHANNEL_NUMBER, response) - - def __cmd_sync_rsp(self, payload: bytearray) -> None: - """ Control Channel Command: SYNC_RSP - - Parameters: - ----------- - payload : bytearray - Command Data of received frame - """ - - # Parse big-endian uint32 - unpacker = Struct(">L") - rcvd_timestamp = unpacker.unpack_from(payload)[0] - - if self.__sync_data.last_sync_command == rcvd_timestamp: - self.__sync_data.last_sync_response = self.__sync_data.last_sync_command - self.__sync_data.is_synced = True - # Process pending Subscriptions - self.__process_subscriptions() - - else: - self.__sync_data.is_synced = False - - def __cmd_scrb(self, payload: bytearray) -> None: - """ Control Channel Command: SCRB - - Parameters: - ----------- - payload : bytearray - Command Data of received frame - """ - - response = bytearray( - SerialMuxProtConstants.CONTROL_CHANNEL_PAYLOAD_LENGTH) - response[SerialMuxProtConstants.CONTROL_CHANNEL_COMMAND_INDEX] = SerialMuxProtConstants.Commands.SCRB_RSP - - # Parse name - channel_name = str(payload[5:], "ascii").strip('\x00') - response[5] = self.get_tx_channel_number(channel_name) - # Name is always sent back. - response[6:] = bytearray(channel_name, 'ascii') - - # Pad array if necessary - response = response.ljust( - SerialMuxProtConstants.CONTROL_CHANNEL_PAYLOAD_LENGTH, b'\x00') - if self.__send(SerialMuxProtConstants.CONTROL_CHANNEL_NUMBER, response) is False: - # Fall out of sync if failed to send. - self.__sync_data.is_synced = False - - def __cmd_scrb_rsp(self, payload: bytearray) -> None: - """ Control Channel Command: SCRB - - Parameters: - ----------- - payload : bytearray - Command Data of received frame - """ - - # Parse payload - channel_number = payload[4] - channel_name = str(payload[5:], "ascii").strip('\x00') - - if (self.__max_configured_channels >= channel_number) and \ - (0 < self.__channels.number_of_pending_channels): - - # Channel Iterator - for potential_channel in self.__channels.pending_channels: - # Check if a SCRB is pending and is the correct channel - if (potential_channel.callback is not None) and\ - (potential_channel.name == channel_name): - # Channel is found in the server - if 0 != channel_number: - channel_array_index = channel_number - 1 - - if self.__channels.rx_channels[channel_array_index].callback is None: - self.__channels.number_of_rx_channels += 1 - - self.__channels.rx_channels[channel_array_index] = Channel( - channel_name, 0, potential_channel.callback) - - # Channel is no longer pending - potential_channel = Channel() - - # Decrease Channel Counter - self.__channels.number_of_pending_channels -= 1 - - # Break out of iterator - break - - def __callback_control_channel(self, payload: bytearray) -> None: - """ Callback for the Control Channel - - Parameters: - ----------- - payload : bytearray - Payload of received frame - """ - if len(payload) != SerialMuxProtConstants.CONTROL_CHANNEL_PAYLOAD_LENGTH: - return - - cmd_byte = payload[SerialMuxProtConstants.CONTROL_CHANNEL_COMMAND_INDEX] - cmd_data = payload[SerialMuxProtConstants.CONTROL_CHANNEL_PAYLOAD_INDEX:] - - # Switch not available until Python 3.10 - # Find command handler - if SerialMuxProtConstants.Commands.SYNC == cmd_byte: - self.__cmd_sync(cmd_data) - elif SerialMuxProtConstants.Commands.SYNC_RSP == cmd_byte: - self.__cmd_sync_rsp(cmd_data) - elif SerialMuxProtConstants.Commands.SCRB == cmd_byte: - self.__cmd_scrb(cmd_data) - elif SerialMuxProtConstants.Commands.SCRB_RSP == cmd_byte: - self.__cmd_scrb_rsp(cmd_data) - -################################################################################ -# Functions -################################################################################ - -################################################################################ -# Main -################################################################################ From ac350e00d4eac3fb09d6fcfd37c9e43038c4bd5c Mon Sep 17 00:00:00 2001 From: Akram Date: Fri, 19 Jul 2024 13:36:08 +0200 Subject: [PATCH 20/58] Added RL-LineFollower World --- webots/worlds/RL_LineFollower.wbt | 46 +++++++++++++++++++++++++++++++ 1 file changed, 46 insertions(+) create mode 100644 webots/worlds/RL_LineFollower.wbt diff --git a/webots/worlds/RL_LineFollower.wbt b/webots/worlds/RL_LineFollower.wbt new file mode 100644 index 00000000..e3dfdac2 --- /dev/null +++ b/webots/worlds/RL_LineFollower.wbt @@ -0,0 +1,46 @@ +#VRML_SIM R2023b utf8 + +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/backgrounds/protos/TexturedBackground.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto" +EXTERNPROTO "../protos/Zumo32U4.proto" +EXTERNPROTO "../protos/LineFollowerTrack.proto" +IMPORTABLE EXTERNPROTO "../protos/Supervisor.proto" + +WorldInfo { + info [ + "Example uses of Track nodes, as caterpillar tracks on a robot, or as a conveyor belt." + ] + title "Track" + basicTimeStep 8 + contactProperties [ + ContactProperties { + material1 "rubber" + material2 "cardboard" + coulombFriction [ + 0.65 + ] + } + ] +} +Viewpoint { + orientation 0.3787583266277594 0.09714544028170935 -0.9203830145339561 2.677946076660944 + position 1.2569587324159737 0.7945022889765716 1.5111964909293643 +} +TexturedBackground { +} +TexturedBackgroundLight { +} +LineFollowerTrack { + contactMaterial "cardboard" +} +DEF ROBOT Zumo32U4 { + translation -0.24713614078815466 -0.04863962992854465 0.013994298332013683 + rotation -1.0564747468923541e-06 8.746699709178704e-07 0.9999999999990595 1.5880805820884731 + name "Zumo" + contactMaterial "rubber" +} +Supervisor { + name "RL_Supervisor" + controller "RL_Supervisor" + supervisor TRUE +} From e5ec72a155da69d6d9a3338f5862821f30406ea2 Mon Sep 17 00:00:00 2001 From: Akram Date: Tue, 23 Jul 2024 11:52:26 +0200 Subject: [PATCH 21/58] Create a separate supervisor for APP Reinforcement Learning, the original supervisor remains unchanged. --- .../RL_Supervisor/RL_Supervisor.py | 144 ++++++++ .../Serial_webots.py | 228 ++++++------- webots/controllers/Supervisor/Supervisor.py | 321 +++++++++++------- 3 files changed, 460 insertions(+), 233 deletions(-) create mode 100644 webots/controllers/RL_Supervisor/RL_Supervisor.py rename webots/controllers/{Supervisor => RL_Supervisor}/Serial_webots.py (96%) diff --git a/webots/controllers/RL_Supervisor/RL_Supervisor.py b/webots/controllers/RL_Supervisor/RL_Supervisor.py new file mode 100644 index 00000000..fc331140 --- /dev/null +++ b/webots/controllers/RL_Supervisor/RL_Supervisor.py @@ -0,0 +1,144 @@ +"""Supervisor controller. + Details: https://github.com/cyberbotics/webots/blob/master/docs/guide/supervisor-programming.md +""" +import sys +from Serial_webots import SerialWebots +from SerialMuxProt import SerialMuxProt +from controller import Supervisor +import struct + +# The PROTO DEF name must be given! +ROBOT_NAME = "ROBOT" + +# The supervisor receiver name. +SUPERVISOR_RX_NAME = "serialComRx" + +# The supervisor Emitter name. +SUPERVISOR_TX_NAME = "serialComTx" + +# Create the Supervisor instance. +supervisor = Supervisor() + +# Get the time step of the current world. +timestep = int(supervisor.getBasicTimeStep()) + +# Get the Supervisor Receiver Device +supervisor_com_rx = supervisor.getDevice(SUPERVISOR_RX_NAME) + +# Get the Supervisor Emitter device +supervisor_com_Tx = supervisor.getDevice(SUPERVISOR_TX_NAME) + +# Set serial rx/tx channels for communication with the Roboter. +supervisor_com_rx.setChannel(2) +supervisor_com_Tx.setChannel(1) + +# Enable supervisor receiver to receive any message from the robot or other +# devices in the simulation. +if supervisor_com_rx is None: + print(f"No supervisor communication possible, because {SUPERVISOR_RX_NAME} not found.") +else: + supervisor_com_rx.enable(timestep) + +# Get robot node which to observe. +robot_node = supervisor.getFromDef(ROBOT_NAME) + +# SerialMuxProt Server Instance. +S_client = SerialWebots(supervisor_com_Tx, supervisor_com_rx) +smp_server = SerialMuxProt(10, S_client) + +#DLC of SPEED_SET Channel +SPEED_SET_DLC = 4 +#DLC of STATUS Channel +STATUS_DLC = 1 + +# Counter for the number of times no line has been detected +noLineDetectionCount = 0 + +# The line sensor threshold. +Line_Sensor_On_Track_Min_Value = 200 + +# The counter of how much it has been reset. +Reset_Count = 0 + +def reinitialize (): + """ Re-initialization of position and orientation of The ROBOT """ + global robot_node,Reset_Count + trans_field = robot_node.getField("translation") + rot_field = robot_node.getField("rotation") + intial_position =[-0.24713614078815466, -0.04863962992854465, 0.013994298332013683] + initial_orientation =[-1.0564747468923541e-06, 8.746699709178704e-07, 0.9999999999990595, 1.5880805820884731] + trans_field.setSFVec3f(intial_position) + rot_field.setSFRotation(initial_orientation) + Reset_Count = 0 + +def callback_Status(payload: bytearray) -> int: + """ Callback Status Channel """ + if payload[0] == 1: + print("the max. time within the robot must be finished its drive is Done") + smp_server.send_data("SPEED_SET", struct.pack('2H', 0, 0)) # Stop motors + smp_server.send_data("STATUS",struct.pack('B', 1)) + reinitialize() + + +def callback_LineSensors(payload: bytearray)-> None: + """ Callback LINE_SENS Channel""" + global noLineDetectionCount, Line_Sensor_On_Track_Min_Value, Reset_Count + sensor_data = struct.unpack('5H', payload) + for idx in range (5): + if idx == 4: + print(f"Sensor[{idx}] = {sensor_data[idx]}") + else : + print(f"Sensor[{idx}] = {sensor_data[idx]}, ", end="") + + if all(value == 0 for value in sensor_data): + noLineDetectionCount += 1 + else: + noLineDetectionCount = 0 + + if noLineDetectionCount >= 20 or all(value >= Line_Sensor_On_Track_Min_Value for value in sensor_data): + smp_server.send_data("SPEED_SET", struct.pack('2H', 0, 0)) # Stop motors, maximum NO Line Detection Counter reached + smp_server.send_data("STATUS",struct.pack('B', 1)) # SEND STATUS DONE + Reset_Count += 1 + else: + smp_server.send_data("SPEED_SET", struct.pack('2H', 1000, 1000)) + +def callback_Mode(payload: bytearray)-> None: + """ Callback MODE Channel""" + driving_mode = payload[0] + if driving_mode: + print("Driving Mode Selected") + else: + print("Train Mode Selected") + + +m_serialMuxProtChannelIdSPEED_SET = smp_server.create_channel("SPEED_SET", SPEED_SET_DLC) +m_serialMuxProtChannelIdSTATUS = smp_server.create_channel("STATUS", STATUS_DLC) +smp_server.subscribe_to_channel("STATUS", callback_Status) +smp_server.subscribe_to_channel("LINE_SENS",callback_LineSensors) +smp_server.subscribe_to_channel("MODE",callback_Mode) + +def main_loop(): + """Main loop: + - Perform simulation steps until Webots is stopping the controller- + + Returns: + number: Status + """ + status = 0 + m_elapsedTimeSinceReset = 0 + + if m_serialMuxProtChannelIdSPEED_SET == 0 or m_serialMuxProtChannelIdSTATUS == 0 : + print("Channel SPEED_SET not created.") + else: + if robot_node is None: + print(f"Robot DEF {ROBOT_NAME} not found.") + status = -1 + else: + while supervisor.step(timestep) != -1: + m_elapsedTimeSinceReset += timestep + smp_server.process(m_elapsedTimeSinceReset) + if Reset_Count != 0: + reinitialize() + return status + +sys.exit(main_loop()) \ No newline at end of file diff --git a/webots/controllers/Supervisor/Serial_webots.py b/webots/controllers/RL_Supervisor/Serial_webots.py similarity index 96% rename from webots/controllers/Supervisor/Serial_webots.py rename to webots/controllers/RL_Supervisor/Serial_webots.py index ffedddea..0863f06e 100644 --- a/webots/controllers/Supervisor/Serial_webots.py +++ b/webots/controllers/RL_Supervisor/Serial_webots.py @@ -1,114 +1,114 @@ -################################################################################ -# Imports -################################################################################ -from controller import device -################################################################################ -# Variables -################################################################################ - -################################################################################ -# Classes -################################################################################ - - -class SerialWebots: - """ - Serial Webots Communication Class - """ - - def __init__(self, Emitter: device, Receiver: device)-> None: - """ SerialWebots Constructor. - - Parameters - ---------- - Emitter : device - Name of Emitter Device - Receiver : device - Name of Receiver Device - - """ - self.m_Emitter = Emitter - self.m_Receiver = Receiver - self.buffer = bytearray() - - - def write(self, payload : bytearray ) -> int: - """ Sends Data to the Server. - - Parameters - ---------- - payload : bytearray - Payload to send. - - Returns - ---------- - Number of bytes sent - """ - self.m_Emitter.send(bytes(payload)) - bytes_sent= len(payload) - - return bytes_sent - - def available(self) -> int: - """ Check if there is anything available for reading - - Returns - ---------- - Number of bytes that are available for reading. - - """ - if len(self.buffer) > 0: - return len(self.buffer) - elif self.m_Receiver.getQueueLength() > 0: - return self.m_Receiver.getDataSize() - return 0 - - def read_bytes(self, length: int) -> tuple[int, bytearray]: - """ Read a given number of Bytes from Serial. - - Returns - ---------- - Tuple: - - int: Number of bytes received. - - bytearray: Received data. - """ - - read = 0 - data = bytearray() - - if len(self.buffer) > 0: - read = min(len(self.buffer), length) - data = self.buffer[:read] - self.buffer = self.buffer[read:] - elif self.m_Receiver.getQueueLength() > 0: - receivedData = self.m_Receiver.getBytes() - receivedData_size = self.m_Receiver.getDataSize() - self.m_Receiver.nextPacket() - - if receivedData_size > length: - data = receivedData[:length] - self.buffer = receivedData[length:] - read = length - else: - data = receivedData - read = receivedData_size - - return read, data - - - - - - - - - - - -################################################################################ -# Functions -################################################################################ - -################################################################################ -# Main -################################################################################ +################################################################################ +# Imports +################################################################################ +from controller import device +################################################################################ +# Variables +################################################################################ + +################################################################################ +# Classes +################################################################################ + + +class SerialWebots: + """ + Serial Webots Communication Class + """ + + def __init__(self, Emitter: device, Receiver: device)-> None: + """ SerialWebots Constructor. + + Parameters + ---------- + Emitter : device + Name of Emitter Device + Receiver : device + Name of Receiver Device + + """ + self.m_Emitter = Emitter + self.m_Receiver = Receiver + self.buffer = bytearray() + + + def write(self, payload : bytearray ) -> int: + """ Sends Data to the Server. + + Parameters + ---------- + payload : bytearray + Payload to send. + + Returns + ---------- + Number of bytes sent + """ + self.m_Emitter.send(bytes(payload)) + bytes_sent= len(payload) + + return bytes_sent + + def available(self) -> int: + """ Check if there is anything available for reading + + Returns + ---------- + Number of bytes that are available for reading. + + """ + if len(self.buffer) > 0: + return len(self.buffer) + elif self.m_Receiver.getQueueLength() > 0: + return self.m_Receiver.getDataSize() + return 0 + + def read_bytes(self, length: int) -> tuple[int, bytearray]: + """ Read a given number of Bytes from Serial. + + Returns + ---------- + Tuple: + - int: Number of bytes received. + - bytearray: Received data. + """ + + read = 0 + data = bytearray() + + if len(self.buffer) > 0: + read = min(len(self.buffer), length) + data = self.buffer[:read] + self.buffer = self.buffer[read:] + elif self.m_Receiver.getQueueLength() > 0: + receivedData = self.m_Receiver.getBytes() + receivedData_size = self.m_Receiver.getDataSize() + self.m_Receiver.nextPacket() + + if receivedData_size > length: + data = receivedData[:length] + self.buffer = receivedData[length:] + read = length + else: + data = receivedData + read = receivedData_size + + return read, data + + + + + + + + + + + +################################################################################ +# Functions +################################################################################ + +################################################################################ +# Main +################################################################################ diff --git a/webots/controllers/Supervisor/Supervisor.py b/webots/controllers/Supervisor/Supervisor.py index 4e1bf0ff..be0bc9e5 100644 --- a/webots/controllers/Supervisor/Supervisor.py +++ b/webots/controllers/Supervisor/Supervisor.py @@ -1,11 +1,11 @@ """Supervisor controller. Details: https://github.com/cyberbotics/webots/blob/master/docs/guide/supervisor-programming.md """ + +import math import sys -from Serial_webots import SerialWebots -from SerialMuxProt import SerialMuxProt from controller import Supervisor -import struct +from robot_observer import RobotObserver # The PROTO DEF name must be given! ROBOT_NAME = "ROBOT" @@ -13,110 +13,123 @@ # The supervisor receiver name. SUPERVISOR_RX_NAME = "serialComRx" -# The supervisor Emitter name. -SUPERVISOR_TX_NAME = "serialComTx" - -# Create the Supervisor instance. -supervisor = Supervisor() - -# Get the time step of the current world. -timestep = int(supervisor.getBasicTimeStep()) - -# Get the Supervisor Receiver Device -supervisor_com_rx = supervisor.getDevice(SUPERVISOR_RX_NAME) - -# Get the Supervisor Emitter device -supervisor_com_Tx = supervisor.getDevice(SUPERVISOR_TX_NAME) - -# Set serial rx/tx channels for communication with the Roboter. -supervisor_com_rx.setChannel(2) -supervisor_com_Tx.setChannel(1) - -# Enable supervisor receiver to receive any message from the robot or other -# devices in the simulation. -if supervisor_com_rx is None: - print(f"No supervisor communication possible, because {SUPERVISOR_RX_NAME} not found.") -else: - supervisor_com_rx.enable(timestep) - -# Get robot node which to observe. -robot_node = supervisor.getFromDef(ROBOT_NAME) - -# SerialMuxProt Server Instance. -S_client = SerialWebots(supervisor_com_Tx, supervisor_com_rx) -smp_server = SerialMuxProt(10, S_client) - -#DLC of SPEED_SET Channel -SPEED_SET_DLC = 4 -#DLC of STATUS Channel -STATUS_DLC = 1 - -# Counter for the number of times no line has been detected -noLineDetectionCount = 0 - -# The line sensor threshold. -Line_Sensor_On_Track_Min_Value = 200 - -# The counter of how much it has been reset. -Reset_Count = 0 - -def reinitialize (): - """ Re-initialization of position and orientation of The ROBOT """ - global robot_node,Reset_Count - trans_field = robot_node.getField("translation") - rot_field = robot_node.getField("rotation") - intial_position =[-0.24713614078815466, -0.04863962992854465, 0.013994298332013683] - initial_orientation =[-1.0564747468923541e-06, 8.746699709178704e-07, 0.9999999999990595, 1.5880805820884731] - trans_field.setSFVec3f(intial_position) - rot_field.setSFRotation(initial_orientation) - Reset_Count = 0 - -def callback_Status(payload: bytearray) -> int: - """ Callback Status Channel """ - if payload[0] == 1: - print("the max. time within the robot must be finished its drive is Done") - smp_server.send_data("SPEED_SET", struct.pack('2H', 0, 0)) # Stop motors - smp_server.send_data("STATUS",struct.pack('B', 1)) - reinitialize() - - -def callback_LineSensors(payload: bytearray)-> None: - """ Callback LINE_SENS Channel""" - global noLineDetectionCount, Line_Sensor_On_Track_Min_Value, Reset_Count - sensor_data = struct.unpack('5H', payload) - for idx in range (5): - if idx == 4: - print(f"Sensor[{idx}] = {sensor_data[idx]}") - else : - print(f"Sensor[{idx}] = {sensor_data[idx]}, ", end="") - - if all(value == 0 for value in sensor_data): - noLineDetectionCount += 1 - else: - noLineDetectionCount = 0 - - if noLineDetectionCount >= 20 or all(value >= Line_Sensor_On_Track_Min_Value for value in sensor_data): - smp_server.send_data("SPEED_SET", struct.pack('2H', 0, 0)) # Stop motors, maximum NO Line Detection Counter reached - smp_server.send_data("STATUS",struct.pack('B', 1)) # SEND STATUS DONE - Reset_Count += 1 - else: - smp_server.send_data("SPEED_SET", struct.pack('2H', 1000, 1000)) - -def callback_Mode(payload: bytearray)-> None: - """ Callback MODE Channel""" - driving_mode = payload[0] - if driving_mode: - print("Driving Mode Selected") - else: - print("Train Mode Selected") +def serial_com_read(serial_com_rx_device): + """Read from serial communication device. + + Args: + serial_com_rx_device (obj): Serial COM device + + Returns: + Union[str, None]: Received data or None. + """ + rx_data = None + + if serial_com_rx_device.getQueueLength() > 0: + rx_data = serial_com_rx_device.getString() + serial_com_rx_device.nextPacket() + + return rx_data + +def rad_to_deg(angle_rad): + """Convert angle in rad to degree. + + Args: + angle_rad (float): Angle in rad + + Returns: + float: Angle in degree + """ + return angle_rad * 180 / math.pi + +def convert_angle_to_2pi(angle): + """Convert angle from range [-PI; PI] in rad to [0; 2PI]. + + Args: + angle (float): Angle in rad, range [-PI; PI]. + + Returns: + float: Angle in rad, range [0; 2PI] + """ + if angle < 0: + angle += 2 * math.pi + + return angle + +def convert_webots_angle_to_ru_angle(webots_angle): + """Convert Webots angle to RadonUlzer angle. + + Args: + webots_angle (float): Webots angle in rad, range [-PI; PI]. + + Returns: + float: Angle in rad, range [-2PI; 2PI] + """ + # Radon Ulzer + # Angle [-2PI; 2PI] + # North are 90° (PI/2) + # + # Webots angle [-PI; PI] + webots_angle += math.pi / 2 + + webots_angle_2pi = convert_angle_to_2pi(webots_angle) + + # TODO Handling the negative range. + + return webots_angle_2pi + +def has_position_changed(position, position_old): + """Returns whether the position changed. + + Args: + position (list[float]): Position 1 + position_old (list[float]): Position 2 + + Returns: + bool: If changed, it will return True otherwise False. + """ + has_changed = False + + for idx, value in enumerate(position): + if int(value) != int(position_old[idx]): + has_changed = True + break + return has_changed + +def has_orientation_changed(orientation, orientation_old): + """Returns whether the orientation changed. + + Args: + orientation (list[float]): Orientation 1 + orientation_old (list[float]): Orientation 2 + + Returns: + bool: If changed, it will return True otherwise False. + """ + has_changed = False + + for idx, value in enumerate(orientation): + if int(rad_to_deg(value)) != int(rad_to_deg(orientation_old[idx])): + has_changed = True + break + + return has_changed + +class OdometryData(): # pylint: disable=too-few-public-methods + """Odometry data container. + """ + def __init__(self) -> None: + self.x = 0 + self.y = 0 + self.yaw_angle = 0 + + def reset(self): + """Reset odometry data. + """ + self.x = 0 + self.y = 0 + self.yaw_angle = 0 -m_serialMuxProtChannelIdSPEED_SET = smp_server.create_channel("SPEED_SET", SPEED_SET_DLC) -m_serialMuxProtChannelIdSTATUS = smp_server.create_channel("STATUS", STATUS_DLC) -smp_server.subscribe_to_channel("STATUS", callback_Status) -smp_server.subscribe_to_channel("LINE_SENS",callback_LineSensors) -smp_server.subscribe_to_channel("MODE",callback_Mode) - def main_loop(): """Main loop: - Perform simulation steps until Webots is stopping the controller- @@ -125,21 +138,91 @@ def main_loop(): number: Status """ status = 0 - m_elapsedTimeSinceReset = 0 - if m_serialMuxProtChannelIdSPEED_SET == 0 or m_serialMuxProtChannelIdSTATUS == 0 : - print("Channel SPEED_SET not created.") + # Create the Supervisor instance. + supervisor = Supervisor() + + # Get the time step of the current world. + timestep = int(supervisor.getBasicTimeStep()) + + # Enable supervisor receiver to receive any message from the robot or other + # devices in the simulation. + supervisor_com_rx = supervisor.getDevice(SUPERVISOR_RX_NAME) + + if supervisor_com_rx is None: + print(f"No supervisor communication possible, because {SUPERVISOR_RX_NAME} not found.") else: - if robot_node is None: - print(f"Robot DEF {ROBOT_NAME} not found.") - status = -1 - else: - while supervisor.step(timestep) != -1: - m_elapsedTimeSinceReset += timestep - smp_server.process(m_elapsedTimeSinceReset) - if Reset_Count != 0: - reinitialize() - return status + supervisor_com_rx.enable(timestep) + + # Get robot node which to observe. + robot_node = supervisor.getFromDef(ROBOT_NAME) + + if robot_node is None: + + print(f"Robot DEF {ROBOT_NAME} not found.") + status = -1 + + else: + robot_observer = RobotObserver(robot_node) + + # Set current position and orientation in the map as reference. + robot_observer.set_current_position_as_reference() + robot_observer.set_current_orientation_as_reference() + + robot_position_old = robot_observer.get_rel_position() + robot_orientation_old = robot_observer.get_rel_orientation() + robot_odometry = OdometryData() + + while supervisor.step(timestep) != -1: + if supervisor_com_rx is not None: + rx_data = serial_com_read(supervisor_com_rx) + + if rx_data is None: + pass + + else: + command = rx_data.split(',') + + # Reset robot position and orientation? + if command[0] == "RST": + print("RST") + robot_observer.set_current_position_as_reference() + robot_observer.set_current_orientation_as_reference() + robot_odometry.reset() + + # Robot odometry data received? + elif command[0] == "ODO": + robot_odometry.x = int(command[1]) # [mm] + robot_odometry.y = int(command[2]) # [mm] + robot_odometry.yaw_angle = float(command[3]) / 1000.0 # [rad] + + # Unknown command. + else: + print(f"Unknown command: {command[0]}") -sys.exit(main_loop()) + robot_position = robot_observer.get_rel_position() + robot_orientation = robot_observer.get_rel_orientation() + + any_change = has_position_changed(robot_position, robot_position_old) + + if any_change is False: + any_change = has_orientation_changed(robot_orientation, robot_orientation_old) + + if any_change is True: + + robot_yaw_angle = robot_orientation[2] + yaw_ru_angle = convert_webots_angle_to_ru_angle(robot_yaw_angle) + + print(f"{int(robot_position[0])}, ", end="") + print(f"{int(robot_position[1])}, ", end="") + print(f"{int(rad_to_deg(yaw_ru_angle))} / ", end="") + print(f"{robot_odometry.x}, ", end="") + print(f"{robot_odometry.y}, ", end="") + print(f"{int(rad_to_deg(robot_odometry.yaw_angle))}") + + robot_position_old = robot_position + robot_orientation_old = robot_orientation + + return status +sys.exit(main_loop()) \ No newline at end of file From 86f9b2c9ab982bb8fd95017900ee410ca72a9c4c Mon Sep 17 00:00:00 2001 From: Akram Date: Tue, 23 Jul 2024 12:02:19 +0200 Subject: [PATCH 22/58] Remove wrong line from the code --- platformio.ini | 1588 ++++++++++++++++++++++++------------------------ 1 file changed, 793 insertions(+), 795 deletions(-) diff --git a/platformio.ini b/platformio.ini index 6b707fb9..dc0c0a4c 100644 --- a/platformio.ini +++ b/platformio.ini @@ -1,795 +1,793 @@ - -======= -;PlatformIO Project Configuration File -; -; Build options: build flags, source filter -; Upload options: custom upload port, speed and extra flags -; Library options: dependencies, extra library storages -; Advanced options: extra scripting -; -; Please visit documentation for the other options and examples -; https://docs.platformio.org/page/projectconf.html - -; ***************************************************************************** -; PlatformIO specific configurations -; ***************************************************************************** -[platformio] -; Define default environments, which shall be automatically be built. -;default_envs = CalibTarget -;default_envs = CalibSim -;default_envs = ConvoyLeaderTarget -;default_envs = ConvoyLeaderSim -;default_envs = ConvoyFollowerSim -;default_envs = ConvoyFollowerTarget -;default_envs = LineFollowerTarget -;default_envs = LineFollowerSim -;default_envs = LineFollowerSimpleSim -;default_envs = RemoteControlTarget -;default_envs = RemoteControlSim -default_envs = ReinforcementLearningSim -;default_envs = SensorFusionTarget -;default_envs = SensorFusionSim -;default_envs = TestSim - -; ***************************************************************************** -; Common configurations, which is independed of the environment. -; ***************************************************************************** -[common] -build_flags = - -DTEAM_NAME_LINE_1="\"Radon\"" - -DTEAM_NAME_LINE_2="\"Ulzer\"" - -; ***************************************************************************** -; Static check configuration -; ***************************************************************************** -[static_check_configuration] -check_tool = cppcheck, clangtidy -check_severity = medium, high -check_patterns = - include - src - lib -check_flags = - cppcheck: cppcheck: --suppress=*:*/libdeps/* --suppress=*:*lib/Webots/* --suppress=noExplicitConstructor --suppress=unusedFunction --std=c++11 - clangtidy: --header-filter='' --checks=-*,clang-analyzer-*,performance-*,portability-*,readability-uppercase-literal-suffix,readability-redundant-control-flow --warnings-as-errors=-*,clang-analyzer-*,performance-*,portability-*,readability-uppercase-literal-suffix,readability-redundant-control-flow -check_skip_packages = yes - -; ***************************************************************************** -; Target environment for Zumo32U4. -; ***************************************************************************** -[hal:Target] -platform = atmelavr @ ~4.1.0 -board = a-star32U4 -framework = arduino -build_flags = - ${common.build_flags} - -Wno-switch - -Werror -lib_deps = - BlueAndi/ZumoHALATmega32u4 @ ~1.0.0 -lib_ignore = - HALTest -extra_scripts = - -monitor_speed = 115200 - -; The monitor port shows the debug output and the test output. -; If you connect the robot to your pc, this is the port you will see. -monitor_port = com11 - -; The upload port is spawned by the bootmonitor and used to update the program on the target. -; If you connect the robot to your pc, press twice reset button to jump to the bootloader, -; this is the port you will see. -upload_port = com5 - -; The test port is spawned by the bootmonitor and used to update the test on the target. -; If you connect the robot to your pc, press twice reset button to jump to the bootloader, -; this is the port you will see. -test_port = com9 - -; ***************************************************************************** -; PC target environment for Webots simulation. -; -; It is assumed that the environment variable WEBOTS_HOME is set to the -; Webots directory, e.g. WEBOTS_HOME=C:\Users\\AppData\Local\Programs\Webots -; ***************************************************************************** -[hal:Sim] -platform = native @ ~1.2.1 -build_flags = - -std=c++11 - -DTARGET_NATIVE - -I./lib/Webots/include/c - -I./lib/Webots/include/cpp -lib_deps = - MainNative - BlueAndi/ZumoHALWebots @ ~1.0.0 - Webots -lib_ignore = - HALTest -extra_scripts = - ./scripts/webots_launcher.py - pre:$PROJECT_LIBDEPS_DIR/$PIOENV/ZumoHALWebots/scripts/create_webots_library.py - pre:$PROJECT_LIBDEPS_DIR/$PIOENV/ZumoHALWebots/scripts/copy_sounds.py - post:$PROJECT_LIBDEPS_DIR/$PIOENV/ZumoHALWebots/scripts/copy_webots_shared_libs.py - post:./scripts/package.py -webots_robot_name = Zumo -webots_robot_serial_rx_channel = 1 -webots_robot_serial_tx_channel = 2 - -; ***************************************************************************** -; PC target environment for tests -; ***************************************************************************** -[hal:Test] -platform = native @ ~1.2.1 -build_flags = - -std=c++11 - -DTARGET_NATIVE - -DUNIT_TEST - -Itest/common -lib_deps = - MainTestNative - BlueAndi/ArduinoNative @ ~0.1.1 - BlueAndi/ZumoHALInterfaces @ ~1.0.0 - HALTest -lib_ignore = - Webots -extra_scripts = - -; ***************************************************************************** -; Calibration application -; ***************************************************************************** -[app:Calib] -build_flags = - ${common.build_flags} - -DLOG_DEBUG_ENABLE -lib_deps = - APPCalib - Service -lib_ignore = - APPConvoyLeader - APPLineFollower - APPReinforcementLearning - APPRemoteControl - APPSensorFusion - APPTest - -; ***************************************************************************** -; Calibration application specific HAL for target -; ***************************************************************************** -[hal_app:CalibTarget] -extends = hal:Target -build_flags = - ${hal:Target.build_flags} -lib_deps = - ${hal:Target.lib_deps} - HALCalibTarget -lib_ignore = - ${hal:Target.lib_ignore} -extra_scripts = - ${hal:Target.extra_scripts} - -; ***************************************************************************** -; Calibration application specific HAL for simulation -; ***************************************************************************** -[hal_app:CalibSim] -extends = hal:Sim -build_flags = - ${hal:Sim.build_flags} -lib_deps = - ${hal:Sim.lib_deps} - HALCalibSim -lib_ignore = - ${hal:Sim.lib_ignore} -extra_scripts = - ${hal:Sim.extra_scripts} - -; ***************************************************************************** -; Convoy leader application -; ***************************************************************************** -[app:ConvoyLeader] -build_flags = - ${common.build_flags} - -DLOG_DEBUG_ENABLE -lib_deps = - APPConvoyLeader - Service - gabryelreyes/SerialMuxProt @ ~2.2.0 -lib_ignore = - APPCalib - APPConvoyFollower - APPLineFollower - APPRemoteControl - APPReinforcementLearning - APPSensorFusion - APPTest - -; ***************************************************************************** -; Convoy leader application specific HAL for target -; ***************************************************************************** -[hal_app:ConvoyLeaderTarget] -extends = hal:Target -build_flags = - ${hal:Target.build_flags} -lib_deps = - ${hal:Target.lib_deps} - HALConvoyLeaderTarget -lib_ignore = - ${hal:Target.lib_ignore} -extra_scripts = - ${hal:Target.extra_scripts} - -; ***************************************************************************** -; Convoy leader application specific HAL for simulation -; ***************************************************************************** -[hal_app:ConvoyLeaderSim] -extends = hal:Sim -build_flags = - ${hal:Sim.build_flags} -lib_deps = - ${hal:Sim.lib_deps} - HALConvoyLeaderSim -lib_ignore = - ${hal:Sim.lib_ignore} -extra_scripts = - ${hal:Sim.extra_scripts} - -; ***************************************************************************** -; Convoy follower application -; ***************************************************************************** -[app:ConvoyFollower] -build_flags = - ${common.build_flags} - -DLOG_DEBUG_ENABLE -lib_deps = - APPConvoyFollower - Service - gabryelreyes/SerialMuxProt @ ~2.2.0 -lib_ignore = - APPCalib - APPConvoyLeader - APPLineFollower - APPRemoteControl - APPReinforcementLearning - APPSensorFusion - APPTest - -; ***************************************************************************** -; Convoy follower application specific HAL for target -; ***************************************************************************** -[hal_app:ConvoyFollowerTarget] -extends = hal:Target -build_flags = - ${hal:Target.build_flags} -lib_deps = - ${hal:Target.lib_deps} - HALConvoyFollowerTarget -lib_ignore = - ${hal:Target.lib_ignore} -extra_scripts = - ${hal:Target.extra_scripts} - -; ***************************************************************************** -; Convoy follower application specific HAL for simulation -; ***************************************************************************** -[hal_app:ConvoyFollowerSim] -extends = hal:Sim -build_flags = - ${hal:Sim.build_flags} -lib_deps = - ${hal:Sim.lib_deps} - HALConvoyFollowerSim -lib_ignore = - ${hal:Sim.lib_ignore} -extra_scripts = - ${hal:Sim.extra_scripts} - -; ***************************************************************************** -; Line follower application -; ***************************************************************************** -[app:LineFollower] -build_flags = - ${common.build_flags} - -DLOG_DEBUG_ENABLE -lib_deps = - APPLineFollower - Service -lib_ignore = - APPCalib - APPConvoyFollower - APPReinforcementLearning - APPConvoyLeader - APPRemoteControl - APPSensorFusion - APPTest - -; ***************************************************************************** -; Simple line follower application -; ***************************************************************************** -[app:LineFollowerSimple] -build_flags = - ${common.build_flags} - -DLOG_DEBUG_ENABLE -lib_deps = - APPLineFollowerSimple - Service -lib_ignore = - APPCalib - APPConvoyFollower - APPReinforcementLearning - APPConvoyLeader - APPLineFollower - APPRemoteControl - APPSensorFusion - APPTest - -; ***************************************************************************** -; Line follower application specific HAL for target -; ***************************************************************************** -[hal_app:LineFollowerTarget] -extends = hal:Target -build_flags = - ${hal:Target.build_flags} -lib_deps = - ${hal:Target.lib_deps} - HALLineFollowerTarget -lib_ignore = - ${hal:Target.lib_ignore} -extra_scripts = - ${hal:Target.extra_scripts} - -; ***************************************************************************** -; Line follower application specific HAL for simulation -; ***************************************************************************** -[hal_app:LineFollowerSim] -extends = hal:Sim -build_flags = - ${hal:Sim.build_flags} -lib_deps = - ${hal:Sim.lib_deps} - HALLineFollowerSim -lib_ignore = - ${hal:Sim.lib_ignore} -extra_scripts = - ${hal:Sim.extra_scripts} - -; ***************************************************************************** -; Reinforcement Learning for Line follower application -; ***************************************************************************** -[app:ReinforcementLearning] -build_flags = - ${common.build_flags} - -DLOG_DEBUG_ENABLE -lib_deps = - APPReinforcementLearning - Service - gabryelreyes/SerialMuxProt @ ~2.2.0 -lib_ignore = - APPCalib - APPConvoyLeader - APPRemoteControl - APPSensorFusion - APPLineFollower - APPTest - -; ***************************************************************************** -; Reinforcement Learning for Line follower application specific HAL for simulation -; ***************************************************************************** -[hal_app:ReinforcementLearningSim] -extends = hal:Sim -build_flags = - ${hal:Sim.build_flags} -lib_deps = - ${hal:Sim.lib_deps} - HALReinforcementLearningSim -lib_ignore = - ${hal:Sim.lib_ignore} -extra_scripts = - ${hal:Sim.extra_scripts} - -; ***************************************************************************** -; Remote control application -; ***************************************************************************** -[app:RemoteControl] -build_flags = - ${common.build_flags} - -DLOG_DEBUG_ENABLE -lib_deps = - APPRemoteControl - Service - gabryelreyes/SerialMuxProt @ ~2.2.0 -lib_ignore = - APPCalib - APPConvoyFollower - APPReinforcementLearning - APPConvoyLeader - APPLineFollower - APPSensorFusion - APPTest - -; ***************************************************************************** -; Remote control application specific HAL for target -; ***************************************************************************** -[hal_app:RemoteControlTarget] -extends = hal:Target -build_flags = - ${hal:Target.build_flags} -lib_deps = - ${hal:Target.lib_deps} - HALRemoteControlTarget -lib_ignore = - ${hal:Target.lib_ignore} -extra_scripts = - ${hal:Target.extra_scripts} - -; ***************************************************************************** -; Remote control application specific HAL for simulation -; ***************************************************************************** -[hal_app:RemoteControlSim] -extends = hal:Sim -build_flags = - ${hal:Sim.build_flags} -lib_deps = - ${hal:Sim.lib_deps} - HALRemoteControlSim -lib_ignore = - ${hal:Sim.lib_ignore} -extra_scripts = - ${hal:Sim.extra_scripts} - -; ***************************************************************************** -; Sensor Fusion application -; ***************************************************************************** -[app:SensorFusion] -build_flags = - ${common.build_flags} - -DLOG_DEBUG_ENABLE -lib_deps = - APPSensorFusion - Service - gabryelreyes/SerialMuxProt @ ~2.2.0 -lib_ignore = - APPCalib - APPConvoyFollower - APPConvoyLeader - APPLineFollower - APPReinforcementLearning - APPRemoteControl - APPTest - -; ***************************************************************************** -; Sensor Fusion application specific HAL for target -; ***************************************************************************** -[hal_app:SensorFusionTarget] -extends = hal:Target -build_flags = - ${hal:Target.build_flags} -lib_deps = - ${hal:Target.lib_deps} - HALSensorFusionTarget -lib_ignore = - ${hal:Target.lib_ignore} -extra_scripts = - ${hal:Target.extra_scripts} - -; ***************************************************************************** -; Sensor Fusion application specific HAL for simulation -; ***************************************************************************** -[hal_app:SensorFusionSim] -extends = hal:Sim -build_flags = - ${hal:Sim.build_flags} -lib_deps = - ${hal:Sim.lib_deps} - HALSensorFusionSim -lib_ignore = - ${hal:Sim.lib_ignore} -extra_scripts = - ${hal:Sim.extra_scripts} - -; ***************************************************************************** -; Test application -; ***************************************************************************** -[app:Test] -build_flags = - ${common.build_flags} -lib_deps = - APPTest - Service -lib_ignore = - APPCalib - APPConvoyFollower - APPConvoyLeader - APPLineFollower - APPReinforcementLearning - APPRemoteControl - APPSensorFusion - -; ***************************************************************************** -; Test application specific HAL for target -; ***************************************************************************** -[hal_app:TestSim] -extends = hal:Test -build_flags = - ${hal:Test.build_flags} -lib_deps = - ${hal:Test.lib_deps} - HALTestSim -lib_ignore = - ${hal:Test.lib_ignore} -extra_scripts = - ${hal:Test.extra_scripts} - -; ***************************************************************************** -; Calibration application on simulation -; ***************************************************************************** -[env:CalibSim] -extends = hal_app:CalibSim, app:Calib, static_check_configuration -build_flags = - ${hal_app:CalibSim.build_flags} - ${app:Calib.build_flags} -lib_deps = - ${hal_app:CalibSim.lib_deps} - ${app:Calib.lib_deps} -lib_ignore = - ${hal_app:CalibSim.lib_ignore} - ${app:Calib.lib_ignore} -extra_scripts = - ${hal_app:CalibSim.extra_scripts} - -; ***************************************************************************** -; Calibration application on target -; ***************************************************************************** -[env:CalibTarget] -extends = hal_app:CalibTarget, app:Calib, static_check_configuration -build_flags = - ${hal_app:CalibTarget.build_flags} - ${app:Calib.build_flags} -lib_deps = - ${hal_app:CalibTarget.lib_deps} - ${app:Calib.lib_deps} -lib_ignore = - ${hal_app:CalibTarget.lib_ignore} - ${app:Calib.lib_ignore} -extra_scripts = - ${hal_app:CalibTarget.extra_scripts} - -; ***************************************************************************** -; Convoy leader application on target -; ***************************************************************************** -[env:ConvoyLeaderTarget] -extends = hal_app:ConvoyLeaderTarget, app:ConvoyLeader, static_check_configuration -build_flags = - ${hal_app:ConvoyLeaderTarget.build_flags} - ${app:ConvoyLeader.build_flags} -lib_deps = - ${hal_app:ConvoyLeaderTarget.lib_deps} - ${app:ConvoyLeader.lib_deps} -lib_ignore = - ${hal_app:ConvoyLeaderTarget.lib_ignore} - ${app:ConvoyLeader.lib_ignore} -extra_scripts = - ${hal_app:ConvoyLeaderTarget.extra_scripts} - -; ***************************************************************************** -; Convoy leader application on simulation -; ***************************************************************************** -[env:ConvoyLeaderSim] -extends = hal_app:ConvoyLeaderSim, app:ConvoyLeader, static_check_configuration -build_flags = - ${hal_app:ConvoyLeaderSim.build_flags} - ${app:ConvoyLeader.build_flags} -lib_deps = - ${hal_app:ConvoyLeaderSim.lib_deps} - ${app:ConvoyLeader.lib_deps} -lib_ignore = - ${hal_app:ConvoyLeaderSim.lib_ignore} - ${app:ConvoyLeader.lib_ignore} -extra_scripts = - ${hal_app:ConvoyLeaderSim.extra_scripts} - -; ***************************************************************************** -; Convoy follower application on target -; ***************************************************************************** -[env:ConvoyFollowerTarget] -extends = hal_app:ConvoyFollowerTarget, app:ConvoyFollower, static_check_configuration -build_flags = - ${hal_app:ConvoyFollowerTarget.build_flags} - ${app:ConvoyFollower.build_flags} -lib_deps = - ${hal_app:ConvoyFollowerTarget.lib_deps} - ${app:ConvoyFollower.lib_deps} -lib_ignore = - ${hal_app:ConvoyFollowerTarget.lib_ignore} - ${app:ConvoyFollower.lib_ignore} -extra_scripts = - ${hal_app:ConvoyFollowerTarget.extra_scripts} - -; ***************************************************************************** -; Convoy follower application on simulation -; ***************************************************************************** -[env:ConvoyFollowerSim] -extends = hal_app:ConvoyFollowerSim, app:ConvoyFollower, static_check_configuration -build_flags = - ${hal_app:ConvoyFollowerSim.build_flags} - ${app:ConvoyFollower.build_flags} -lib_deps = - ${hal_app:ConvoyFollowerSim.lib_deps} - ${app:ConvoyFollower.lib_deps} -lib_ignore = - ${hal_app:ConvoyFollowerSim.lib_ignore} - ${app:ConvoyFollower.lib_ignore} -extra_scripts = - ${hal_app:ConvoyFollowerSim.extra_scripts} - -; ***************************************************************************** -; Line follower application on target -; ***************************************************************************** -[env:LineFollowerTarget] -extends = hal_app:LineFollowerTarget, app:LineFollower, static_check_configuration -build_flags = - ${hal_app:LineFollowerTarget.build_flags} - ${app:LineFollower.build_flags} -lib_deps = - ${hal_app:LineFollowerTarget.lib_deps} - ${app:LineFollower.lib_deps} -lib_ignore = - ${hal_app:LineFollowerTarget.lib_ignore} - ${app:LineFollower.lib_ignore} -extra_scripts = - ${hal_app:LineFollowerTarget.extra_scripts} - -; ***************************************************************************** -; Line follower application on simulation -; ***************************************************************************** -[env:LineFollowerSim] -extends = hal_app:LineFollowerSim, app:LineFollower, static_check_configuration -build_flags = - ${hal_app:LineFollowerSim.build_flags} - ${app:LineFollower.build_flags} - -D DEBUG_ALGORITHM - ;-D DEBUG_ODOMETRY -lib_deps = - ${hal_app:LineFollowerSim.lib_deps} - ${app:LineFollower.lib_deps} -lib_ignore = - ${hal_app:LineFollowerSim.lib_ignore} - ${app:LineFollower.lib_ignore} -extra_scripts = - ${hal_app:LineFollowerSim.extra_scripts} - -; ***************************************************************************** -; Simple line follower application on target -; ***************************************************************************** -[env:LineFollowerSimpleTarget] -extends = hal_app:LineFollowerTarget, app:LineFollowerSimple, static_check_configuration -build_flags = - ${hal_app:LineFollowerTarget.build_flags} - ${app:LineFollowerSimple.build_flags} -lib_deps = - ${hal_app:LineFollowerTarget.lib_deps} - ${app:LineFollowerSimple.lib_deps} -lib_ignore = - ${hal_app:LineFollowerTarget.lib_ignore} - ${app:LineFollowerSimple.lib_ignore} -extra_scripts = - ${hal_app:LineFollowerTarget.extra_scripts} - -; ***************************************************************************** -; Simple line follower application on simulation -; ***************************************************************************** -[env:LineFollowerSimpleSim] -extends = hal_app:LineFollowerSim, app:LineFollowerSimple, static_check_configuration -build_flags = - ${hal_app:LineFollowerSim.build_flags} - ${app:LineFollowerSimple.build_flags} -lib_deps = - ${hal_app:LineFollowerSim.lib_deps} - ${app:LineFollowerSimple.lib_deps} -lib_ignore = - ${hal_app:LineFollowerSim.lib_ignore} - ${app:LineFollowerSimple.lib_ignore} -extra_scripts = - ${hal_app:LineFollowerSim.extra_scripts} - -; ***************************************************************************** -; Reinforcement Learning for Line follower application on simulation -; ***************************************************************************** -[env:ReinforcementLearningSim] -extends = hal_app:ReinforcementLearningSim, app:ReinforcementLearning, static_check_configuration -build_flags = - ${hal_app:ReinforcementLearningSim.build_flags} - ${app:ReinforcementLearning.build_flags} -lib_deps = - ${hal_app:ReinforcementLearningSim.lib_deps} - ${app:ReinforcementLearning.lib_deps} -lib_ignore = - ${hal_app:ReinforcementLearningSim.lib_ignore} - ${app:ReinforcementLearning.lib_ignore} -extra_scripts = - ${hal_app:ReinforcementLearningSim.extra_scripts} - -; ***************************************************************************** -; Remote control application on target -; ***************************************************************************** -[env:RemoteControlTarget] -extends = hal_app:RemoteControlTarget, app:RemoteControl, static_check_configuration -build_flags = - ${hal_app:RemoteControlTarget.build_flags} - ${app:RemoteControl.build_flags} -lib_deps = - ${hal_app:RemoteControlTarget.lib_deps} - ${app:RemoteControl.lib_deps} -lib_ignore = - ${hal_app:RemoteControlTarget.lib_ignore} - ${app:RemoteControl.lib_ignore} -extra_scripts = - ${hal_app:RemoteControlTarget.extra_scripts} - -; ***************************************************************************** -; Remote control application on simulation -; ***************************************************************************** -[env:RemoteControlSim] -extends = hal_app:RemoteControlSim, app:RemoteControl, static_check_configuration -build_flags = - ${hal_app:RemoteControlSim.build_flags} - ${app:RemoteControl.build_flags} -lib_deps = - ${hal_app:RemoteControlSim.lib_deps} - ${app:RemoteControl.lib_deps} -lib_ignore = - ${hal_app:RemoteControlSim.lib_ignore} - ${app:RemoteControl.lib_ignore} -extra_scripts = - ${hal_app:RemoteControlSim.extra_scripts} - -; ***************************************************************************** -; Sensor Fusion application on target -; ***************************************************************************** -[env:SensorFusionTarget] -extends = hal_app:SensorFusionTarget, app:SensorFusion, static_check_configuration -build_flags = - ${hal_app:SensorFusionTarget.build_flags} - ${app:SensorFusion.build_flags} -lib_deps = - ${hal_app:SensorFusionTarget.lib_deps} - ${app:SensorFusion.lib_deps} -lib_ignore = - ${hal_app:SensorFusionTarget.lib_ignore} - ${app:SensorFusion.lib_ignore} -extra_scripts = - ${hal_app:SensorFusionTarget.extra_scripts} - -; ***************************************************************************** -; Sensor Fusion application on simulation -; ***************************************************************************** -[env:SensorFusionSim] -extends = hal_app:SensorFusionSim, app:SensorFusion, static_check_configuration -build_flags = - ${hal_app:SensorFusionSim.build_flags} - ${app:SensorFusion.build_flags} -lib_deps = - ${hal_app:SensorFusionSim.lib_deps} - ${app:SensorFusion.lib_deps} -lib_ignore = - ${hal_app:SensorFusionSim.lib_ignore} - ${app:SensorFusion.lib_ignore} -extra_scripts = - ${hal_app:SensorFusionSim.extra_scripts} - -; ***************************************************************************** -; PC target environment for tests -; ***************************************************************************** -[env:TestSim] -extends = hal_app:TestSim, app:Test, static_check_configuration -build_flags = - ${hal_app:TestSim.build_flags} - ${app:Test.build_flags} -lib_deps = - ${hal_app:TestSim.lib_deps} - ${app:Test.lib_deps} -lib_ignore = - ${hal_app:TestSim.lib_ignore} - ${app:Test.lib_ignore} -extra_scripts = - ${hal_app:TestSim.extra_scripts} - +;PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +; ***************************************************************************** +; PlatformIO specific configurations +; ***************************************************************************** +[platformio] +; Define default environments, which shall be automatically be built. +;default_envs = CalibTarget +;default_envs = CalibSim +;default_envs = ConvoyLeaderTarget +;default_envs = ConvoyLeaderSim +;default_envs = ConvoyFollowerSim +;default_envs = ConvoyFollowerTarget +;default_envs = LineFollowerTarget +;default_envs = LineFollowerSim +;default_envs = LineFollowerSimpleSim +;default_envs = RemoteControlTarget +;default_envs = RemoteControlSim +default_envs = ReinforcementLearningSim +;default_envs = SensorFusionTarget +;default_envs = SensorFusionSim +;default_envs = TestSim + +; ***************************************************************************** +; Common configurations, which is independed of the environment. +; ***************************************************************************** +[common] +build_flags = + -DTEAM_NAME_LINE_1="\"Radon\"" + -DTEAM_NAME_LINE_2="\"Ulzer\"" + +; ***************************************************************************** +; Static check configuration +; ***************************************************************************** +[static_check_configuration] +check_tool = cppcheck, clangtidy +check_severity = medium, high +check_patterns = + include + src + lib +check_flags = + cppcheck: cppcheck: --suppress=*:*/libdeps/* --suppress=*:*lib/Webots/* --suppress=noExplicitConstructor --suppress=unusedFunction --std=c++11 + clangtidy: --header-filter='' --checks=-*,clang-analyzer-*,performance-*,portability-*,readability-uppercase-literal-suffix,readability-redundant-control-flow --warnings-as-errors=-*,clang-analyzer-*,performance-*,portability-*,readability-uppercase-literal-suffix,readability-redundant-control-flow +check_skip_packages = yes + +; ***************************************************************************** +; Target environment for Zumo32U4. +; ***************************************************************************** +[hal:Target] +platform = atmelavr @ ~4.1.0 +board = a-star32U4 +framework = arduino +build_flags = + ${common.build_flags} + -Wno-switch + -Werror +lib_deps = + BlueAndi/ZumoHALATmega32u4 @ ~1.0.0 +lib_ignore = + HALTest +extra_scripts = + +monitor_speed = 115200 + +; The monitor port shows the debug output and the test output. +; If you connect the robot to your pc, this is the port you will see. +monitor_port = com11 + +; The upload port is spawned by the bootmonitor and used to update the program on the target. +; If you connect the robot to your pc, press twice reset button to jump to the bootloader, +; this is the port you will see. +upload_port = com5 + +; The test port is spawned by the bootmonitor and used to update the test on the target. +; If you connect the robot to your pc, press twice reset button to jump to the bootloader, +; this is the port you will see. +test_port = com9 + +; ***************************************************************************** +; PC target environment for Webots simulation. +; +; It is assumed that the environment variable WEBOTS_HOME is set to the +; Webots directory, e.g. WEBOTS_HOME=C:\Users\\AppData\Local\Programs\Webots +; ***************************************************************************** +[hal:Sim] +platform = native @ ~1.2.1 +build_flags = + -std=c++11 + -DTARGET_NATIVE + -I./lib/Webots/include/c + -I./lib/Webots/include/cpp +lib_deps = + MainNative + BlueAndi/ZumoHALWebots @ ~1.0.0 + Webots +lib_ignore = + HALTest +extra_scripts = + ./scripts/webots_launcher.py + pre:$PROJECT_LIBDEPS_DIR/$PIOENV/ZumoHALWebots/scripts/create_webots_library.py + pre:$PROJECT_LIBDEPS_DIR/$PIOENV/ZumoHALWebots/scripts/copy_sounds.py + post:$PROJECT_LIBDEPS_DIR/$PIOENV/ZumoHALWebots/scripts/copy_webots_shared_libs.py + post:./scripts/package.py +webots_robot_name = Zumo +webots_robot_serial_rx_channel = 1 +webots_robot_serial_tx_channel = 2 + +; ***************************************************************************** +; PC target environment for tests +; ***************************************************************************** +[hal:Test] +platform = native @ ~1.2.1 +build_flags = + -std=c++11 + -DTARGET_NATIVE + -DUNIT_TEST + -Itest/common +lib_deps = + MainTestNative + BlueAndi/ArduinoNative @ ~0.1.1 + BlueAndi/ZumoHALInterfaces @ ~1.0.0 + HALTest +lib_ignore = + Webots +extra_scripts = + +; ***************************************************************************** +; Calibration application +; ***************************************************************************** +[app:Calib] +build_flags = + ${common.build_flags} + -DLOG_DEBUG_ENABLE +lib_deps = + APPCalib + Service +lib_ignore = + APPConvoyLeader + APPLineFollower + APPReinforcementLearning + APPRemoteControl + APPSensorFusion + APPTest + +; ***************************************************************************** +; Calibration application specific HAL for target +; ***************************************************************************** +[hal_app:CalibTarget] +extends = hal:Target +build_flags = + ${hal:Target.build_flags} +lib_deps = + ${hal:Target.lib_deps} + HALCalibTarget +lib_ignore = + ${hal:Target.lib_ignore} +extra_scripts = + ${hal:Target.extra_scripts} + +; ***************************************************************************** +; Calibration application specific HAL for simulation +; ***************************************************************************** +[hal_app:CalibSim] +extends = hal:Sim +build_flags = + ${hal:Sim.build_flags} +lib_deps = + ${hal:Sim.lib_deps} + HALCalibSim +lib_ignore = + ${hal:Sim.lib_ignore} +extra_scripts = + ${hal:Sim.extra_scripts} + +; ***************************************************************************** +; Convoy leader application +; ***************************************************************************** +[app:ConvoyLeader] +build_flags = + ${common.build_flags} + -DLOG_DEBUG_ENABLE +lib_deps = + APPConvoyLeader + Service + gabryelreyes/SerialMuxProt @ ~2.2.0 +lib_ignore = + APPCalib + APPConvoyFollower + APPLineFollower + APPRemoteControl + APPReinforcementLearning + APPSensorFusion + APPTest + +; ***************************************************************************** +; Convoy leader application specific HAL for target +; ***************************************************************************** +[hal_app:ConvoyLeaderTarget] +extends = hal:Target +build_flags = + ${hal:Target.build_flags} +lib_deps = + ${hal:Target.lib_deps} + HALConvoyLeaderTarget +lib_ignore = + ${hal:Target.lib_ignore} +extra_scripts = + ${hal:Target.extra_scripts} + +; ***************************************************************************** +; Convoy leader application specific HAL for simulation +; ***************************************************************************** +[hal_app:ConvoyLeaderSim] +extends = hal:Sim +build_flags = + ${hal:Sim.build_flags} +lib_deps = + ${hal:Sim.lib_deps} + HALConvoyLeaderSim +lib_ignore = + ${hal:Sim.lib_ignore} +extra_scripts = + ${hal:Sim.extra_scripts} + +; ***************************************************************************** +; Convoy follower application +; ***************************************************************************** +[app:ConvoyFollower] +build_flags = + ${common.build_flags} + -DLOG_DEBUG_ENABLE +lib_deps = + APPConvoyFollower + Service + gabryelreyes/SerialMuxProt @ ~2.2.0 +lib_ignore = + APPCalib + APPConvoyLeader + APPLineFollower + APPRemoteControl + APPReinforcementLearning + APPSensorFusion + APPTest + +; ***************************************************************************** +; Convoy follower application specific HAL for target +; ***************************************************************************** +[hal_app:ConvoyFollowerTarget] +extends = hal:Target +build_flags = + ${hal:Target.build_flags} +lib_deps = + ${hal:Target.lib_deps} + HALConvoyFollowerTarget +lib_ignore = + ${hal:Target.lib_ignore} +extra_scripts = + ${hal:Target.extra_scripts} + +; ***************************************************************************** +; Convoy follower application specific HAL for simulation +; ***************************************************************************** +[hal_app:ConvoyFollowerSim] +extends = hal:Sim +build_flags = + ${hal:Sim.build_flags} +lib_deps = + ${hal:Sim.lib_deps} + HALConvoyFollowerSim +lib_ignore = + ${hal:Sim.lib_ignore} +extra_scripts = + ${hal:Sim.extra_scripts} + +; ***************************************************************************** +; Line follower application +; ***************************************************************************** +[app:LineFollower] +build_flags = + ${common.build_flags} + -DLOG_DEBUG_ENABLE +lib_deps = + APPLineFollower + Service +lib_ignore = + APPCalib + APPConvoyFollower + APPReinforcementLearning + APPConvoyLeader + APPRemoteControl + APPSensorFusion + APPTest + +; ***************************************************************************** +; Simple line follower application +; ***************************************************************************** +[app:LineFollowerSimple] +build_flags = + ${common.build_flags} + -DLOG_DEBUG_ENABLE +lib_deps = + APPLineFollowerSimple + Service +lib_ignore = + APPCalib + APPConvoyFollower + APPReinforcementLearning + APPConvoyLeader + APPLineFollower + APPRemoteControl + APPSensorFusion + APPTest + +; ***************************************************************************** +; Line follower application specific HAL for target +; ***************************************************************************** +[hal_app:LineFollowerTarget] +extends = hal:Target +build_flags = + ${hal:Target.build_flags} +lib_deps = + ${hal:Target.lib_deps} + HALLineFollowerTarget +lib_ignore = + ${hal:Target.lib_ignore} +extra_scripts = + ${hal:Target.extra_scripts} + +; ***************************************************************************** +; Line follower application specific HAL for simulation +; ***************************************************************************** +[hal_app:LineFollowerSim] +extends = hal:Sim +build_flags = + ${hal:Sim.build_flags} +lib_deps = + ${hal:Sim.lib_deps} + HALLineFollowerSim +lib_ignore = + ${hal:Sim.lib_ignore} +extra_scripts = + ${hal:Sim.extra_scripts} + +; ***************************************************************************** +; Reinforcement Learning for Line follower application +; ***************************************************************************** +[app:ReinforcementLearning] +build_flags = + ${common.build_flags} + -DLOG_DEBUG_ENABLE +lib_deps = + APPReinforcementLearning + Service + gabryelreyes/SerialMuxProt @ ~2.2.0 +lib_ignore = + APPCalib + APPConvoyLeader + APPRemoteControl + APPSensorFusion + APPLineFollower + APPTest + +; ***************************************************************************** +; Reinforcement Learning for Line follower application specific HAL for simulation +; ***************************************************************************** +[hal_app:ReinforcementLearningSim] +extends = hal:Sim +build_flags = + ${hal:Sim.build_flags} +lib_deps = + ${hal:Sim.lib_deps} + HALReinforcementLearningSim +lib_ignore = + ${hal:Sim.lib_ignore} +extra_scripts = + ${hal:Sim.extra_scripts} + +; ***************************************************************************** +; Remote control application +; ***************************************************************************** +[app:RemoteControl] +build_flags = + ${common.build_flags} + -DLOG_DEBUG_ENABLE +lib_deps = + APPRemoteControl + Service + gabryelreyes/SerialMuxProt @ ~2.2.0 +lib_ignore = + APPCalib + APPConvoyFollower + APPReinforcementLearning + APPConvoyLeader + APPLineFollower + APPSensorFusion + APPTest + +; ***************************************************************************** +; Remote control application specific HAL for target +; ***************************************************************************** +[hal_app:RemoteControlTarget] +extends = hal:Target +build_flags = + ${hal:Target.build_flags} +lib_deps = + ${hal:Target.lib_deps} + HALRemoteControlTarget +lib_ignore = + ${hal:Target.lib_ignore} +extra_scripts = + ${hal:Target.extra_scripts} + +; ***************************************************************************** +; Remote control application specific HAL for simulation +; ***************************************************************************** +[hal_app:RemoteControlSim] +extends = hal:Sim +build_flags = + ${hal:Sim.build_flags} +lib_deps = + ${hal:Sim.lib_deps} + HALRemoteControlSim +lib_ignore = + ${hal:Sim.lib_ignore} +extra_scripts = + ${hal:Sim.extra_scripts} + +; ***************************************************************************** +; Sensor Fusion application +; ***************************************************************************** +[app:SensorFusion] +build_flags = + ${common.build_flags} + -DLOG_DEBUG_ENABLE +lib_deps = + APPSensorFusion + Service + gabryelreyes/SerialMuxProt @ ~2.2.0 +lib_ignore = + APPCalib + APPConvoyFollower + APPConvoyLeader + APPLineFollower + APPReinforcementLearning + APPRemoteControl + APPTest + +; ***************************************************************************** +; Sensor Fusion application specific HAL for target +; ***************************************************************************** +[hal_app:SensorFusionTarget] +extends = hal:Target +build_flags = + ${hal:Target.build_flags} +lib_deps = + ${hal:Target.lib_deps} + HALSensorFusionTarget +lib_ignore = + ${hal:Target.lib_ignore} +extra_scripts = + ${hal:Target.extra_scripts} + +; ***************************************************************************** +; Sensor Fusion application specific HAL for simulation +; ***************************************************************************** +[hal_app:SensorFusionSim] +extends = hal:Sim +build_flags = + ${hal:Sim.build_flags} +lib_deps = + ${hal:Sim.lib_deps} + HALSensorFusionSim +lib_ignore = + ${hal:Sim.lib_ignore} +extra_scripts = + ${hal:Sim.extra_scripts} + +; ***************************************************************************** +; Test application +; ***************************************************************************** +[app:Test] +build_flags = + ${common.build_flags} +lib_deps = + APPTest + Service +lib_ignore = + APPCalib + APPConvoyFollower + APPConvoyLeader + APPLineFollower + APPReinforcementLearning + APPRemoteControl + APPSensorFusion + +; ***************************************************************************** +; Test application specific HAL for target +; ***************************************************************************** +[hal_app:TestSim] +extends = hal:Test +build_flags = + ${hal:Test.build_flags} +lib_deps = + ${hal:Test.lib_deps} + HALTestSim +lib_ignore = + ${hal:Test.lib_ignore} +extra_scripts = + ${hal:Test.extra_scripts} + +; ***************************************************************************** +; Calibration application on simulation +; ***************************************************************************** +[env:CalibSim] +extends = hal_app:CalibSim, app:Calib, static_check_configuration +build_flags = + ${hal_app:CalibSim.build_flags} + ${app:Calib.build_flags} +lib_deps = + ${hal_app:CalibSim.lib_deps} + ${app:Calib.lib_deps} +lib_ignore = + ${hal_app:CalibSim.lib_ignore} + ${app:Calib.lib_ignore} +extra_scripts = + ${hal_app:CalibSim.extra_scripts} + +; ***************************************************************************** +; Calibration application on target +; ***************************************************************************** +[env:CalibTarget] +extends = hal_app:CalibTarget, app:Calib, static_check_configuration +build_flags = + ${hal_app:CalibTarget.build_flags} + ${app:Calib.build_flags} +lib_deps = + ${hal_app:CalibTarget.lib_deps} + ${app:Calib.lib_deps} +lib_ignore = + ${hal_app:CalibTarget.lib_ignore} + ${app:Calib.lib_ignore} +extra_scripts = + ${hal_app:CalibTarget.extra_scripts} + +; ***************************************************************************** +; Convoy leader application on target +; ***************************************************************************** +[env:ConvoyLeaderTarget] +extends = hal_app:ConvoyLeaderTarget, app:ConvoyLeader, static_check_configuration +build_flags = + ${hal_app:ConvoyLeaderTarget.build_flags} + ${app:ConvoyLeader.build_flags} +lib_deps = + ${hal_app:ConvoyLeaderTarget.lib_deps} + ${app:ConvoyLeader.lib_deps} +lib_ignore = + ${hal_app:ConvoyLeaderTarget.lib_ignore} + ${app:ConvoyLeader.lib_ignore} +extra_scripts = + ${hal_app:ConvoyLeaderTarget.extra_scripts} + +; ***************************************************************************** +; Convoy leader application on simulation +; ***************************************************************************** +[env:ConvoyLeaderSim] +extends = hal_app:ConvoyLeaderSim, app:ConvoyLeader, static_check_configuration +build_flags = + ${hal_app:ConvoyLeaderSim.build_flags} + ${app:ConvoyLeader.build_flags} +lib_deps = + ${hal_app:ConvoyLeaderSim.lib_deps} + ${app:ConvoyLeader.lib_deps} +lib_ignore = + ${hal_app:ConvoyLeaderSim.lib_ignore} + ${app:ConvoyLeader.lib_ignore} +extra_scripts = + ${hal_app:ConvoyLeaderSim.extra_scripts} + +; ***************************************************************************** +; Convoy follower application on target +; ***************************************************************************** +[env:ConvoyFollowerTarget] +extends = hal_app:ConvoyFollowerTarget, app:ConvoyFollower, static_check_configuration +build_flags = + ${hal_app:ConvoyFollowerTarget.build_flags} + ${app:ConvoyFollower.build_flags} +lib_deps = + ${hal_app:ConvoyFollowerTarget.lib_deps} + ${app:ConvoyFollower.lib_deps} +lib_ignore = + ${hal_app:ConvoyFollowerTarget.lib_ignore} + ${app:ConvoyFollower.lib_ignore} +extra_scripts = + ${hal_app:ConvoyFollowerTarget.extra_scripts} + +; ***************************************************************************** +; Convoy follower application on simulation +; ***************************************************************************** +[env:ConvoyFollowerSim] +extends = hal_app:ConvoyFollowerSim, app:ConvoyFollower, static_check_configuration +build_flags = + ${hal_app:ConvoyFollowerSim.build_flags} + ${app:ConvoyFollower.build_flags} +lib_deps = + ${hal_app:ConvoyFollowerSim.lib_deps} + ${app:ConvoyFollower.lib_deps} +lib_ignore = + ${hal_app:ConvoyFollowerSim.lib_ignore} + ${app:ConvoyFollower.lib_ignore} +extra_scripts = + ${hal_app:ConvoyFollowerSim.extra_scripts} + +; ***************************************************************************** +; Line follower application on target +; ***************************************************************************** +[env:LineFollowerTarget] +extends = hal_app:LineFollowerTarget, app:LineFollower, static_check_configuration +build_flags = + ${hal_app:LineFollowerTarget.build_flags} + ${app:LineFollower.build_flags} +lib_deps = + ${hal_app:LineFollowerTarget.lib_deps} + ${app:LineFollower.lib_deps} +lib_ignore = + ${hal_app:LineFollowerTarget.lib_ignore} + ${app:LineFollower.lib_ignore} +extra_scripts = + ${hal_app:LineFollowerTarget.extra_scripts} + +; ***************************************************************************** +; Line follower application on simulation +; ***************************************************************************** +[env:LineFollowerSim] +extends = hal_app:LineFollowerSim, app:LineFollower, static_check_configuration +build_flags = + ${hal_app:LineFollowerSim.build_flags} + ${app:LineFollower.build_flags} + -D DEBUG_ALGORITHM + ;-D DEBUG_ODOMETRY +lib_deps = + ${hal_app:LineFollowerSim.lib_deps} + ${app:LineFollower.lib_deps} +lib_ignore = + ${hal_app:LineFollowerSim.lib_ignore} + ${app:LineFollower.lib_ignore} +extra_scripts = + ${hal_app:LineFollowerSim.extra_scripts} + +; ***************************************************************************** +; Simple line follower application on target +; ***************************************************************************** +[env:LineFollowerSimpleTarget] +extends = hal_app:LineFollowerTarget, app:LineFollowerSimple, static_check_configuration +build_flags = + ${hal_app:LineFollowerTarget.build_flags} + ${app:LineFollowerSimple.build_flags} +lib_deps = + ${hal_app:LineFollowerTarget.lib_deps} + ${app:LineFollowerSimple.lib_deps} +lib_ignore = + ${hal_app:LineFollowerTarget.lib_ignore} + ${app:LineFollowerSimple.lib_ignore} +extra_scripts = + ${hal_app:LineFollowerTarget.extra_scripts} + +; ***************************************************************************** +; Simple line follower application on simulation +; ***************************************************************************** +[env:LineFollowerSimpleSim] +extends = hal_app:LineFollowerSim, app:LineFollowerSimple, static_check_configuration +build_flags = + ${hal_app:LineFollowerSim.build_flags} + ${app:LineFollowerSimple.build_flags} +lib_deps = + ${hal_app:LineFollowerSim.lib_deps} + ${app:LineFollowerSimple.lib_deps} +lib_ignore = + ${hal_app:LineFollowerSim.lib_ignore} + ${app:LineFollowerSimple.lib_ignore} +extra_scripts = + ${hal_app:LineFollowerSim.extra_scripts} + +; ***************************************************************************** +; Reinforcement Learning for Line follower application on simulation +; ***************************************************************************** +[env:ReinforcementLearningSim] +extends = hal_app:ReinforcementLearningSim, app:ReinforcementLearning, static_check_configuration +build_flags = + ${hal_app:ReinforcementLearningSim.build_flags} + ${app:ReinforcementLearning.build_flags} +lib_deps = + ${hal_app:ReinforcementLearningSim.lib_deps} + ${app:ReinforcementLearning.lib_deps} +lib_ignore = + ${hal_app:ReinforcementLearningSim.lib_ignore} + ${app:ReinforcementLearning.lib_ignore} +extra_scripts = + ${hal_app:ReinforcementLearningSim.extra_scripts} + +; ***************************************************************************** +; Remote control application on target +; ***************************************************************************** +[env:RemoteControlTarget] +extends = hal_app:RemoteControlTarget, app:RemoteControl, static_check_configuration +build_flags = + ${hal_app:RemoteControlTarget.build_flags} + ${app:RemoteControl.build_flags} +lib_deps = + ${hal_app:RemoteControlTarget.lib_deps} + ${app:RemoteControl.lib_deps} +lib_ignore = + ${hal_app:RemoteControlTarget.lib_ignore} + ${app:RemoteControl.lib_ignore} +extra_scripts = + ${hal_app:RemoteControlTarget.extra_scripts} + +; ***************************************************************************** +; Remote control application on simulation +; ***************************************************************************** +[env:RemoteControlSim] +extends = hal_app:RemoteControlSim, app:RemoteControl, static_check_configuration +build_flags = + ${hal_app:RemoteControlSim.build_flags} + ${app:RemoteControl.build_flags} +lib_deps = + ${hal_app:RemoteControlSim.lib_deps} + ${app:RemoteControl.lib_deps} +lib_ignore = + ${hal_app:RemoteControlSim.lib_ignore} + ${app:RemoteControl.lib_ignore} +extra_scripts = + ${hal_app:RemoteControlSim.extra_scripts} + +; ***************************************************************************** +; Sensor Fusion application on target +; ***************************************************************************** +[env:SensorFusionTarget] +extends = hal_app:SensorFusionTarget, app:SensorFusion, static_check_configuration +build_flags = + ${hal_app:SensorFusionTarget.build_flags} + ${app:SensorFusion.build_flags} +lib_deps = + ${hal_app:SensorFusionTarget.lib_deps} + ${app:SensorFusion.lib_deps} +lib_ignore = + ${hal_app:SensorFusionTarget.lib_ignore} + ${app:SensorFusion.lib_ignore} +extra_scripts = + ${hal_app:SensorFusionTarget.extra_scripts} + +; ***************************************************************************** +; Sensor Fusion application on simulation +; ***************************************************************************** +[env:SensorFusionSim] +extends = hal_app:SensorFusionSim, app:SensorFusion, static_check_configuration +build_flags = + ${hal_app:SensorFusionSim.build_flags} + ${app:SensorFusion.build_flags} +lib_deps = + ${hal_app:SensorFusionSim.lib_deps} + ${app:SensorFusion.lib_deps} +lib_ignore = + ${hal_app:SensorFusionSim.lib_ignore} + ${app:SensorFusion.lib_ignore} +extra_scripts = + ${hal_app:SensorFusionSim.extra_scripts} + +; ***************************************************************************** +; PC target environment for tests +; ***************************************************************************** +[env:TestSim] +extends = hal_app:TestSim, app:Test, static_check_configuration +build_flags = + ${hal_app:TestSim.build_flags} + ${app:Test.build_flags} +lib_deps = + ${hal_app:TestSim.lib_deps} + ${app:Test.lib_deps} +lib_ignore = + ${hal_app:TestSim.lib_ignore} + ${app:Test.lib_ignore} +extra_scripts = + ${hal_app:TestSim.extra_scripts} + From 745f6c2aa9936ae646f46be22052345aa6364947 Mon Sep 17 00:00:00 2001 From: Akram Date: Fri, 26 Jul 2024 11:07:52 +0200 Subject: [PATCH 23/58] Delet unused files --- lib/APPReinforcementLearning/src/App.cpp | 146 --- lib/APPReinforcementLearning/src/App.h | 123 --- .../src/DrivingState.cpp | 899 ------------------ .../src/DrivingState.h | 360 ------- .../src/ErrorState.cpp | 139 --- lib/APPReinforcementLearning/src/ErrorState.h | 149 --- lib/APPReinforcementLearning/src/IBoard.h | 205 ---- .../src/LineSensorsCalibrationState.cpp | 213 ----- .../src/LineSensorsCalibrationState.h | 175 ---- .../src/MotorSpeedCalibrationState.cpp | 245 ----- .../src/MotorSpeedCalibrationState.h | 179 ---- .../src/ParameterSets.cpp | 150 --- .../src/ParameterSets.h | 159 ---- .../src/ReadyState.cpp | 168 ---- lib/APPReinforcementLearning/src/ReadyState.h | 144 --- .../src/ReleaseTrackState.cpp | 129 --- .../src/ReleaseTrackState.h | 144 --- .../src/StartupState.cpp | 200 ---- .../src/StartupState.h | 169 ---- 19 files changed, 4096 deletions(-) delete mode 100644 lib/APPReinforcementLearning/src/App.cpp delete mode 100644 lib/APPReinforcementLearning/src/App.h delete mode 100644 lib/APPReinforcementLearning/src/DrivingState.cpp delete mode 100644 lib/APPReinforcementLearning/src/DrivingState.h delete mode 100644 lib/APPReinforcementLearning/src/ErrorState.cpp delete mode 100644 lib/APPReinforcementLearning/src/ErrorState.h delete mode 100644 lib/APPReinforcementLearning/src/IBoard.h delete mode 100644 lib/APPReinforcementLearning/src/LineSensorsCalibrationState.cpp delete mode 100644 lib/APPReinforcementLearning/src/LineSensorsCalibrationState.h delete mode 100644 lib/APPReinforcementLearning/src/MotorSpeedCalibrationState.cpp delete mode 100644 lib/APPReinforcementLearning/src/MotorSpeedCalibrationState.h delete mode 100644 lib/APPReinforcementLearning/src/ParameterSets.cpp delete mode 100644 lib/APPReinforcementLearning/src/ParameterSets.h delete mode 100644 lib/APPReinforcementLearning/src/ReadyState.cpp delete mode 100644 lib/APPReinforcementLearning/src/ReadyState.h delete mode 100644 lib/APPReinforcementLearning/src/ReleaseTrackState.cpp delete mode 100644 lib/APPReinforcementLearning/src/ReleaseTrackState.h delete mode 100644 lib/APPReinforcementLearning/src/StartupState.cpp delete mode 100644 lib/APPReinforcementLearning/src/StartupState.h diff --git a/lib/APPReinforcementLearning/src/App.cpp b/lib/APPReinforcementLearning/src/App.cpp deleted file mode 100644 index 1a7f376c..00000000 --- a/lib/APPReinforcementLearning/src/App.cpp +++ /dev/null @@ -1,146 +0,0 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief LineFollower application - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "App.h" -#include "StartupState.h" -#include -#include -#include -#include -#include -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void App::setup() -{ - Serial.begin(SERIAL_BAUDRATE); - - /* Initialize HAL */ - Board::getInstance().init(); - - /* Setup the state machine with the first state. */ - m_systemStateMachine.setState(&StartupState::getInstance()); - - /* Setup the periodically processing of robot control. */ - m_controlInterval.start(DIFFERENTIAL_DRIVE_CONTROL_PERIOD); - -#ifdef DEBUG_ODOMETRY - /* Reset supervisor which set its observed position and orientation to 0. */ - Board::getInstance().getSender().send("RST"); -#endif /* DEBUG_ODOMETRY */ - - /* Surprise the audience. */ - Sound::playMelody(Sound::MELODY_WELCOME); -} - -void App::loop() -{ - Board::getInstance().process(); - Speedometer::getInstance().process(); - - if (true == m_controlInterval.isTimeout()) - { - /* The differential drive control needs the measured speed of the - * left and right wheel. Therefore it shall be processed after - * the speedometer. - */ - DifferentialDrive::getInstance().process(DIFFERENTIAL_DRIVE_CONTROL_PERIOD); - - /* The odometry unit needs to detect motor speed changes to be able to - * calculate correct values. Therefore it shall be processed right after - * the differential drive control. - */ - Odometry::getInstance().process(); - -#ifdef DEBUG_ODOMETRY - { - Odometry& odo = Odometry::getInstance(); - int32_t posX = 0; - int32_t posY = 0; - int32_t orientation = odo.getOrientation(); - const size_t BUFFER_SIZE = 128U; - char buffer[BUFFER_SIZE]; - - odo.getPosition(posX, posY); - - snprintf(buffer, BUFFER_SIZE, "ODO,%d,%d,%d", posX, posY, orientation); - - Board::getInstance().getSender().send(buffer); - } -#endif /* DEBUG_ODOMETRY */ - - m_controlInterval.restart(); - } - - m_systemStateMachine.process(); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ \ No newline at end of file diff --git a/lib/APPReinforcementLearning/src/App.h b/lib/APPReinforcementLearning/src/App.h deleted file mode 100644 index b09e1c8f..00000000 --- a/lib/APPReinforcementLearning/src/App.h +++ /dev/null @@ -1,123 +0,0 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief LineFollower application - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef APP_H -#define APP_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The line follower application. */ -class App -{ -public: - /** - * Construct the line follower application. - */ - App() : m_systemStateMachine(), m_controlInterval() - { - } - - /** - * Destroy the line follower application. - */ - ~App() - { - } - - /** - * Setup the application. - */ - void setup(); - - /** - * Process the application periodically. - */ - void loop(); - -private: - /** Differential drive control period in ms. */ - static const uint32_t DIFFERENTIAL_DRIVE_CONTROL_PERIOD = 5U; - - /** Baudrate for Serial Communication */ - static const uint32_t SERIAL_BAUDRATE = 115200U; - - /** The system state machine. */ - StateMachine m_systemStateMachine; - - /** Timer used for differential drive control processing. */ - SimpleTimer m_controlInterval; - - /** - * Copy construction of an instance. - * Not allowed. - * - * @param[in] app Source instance. - */ - App(const App& app); - - /** - * Assignment of an instance. - * Not allowed. - * - * @param[in] app Source instance. - * - * @returns Reference to App instance. - */ - App& operator=(const App& app); -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* APP_H */ -/** @} */ diff --git a/lib/APPReinforcementLearning/src/DrivingState.cpp b/lib/APPReinforcementLearning/src/DrivingState.cpp deleted file mode 100644 index 00523b22..00000000 --- a/lib/APPReinforcementLearning/src/DrivingState.cpp +++ /dev/null @@ -1,899 +0,0 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Driving state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "DrivingState.h" -#include -#include -#include -#include -#include -#include "ReadyState.h" -#include "ParameterSets.h" -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -#ifdef DEBUG_ALGORITHM -static void logCsvDataTitles(uint8_t length); -static void logCsvDataTimestamp(); -static void logCsvData(const uint16_t* lineSensorValues, uint8_t length, int16_t pos, int16_t pos3, bool isPos3Valid); -void logCsvDataTrackStatus(DrivingState::TrackStatus trackStatus); -static void logCsvDataSpeed(int16_t leftSpeed, int16_t rightSpeed); -#endif /* DEBUG_ALGORITHM */ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -#ifdef DEBUG_ALGORITHM -static int16_t gSpeedLeft = 0; -static int16_t gSpeedRight = 0; -#endif /* DEBUG_ALGORITHM */ - -const uint16_t DrivingState::SENSOR_VALUE_MAX = Board::getInstance().getLineSensors().getSensorValueMax(); - -/* Calculate the position set point to be generic. */ -const int16_t DrivingState::POSITION_SET_POINT = - (SENSOR_VALUE_MAX * (Board::getInstance().getLineSensors().getNumLineSensors() - 1)) / 2; - -/* Initialize the required sensor IDs to be generic. */ -const uint8_t DrivingState::SENSOR_ID_MOST_LEFT = 0U; -const uint8_t DrivingState::SENSOR_ID_MIDDLE = (Board::getInstance().getLineSensors().getNumLineSensors() - 1U) / 2U; -const uint8_t DrivingState::SENSOR_ID_MOST_RIGHT = Board::getInstance().getLineSensors().getNumLineSensors() - 1U; - -/* Initialize the position values used by the algorithmic. */ -const int16_t DrivingState::POSITION_MIN = 0; -const int16_t DrivingState::POSITION_MAX = - static_cast(Board::getInstance().getLineSensors().getNumLineSensors() - 1U) * 1000; -const int16_t DrivingState::POSITION_MIDDLE_MIN = POSITION_SET_POINT - (SENSOR_VALUE_MAX / 2); -const int16_t DrivingState::POSITION_MIDDLE_MAX = POSITION_SET_POINT + (SENSOR_VALUE_MAX / 2); - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void DrivingState::entry() -{ - IDisplay& display = Board::getInstance().getDisplay(); - const ParameterSets::ParameterSet& parSet = ParameterSets::getInstance().getParameterSet(); - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - const int16_t maxSpeed = diffDrive.getMaxMotorSpeed(); /* [steps/s] */ - - m_observationTimer.start(OBSERVATION_DURATION); - m_pidProcessTime.start(0); /* Immediate */ - m_lineStatus = LINE_STATUS_NO_START_LINE_DETECTED; - m_trackStatus = TRACK_STATUS_NORMAL; /* Assume that the robot is placed on track. */ - m_isTrackLost = false; /* Assume that the robot is placed on track. */ - m_isStartStopLineDetected = false; - - /* Configure PID controller with selected parameter set. */ - m_topSpeed = parSet.topSpeed; - m_pidCtrl.clear(); - m_pidCtrl.setPFactor(parSet.kPNumerator, parSet.kPDenominator); - m_pidCtrl.setIFactor(parSet.kINumerator, parSet.kIDenominator); - m_pidCtrl.setDFactor(parSet.kDNumerator, parSet.kDDenominator); - m_pidCtrl.setSampleTime(PID_PROCESS_PERIOD); - m_pidCtrl.setLimits(-maxSpeed, maxSpeed); - m_pidCtrl.setDerivativeOnMeasurement(false); - - display.clear(); - display.print("DRV"); - -#ifdef DEBUG_ALGORITHM - logCsvDataTitles(Board::getInstance().getLineSensors().getNumLineSensors()); -#endif /* DEBUG_ALGORITHM */ -} - -void DrivingState::process(StateMachine& sm) -{ - ILineSensors& lineSensors = Board::getInstance().getLineSensors(); - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - TrackStatus nextTrackStatus = m_trackStatus; - bool allowNegativeMotorSpeed = true; - - /* Get the position of the line and each sensor value. */ - int16_t position = lineSensors.readLine(); - const uint16_t* lineSensorValues = lineSensors.getSensorValues(); - uint8_t numLineSensors = lineSensors.getNumLineSensors(); - int16_t position3 = 0; - bool isPosition3Valid = calcPosition3(position3, lineSensorValues, numLineSensors); - bool isTrackLost = isNoLineDetected(lineSensorValues, numLineSensors); - -#ifdef DEBUG_ALGORITHM - logCsvDataTimestamp(); - logCsvData(lineSensorValues, numLineSensors, position, position3, isPosition3Valid); -#endif /* DEBUG_ALGORITHM */ - - /* If the position calculated with the inner sensors is not valid, the - * position will be taken. - */ - if (true == isPosition3Valid) - { - position3 = position; - } - - /* ======================================================================== - * Evaluate the situation based on the sensor values. - * ======================================================================== - */ - nextTrackStatus = evaluateSituation(lineSensorValues, numLineSensors, position, position3, isTrackLost); - - /* ======================================================================== - * Initiate measures depended on the situation. - * ======================================================================== - */ - processSituation(position, allowNegativeMotorSpeed, nextTrackStatus, position3); - - /* If the tracks status changes, the PID integral and derivative part - * will be reset to provide a smooth reaction. - */ - if (m_trackStatus != nextTrackStatus) - { - m_pidCtrl.clear(); - } - - /* ======================================================================== - * Handle start-/stop-line actions. - * ======================================================================== - */ - if ((TRACK_STATUS_START_STOP_LINE != m_trackStatus) && (TRACK_STATUS_START_STOP_LINE == nextTrackStatus)) - { - /* Start line detected? */ - if (LINE_STATUS_NO_START_LINE_DETECTED == m_lineStatus) - { - /* Measure the lap time and use as start point the detected start line. */ - m_lapTime.start(0); - - m_lineStatus = LINE_STATUS_START_LINE_DETECTED; - } - /* Stop line detected. */ - else - { - /* Calculate lap time and show it. */ - ReadyState::getInstance().setLapTime(m_lapTime.getCurrentDuration()); - - m_lineStatus = LINE_STATUS_STOP_LINE_DETECTED; - - /* Overwrite track status. */ - nextTrackStatus = TRACK_STATUS_FINISHED; - } - - /* Notify user about start-/stop-line detection. */ - Sound::playBeep(); - } - - /* ======================================================================== - * Handle track lost or back on track actions. - * ======================================================================== - */ - - /* Track lost just in this process cycle? */ - if ((false == m_isTrackLost) && (true == isTrackLost)) - { - /* Notify user by yellow LED. */ - Board::getInstance().getYellowLed().enable(true); - - /* Set mileage to 0, to be able to measure the max. distance, till - * the track must be found again. - */ - Odometry::getInstance().clearMileage(); - } - /* Track found again just in this process cycle? */ - else if ((true == m_isTrackLost) && (false == isTrackLost)) - { - Board::getInstance().getYellowLed().enable(false); - } - else - { - ; - } - - /* ======================================================================== - * Handle the abort conditions which will cause a alarm stop. - * ======================================================================== - */ - - /* Check whether the abort conditions are true. */ - if (true == isAbortRequired()) - { - /* Stop motors immediately. Don't move this to a later position, - * as this would extend the driven length. - */ - diffDrive.setLinearSpeed(0, 0); - - Sound::playAlarm(); /* Blocking! */ - - /* Clear lap time. */ - ReadyState::getInstance().setLapTime(0); - - /* Overwrite track status. */ - nextTrackStatus = TRACK_STATUS_FINISHED; - } - - /* ======================================================================== - * Handle driving based on position or normal stop condition. - * ======================================================================== - */ - - /* Periodically adapt driving and check the abort conditions, except - * the round is finished. - */ - if (TRACK_STATUS_FINISHED != nextTrackStatus) - { - /* Process the line follower PID controller periodically to adapt driving. */ - if (true == m_pidProcessTime.isTimeout()) - { - adaptDriving(position, allowNegativeMotorSpeed); - - m_pidProcessTime.start(PID_PROCESS_PERIOD); - } - } - /* Finished. */ - else - { - /* Stop motors immediately. Don't move this to a later position, - * as this would extend the driven length. - */ - diffDrive.setLinearSpeed(0, 0); - - /* Change to ready state. */ - sm.setState(&ReadyState::getInstance()); - } - -#ifdef DEBUG_ALGORITHM - logCsvDataTrackStatus(nextTrackStatus); - logCsvDataSpeed(gSpeedLeft, gSpeedRight); - printf("\n"); -#endif /* DEBUG_ALGORITHM */ - - /* Take over values for next cycle. */ - m_trackStatus = nextTrackStatus; - m_isTrackLost = isTrackLost; - m_lastPosition = position; -} - -void DrivingState::exit() -{ - m_observationTimer.stop(); - Board::getInstance().getYellowLed().enable(false); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -DrivingState::DrivingState() : - m_observationTimer(), - m_lapTime(), - m_pidProcessTime(), - m_pidCtrl(), - m_topSpeed(0), - m_lineStatus(LINE_STATUS_NO_START_LINE_DETECTED), - m_trackStatus(TRACK_STATUS_NORMAL), - m_isStartStopLineDetected(false), - m_lastSensorIdSawTrack(SENSOR_ID_MIDDLE), - m_lastPosition(0), - m_isTrackLost(false) -{ -} - -bool DrivingState::calcPosition3(int16_t& position, const uint16_t* lineSensorValues, uint8_t length) const -{ - const int32_t WEIGHT = SENSOR_VALUE_MAX; - bool isValid = true; - int32_t numerator = 0U; - int32_t denominator = 0U; - int32_t idxBegin = 1; - int32_t idxEnd = length - 1; - - for (int32_t idx = idxBegin; idx < idxEnd; ++idx) - { - int32_t sensorValue = static_cast(lineSensorValues[idx]); - - numerator += idx * WEIGHT * sensorValue; - denominator += sensorValue; - } - - if (0 == denominator) - { - isValid = false; - } - else - { - position = numerator / denominator; - } - - return isValid; -} - -DrivingState::TrackStatus DrivingState::evaluateSituation(const uint16_t* lineSensorValues, uint8_t length, - int16_t position, int16_t position3, bool isTrackLost) const -{ - TrackStatus nextTrackStatus = m_trackStatus; - - /* Driving over start-/stop-line? */ - if (TRACK_STATUS_START_STOP_LINE == m_trackStatus) - { - /* Left the start-/stop-line? - * If the robot is not exact on the start-/stop-line, the calculated position - * may misslead. Therefore additional the most left and right sensor values - * are evaluated too. - */ - if ((POSITION_MIDDLE_MIN <= position) && (POSITION_MIDDLE_MAX >= position) && - (LINE_SENSOR_ON_TRACK_MIN_VALUE > lineSensorValues[SENSOR_ID_MOST_LEFT]) && - (LINE_SENSOR_ON_TRACK_MIN_VALUE > lineSensorValues[SENSOR_ID_MOST_RIGHT])) - { - nextTrackStatus = TRACK_STATUS_NORMAL; - } - } - /* Is the start-/stop-line detected? */ - else if (true == isStartStopLineDetected(lineSensorValues, length)) - { - nextTrackStatus = TRACK_STATUS_START_STOP_LINE; - } - else if (TRACK_STATUS_RIGHT_ANGLE_CURVE_LEFT == m_trackStatus) - { - if ((POSITION_MIDDLE_MIN <= position) && (POSITION_MIDDLE_MAX >= position)) - { - nextTrackStatus = TRACK_STATUS_NORMAL; - } - } - else if (TRACK_STATUS_RIGHT_ANGLE_CURVE_RIGHT == m_trackStatus) - { - if ((POSITION_MIDDLE_MIN <= position) && (POSITION_MIDDLE_MAX >= position)) - { - nextTrackStatus = TRACK_STATUS_NORMAL; - } - } - /* Sharp curve to the left expected? */ - else if (TRACK_STATUS_SHARP_CURVE_LEFT == m_trackStatus) - { - /* Turn just before the robot leaves the line. */ - if (true == isTrackLost) - { - nextTrackStatus = TRACK_STATUS_SHARP_CURVE_LEFT_TURN; - } - } - /* Sharp curve to the right expected? */ - else if (TRACK_STATUS_SHARP_CURVE_RIGHT == m_trackStatus) - { - /* Turn just before the robot leaves the line. */ - if (true == isTrackLost) - { - nextTrackStatus = TRACK_STATUS_SHARP_CURVE_RIGHT_TURN; - } - } - /* Sharp curve to the left turning now! */ - else if (TRACK_STATUS_SHARP_CURVE_LEFT_TURN == m_trackStatus) - { - if ((POSITION_MIDDLE_MIN <= position3) && (POSITION_MIDDLE_MAX >= position3)) - { - nextTrackStatus = TRACK_STATUS_NORMAL; - } - } - /* Sharp curve to the right turning now! */ - else if (TRACK_STATUS_SHARP_CURVE_RIGHT_TURN == m_trackStatus) - { - if ((POSITION_MIDDLE_MIN <= position3) && (POSITION_MIDDLE_MAX >= position3)) - { - nextTrackStatus = TRACK_STATUS_NORMAL; - } - } - /* Is the track lost or just a gap in the track? */ - else if (true == isTrackLost) - { - const int16_t POS_MIN = POSITION_SET_POINT - SENSOR_VALUE_MAX; - const int16_t POS_MAX = POSITION_SET_POINT + SENSOR_VALUE_MAX; - - /* If its a gap in the track, last position will be well. */ - if ((POS_MIN <= m_lastPosition) && (POS_MAX > m_lastPosition)) - { - nextTrackStatus = TRACK_STATUS_TRACK_LOST_BY_GAP; - } - else - { - /* In any other case, route the calculated position through and - * hope. It will be the position of the most left sensor or the - * most right sensor. - */ - nextTrackStatus = TRACK_STATUS_TRACK_LOST_BY_MANOEUVRE; - } - } - /* Right angle curve to left detected? */ - else if (true == isRightAngleCurveToLeft(lineSensorValues, length)) - { - nextTrackStatus = TRACK_STATUS_RIGHT_ANGLE_CURVE_LEFT; - } - /* Right angle curve to right detected? */ - else if (true == isRightAngleCurveToRight(lineSensorValues, length)) - { - nextTrackStatus = TRACK_STATUS_RIGHT_ANGLE_CURVE_RIGHT; - } - /* Sharp curve to left detected? */ - else if (true == isSharpLeftCurveDetected(lineSensorValues, length, position3)) - { - nextTrackStatus = TRACK_STATUS_SHARP_CURVE_LEFT; - } - /* Sharp curve to right detected? */ - else if (true == isSharpRightCurveDetected(lineSensorValues, length, position3)) - { - nextTrackStatus = TRACK_STATUS_SHARP_CURVE_RIGHT; - } - /* Nothing special. */ - else - { - /* Just follow the line by calculated position. */ - nextTrackStatus = TRACK_STATUS_NORMAL; - } - - return nextTrackStatus; -} - -bool DrivingState::isStartStopLineDetected(const uint16_t* lineSensorValues, uint8_t length) const -{ - bool isDetected = false; - const uint32_t LINE_MAX_30 = (SENSOR_VALUE_MAX * 3U) / 10U; /* 30 % of max. value */ - const uint32_t LINE_MAX_70 = (SENSOR_VALUE_MAX * 7U) / 10U; /* 70 % of max. value */ - - /* - * === = === - * + + + + + - * L M R - */ - if ((LINE_MAX_30 <= lineSensorValues[SENSOR_ID_MOST_LEFT]) && - (LINE_MAX_70 > lineSensorValues[SENSOR_ID_MIDDLE - 1U]) && - (LINE_MAX_70 <= lineSensorValues[SENSOR_ID_MIDDLE]) && - (LINE_MAX_70 > lineSensorValues[SENSOR_ID_MIDDLE + 1U]) && - (LINE_MAX_30 <= lineSensorValues[SENSOR_ID_MOST_RIGHT])) - { - isDetected = true; - } - - return isDetected; -} - -bool DrivingState::isNoLineDetected(const uint16_t* lineSensorValues, uint8_t length) const -{ - bool isDetected = true; - uint8_t idx = SENSOR_ID_MOST_RIGHT; - - /* - * - * + + + + + - * L M R - */ - for (idx = SENSOR_ID_MOST_LEFT; idx <= SENSOR_ID_MOST_RIGHT; ++idx) - { - if (LINE_SENSOR_ON_TRACK_MIN_VALUE <= lineSensorValues[idx]) - { - isDetected = false; - break; - } - } - - return isDetected; -} - -bool DrivingState::isRightAngleCurveToLeft(const uint16_t* lineSensorValues, uint8_t length) const -{ - bool isDetected = true; - uint8_t idx = SENSOR_ID_MOST_LEFT; - - /* - * ========= - * + + + + + - * L M R - */ - for (idx = SENSOR_ID_MOST_LEFT; idx <= SENSOR_ID_MIDDLE; ++idx) - { - if (LINE_SENSOR_ON_TRACK_MIN_VALUE > lineSensorValues[idx]) - { - isDetected = false; - break; - } - } - - return isDetected; -} - -bool DrivingState::isRightAngleCurveToRight(const uint16_t* lineSensorValues, uint8_t length) const -{ - bool isDetected = true; - uint8_t idx = SENSOR_ID_MOST_RIGHT; - - /* - * ========= - * + + + + + - * L M R - */ - for (idx = SENSOR_ID_MIDDLE; idx <= SENSOR_ID_MOST_RIGHT; ++idx) - { - if (LINE_SENSOR_ON_TRACK_MIN_VALUE > lineSensorValues[idx]) - { - isDetected = false; - break; - } - } - - return isDetected; -} - -bool DrivingState::isSharpLeftCurveDetected(const uint16_t* lineSensorValues, uint8_t length, int16_t position3) const -{ - bool isDetected = false; - const uint32_t LINE_MAX_30 = (SENSOR_VALUE_MAX * 3U) / 10U; /* 30 % of max. value */ - - /* - * = = - * + + + + + - * L M R - */ - if ((LINE_MAX_30 <= lineSensorValues[SENSOR_ID_MOST_LEFT]) && (POSITION_MIDDLE_MIN <= position3) && - (POSITION_MIDDLE_MAX >= position3)) - { - isDetected = true; - } - - return isDetected; -} - -bool DrivingState::isSharpRightCurveDetected(const uint16_t* lineSensorValues, uint8_t length, int16_t position3) const -{ - bool isDetected = false; - const uint32_t LINE_MAX_30 = (SENSOR_VALUE_MAX * 3U) / 10U; /* 30 % of max. value */ - - /* - * = = - * + + + + + - * L M R - */ - if ((LINE_MAX_30 <= lineSensorValues[SENSOR_ID_MOST_RIGHT]) && (POSITION_MIDDLE_MIN <= position3) && - (POSITION_MIDDLE_MAX >= position3)) - { - isDetected = true; - } - - return isDetected; -} - -void DrivingState::processSituation(int16_t& position, bool& allowNegativeMotorSpeed, TrackStatus trackStatus, int16_t position3) -{ - switch (trackStatus) - { - case TRACK_STATUS_NORMAL: - /* The position is used by default to follow the line. */ - break; - - case TRACK_STATUS_START_STOP_LINE: - /* Use the inner sensors only for driving to avoid jerky movements, caused - * by the most left or right sensor. - */ - position = position3; - - /* Avoid that the robot turns in place. */ - allowNegativeMotorSpeed = false; - break; - - case TRACK_STATUS_RIGHT_ANGLE_CURVE_LEFT: - /* Enfore max. error in PID to turn sharp left at place. */ - position = POSITION_MIN; - break; - - case TRACK_STATUS_RIGHT_ANGLE_CURVE_RIGHT: - /* Enfore max. error in PID to turn sharp right at place. */ - position = POSITION_MAX; - break; - - case TRACK_STATUS_SHARP_CURVE_LEFT: - /* Stratey is to drive till the end of the curve with the inner sensors. - * Then a hard in place turn will appear, see TRACK_STATUS_SHARP_CURVE_LEFT_TURN. - */ - position = position3; - break; - - case TRACK_STATUS_SHARP_CURVE_RIGHT: - /* Stratey is to drive till the end of the curve with the inner sensors. - * Then a hard in place turn will appear, see TRACK_STATUS_SHARP_CURVE_LEFT_TURN. - */ - position = position3; - break; - - case TRACK_STATUS_SHARP_CURVE_LEFT_TURN: - /* Enfore max. error in PID to turn sharp left at place. */ - position = POSITION_MIN; - break; - - case TRACK_STATUS_SHARP_CURVE_RIGHT_TURN: - /* Enfore max. error in PID to turn sharp right at place. */ - position = POSITION_MAX; - break; - - case TRACK_STATUS_TRACK_LOST_BY_GAP: - /* Overwrite the positin to drive straight forward in hope to see the - * line again. - */ - position = POSITION_SET_POINT; - - /* Avoid that the robots turns. */ - allowNegativeMotorSpeed = false; - break; - - case TRACK_STATUS_TRACK_LOST_BY_MANOEUVRE: - /* If the track is lost by a robot manoeuvre and not becase the track - * has a gap, the last position given by most left or right sensor - * will be automatically used to find the track again. - * - * See ILineSensors::readLine() description regarding the behaviour in - * case the line is not detected anymore. - */ - break; - - case TRACK_STATUS_FINISHED: - /* Nothing to do. */ - break; - - default: - /* Should never happen. */ - break; - } -} - -void DrivingState::adaptDriving(int16_t position, bool allowNegativeMotorSpeed) -{ - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - const int16_t MAX_MOTOR_SPEED = diffDrive.getMaxMotorSpeed(); - const int16_t MIN_MOTOR_SPEED = (false == allowNegativeMotorSpeed) ? 0 : (-MAX_MOTOR_SPEED); - int16_t speedDifference = 0; /* [steps/s] */ - int16_t leftSpeed = 0; /* [steps/s] */ - int16_t rightSpeed = 0; /* [steps/s] */ - - /* Our "error" is how far we are away from the center of the - * line, which corresponds to position (max. line sensor value multiplied - * with sensor index). - * - * Get motor speed difference using PID terms. - */ - speedDifference = m_pidCtrl.calculate(POSITION_SET_POINT, position); - - /* Get individual motor speeds. The sign of speedDifference - * determines if the robot turns left or right. - */ - leftSpeed = m_topSpeed - speedDifference; - rightSpeed = m_topSpeed + speedDifference; - - /* Constrain our motor speeds to be between 0 and maxSpeed. - * One motor will always be turning at maxSpeed, and the other - * will be at maxSpeed-|speedDifference| if that is positive, - * else it will be stationary. For some applications, you - * might want to allow the motor speed to go negative so that - * it can spin in reverse. - */ - leftSpeed = constrain(leftSpeed, MIN_MOTOR_SPEED, MAX_MOTOR_SPEED); - rightSpeed = constrain(rightSpeed, MIN_MOTOR_SPEED, MAX_MOTOR_SPEED); - -#ifdef DEBUG_ALGORITHM - gSpeedLeft = leftSpeed; - gSpeedRight = rightSpeed; -#endif /* DEBUG_ALGORITHM */ - - diffDrive.setLinearSpeed(leftSpeed, rightSpeed); -} - -bool DrivingState::isAbortRequired() -{ - bool isAbort = false; - - /* If track is lost over a certain distance, abort driving. */ - if (true == m_isTrackLost) - { - /* Max. distance driven, but track still not found? */ - if (MAX_DISTANCE < Odometry::getInstance().getMileageCenter()) - { - isAbort = true; - } - } - - /* If track is not finished over a certain time, abort driving. */ - if (TRACK_STATUS_FINISHED != m_trackStatus) - { - if (true == m_observationTimer.isTimeout()) - { - isAbort = true; - } - } - - return isAbort; -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ - -#ifdef DEBUG_ALGORITHM - -/** - * Log the titles of each column as CSV data. - * - * @param[in] length Number of line sensors. - */ -static void logCsvDataTitles(uint8_t length) -{ - uint8_t idx = 0; - - printf("Timestamp"); - - while (length > idx) - { - printf(";Sensor %u", idx); - - ++idx; - } - - printf(";Position;Position3;Scenario;Speed Left; Speed Right\n"); -} - -/** - * Log the timestamp as CSV data. - */ -static void logCsvDataTimestamp() -{ - printf("%u;", millis()); -} - -/** - * Log the sensor values and the calculated positions as CSV data. - * - * @param[in] lineSensorValues The array of line sensor values. - * @param[in] length The number of line sensors. - * @param[in] pos The calculated position by all line sensors. - * @param[in] pos3 The calculated position by the inner line sensors. - * @param[in] isPos3Valid Flag to see whether the inner line sensors position is valid or not. - */ -static void logCsvData(const uint16_t* lineSensorValues, uint8_t length, int16_t pos, int16_t pos3, bool isPos3Valid) -{ - uint8_t idx = 0; - - while (length > idx) - { - if (0 < idx) - { - printf(";"); - } - - printf("%u", lineSensorValues[idx]); - - ++idx; - } - - printf(";%d;", pos); - - if (true == isPos3Valid) - { - printf("%d", pos3); - } -} - -/** - * Log the track status as CSV data. - * - * @param[in] trackStatus The track status. - */ -void logCsvDataTrackStatus(DrivingState::TrackStatus trackStatus) -{ - switch (trackStatus) - { - case DrivingState::TRACK_STATUS_NORMAL: - printf(";\"Normal\""); - break; - - case DrivingState::TRACK_STATUS_START_STOP_LINE: - printf(";\"Start-/Stop-line\""); - break; - - case DrivingState::TRACK_STATUS_RIGHT_ANGLE_CURVE_LEFT: - printf(";\"Right angle curve left\""); - break; - - case DrivingState::TRACK_STATUS_RIGHT_ANGLE_CURVE_RIGHT: - printf(";\"Right angle curve right\""); - break; - - case DrivingState::TRACK_STATUS_SHARP_CURVE_LEFT: - printf(";\"Sharp curve left\""); - break; - - case DrivingState::TRACK_STATUS_SHARP_CURVE_RIGHT: - printf(";\"Sharp curve right\""); - break; - - case DrivingState::TRACK_STATUS_SHARP_CURVE_LEFT_TURN: - printf(";\"Sharp curve left turn\""); - break; - - case DrivingState::TRACK_STATUS_SHARP_CURVE_RIGHT_TURN: - printf(";\"Sharp curve right turn\""); - break; - - case DrivingState::TRACK_STATUS_TRACK_LOST_BY_GAP: - printf(";\"Track lost by gap\""); - break; - - case DrivingState::TRACK_STATUS_TRACK_LOST_BY_MANOEUVRE: - printf(";\"Track lost by manoeuvre\""); - break; - - case DrivingState::TRACK_STATUS_FINISHED: - printf(";\"Track finished\""); - break; - - default: - printf(";\"?\""); - break; - } -} - -/** - * Log the motor speed set points as CSV data. - * - * @param[in] leftSpeed Right motor speed set point in steps/s. - * @param[in] rightSpeed Left motor speed set point in step/s. - */ -static void logCsvDataSpeed(int16_t leftSpeed, int16_t rightSpeed) -{ - printf(";%d;%d", leftSpeed, rightSpeed); -} - -#endif /* DEBUG_ALGORITHM */ \ No newline at end of file diff --git a/lib/APPReinforcementLearning/src/DrivingState.h b/lib/APPReinforcementLearning/src/DrivingState.h deleted file mode 100644 index 5a94b5c7..00000000 --- a/lib/APPReinforcementLearning/src/DrivingState.h +++ /dev/null @@ -1,360 +0,0 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Driving state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef DRIVING_STATE_H -#define DRIVING_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The system driving state. */ -class DrivingState : public IState -{ -public: - /** - * Get state instance. - * - * @return State instance. - */ - static DrivingState& getInstance() - { - static DrivingState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - -protected: -private: - /** - * The line detection status. - */ - enum LineStatus - { - LINE_STATUS_NO_START_LINE_DETECTED = 0, /**< No start line detected. */ - LINE_STATUS_START_LINE_DETECTED, /**< Start line detected. */ - LINE_STATUS_STOP_LINE_DETECTED /**< Stop line detected. */ - }; - - /** - * The track status. - */ - enum TrackStatus - { - TRACK_STATUS_NORMAL = 0, /**< Normal line conditions. */ - TRACK_STATUS_START_STOP_LINE, /**< Driving over start-/stop-line. */ - TRACK_STATUS_RIGHT_ANGLE_CURVE_LEFT, /**< Right angle curve to the left detected. */ - TRACK_STATUS_RIGHT_ANGLE_CURVE_RIGHT, /**< Right angle curve to the right detected. */ - TRACK_STATUS_SHARP_CURVE_LEFT, /**< Sharp curve to the left detected. */ - TRACK_STATUS_SHARP_CURVE_RIGHT, /**< Sharp curve to the right detected. */ - TRACK_STATUS_SHARP_CURVE_LEFT_TURN, /**< Sharp curve to the left turns now. */ - TRACK_STATUS_SHARP_CURVE_RIGHT_TURN, /**< Sharp curve to the right turns now. */ - TRACK_STATUS_TRACK_LOST_BY_GAP, /**< Track lost by gap. */ - TRACK_STATUS_TRACK_LOST_BY_MANOEUVRE, /**< Track lost by driving manoeuvre. */ - TRACK_STATUS_FINISHED /**< Robot found the end line or a error happened. */ - }; - - /** Observation duration in ms. This is the max. time within the robot must be finished its drive. */ - static const uint32_t OBSERVATION_DURATION = 3000000; - - /** Max. distance in mm after a lost track must be found again. */ - static const uint32_t MAX_DISTANCE = 200; - - /** Period in ms for PID processing. */ - static const uint32_t PID_PROCESS_PERIOD = 10; - - /** - * The line sensor threshold (normalized) used to detect the track. - * The track is detected in case the received value is greater or equal than - * the threshold. - */ - static const uint16_t LINE_SENSOR_ON_TRACK_MIN_VALUE = 200U; - - /** - * The max. normalized value of a sensor in digits. - */ - static const uint16_t SENSOR_VALUE_MAX; - - /** - * Position set point which is the perfect on track position. - * This is the goal to achieve. - */ - static const int16_t POSITION_SET_POINT; - - /** - * ID of most left sensor. - */ - static const uint8_t SENSOR_ID_MOST_LEFT; - - /** - * ID of most right sensor. - */ - static const uint8_t SENSOR_ID_MIDDLE; - - /** - * ID of middle sensor. - */ - static const uint8_t SENSOR_ID_MOST_RIGHT; - - /** - * Minimum position in digits. - */ - static const int16_t POSITION_MIN; - - /** - * Maximum position in digits. - */ - static const int16_t POSITION_MAX; - - /** - * Lower border position in digits for driving will on the line. - */ - static const int16_t POSITION_MIDDLE_MIN; - - /** - * Higher border position in digits for driving will on the line. - */ - static const int16_t POSITION_MIDDLE_MAX; - - SimpleTimer m_observationTimer; /**< Observation timer to observe the max. time per challenge. */ - SimpleTimer m_lapTime; /**< Timer used to calculate the lap time. */ - SimpleTimer m_pidProcessTime; /**< Timer used for periodically PID processing. */ - PIDController m_pidCtrl; /**< PID controller, used for driving. */ - int16_t m_topSpeed; /**< Top speed in [steps/s]. It might be lower or equal to the max. speed! */ - LineStatus m_lineStatus; /**< Status of start-/end line detection */ - TrackStatus m_trackStatus; /**< Status of track which means on track or track lost, etc. */ - bool m_isStartStopLineDetected; /**< Is the start/stop line detected? */ - uint8_t m_lastSensorIdSawTrack; /**< The sensor id of the sensor which saw the track as last. */ - int16_t m_lastPosition; /**< Last position, used to decide strategy in case of a track gap. */ - bool m_isTrackLost; /**< Is the track lost? Lost means the line sensors didn't detect it. */ - - /** - * Default constructor. - */ - DrivingState(); - - /** - * Default destructor. - */ - ~DrivingState() - { - } - - /** - * Copy construction of an instance. - * Not allowed. - * - * @param[in] state Source instance. - */ - DrivingState(const DrivingState& state); - - /** - * Assignment of an instance. - * Not allowed. - * - * @param[in] state Source instance. - * - * @returns Reference to DrivingState instance. - */ - DrivingState& operator=(const DrivingState& state); - - /** - * Calculate the position with the inner 3 line sensors. - * - * @param[out] position The position result. - * @param[in] lineSensorValues Array of line sensor values. - * @param[in] length Array length. - * - * @return If successful, it will return true otherwise false. - */ - bool calcPosition3(int16_t& position, const uint16_t* lineSensorValues, uint8_t length) const; - - /** - * Evaluate the situation by line sensor values and position and determine - * the track status. The result influences the measures to keep track on - * the line. - * - * @param[in] lineSensorValues The line sensor values as array. - * @param[in] length The number of line sensor values. - * @param[in] position The position calculated with all sensors. - * @param[in] position3 The position calculated with the inner 3 sensors only. - * @param[in] isTrackLost Information whether the track is lost or not. - * - * @return The track status result. - */ - TrackStatus evaluateSituation(const uint16_t* lineSensorValues, uint8_t length, int16_t position, int16_t position3, - bool isTrackLost) const; - - /** - * Is the start/stop line detected? - * - * @param[in] lineSensorValues The line sensor values as array. - * @param[in] length The number of line sensor values. - * - * @return If start/stop line detected, it will return true otherwise false. - */ - bool isStartStopLineDetected(const uint16_t* lineSensorValues, uint8_t length) const; - - /** - * Is no line detected? - * - * @param[in] lineSensorValues The line sensor values as array. - * @param[in] length The number of line sensor values. - * - * @return If no line is detected, it will return true otherwise false. - */ - bool isNoLineDetected(const uint16_t* lineSensorValues, uint8_t length) const; - - /** - * Is right angle curve to the left detected? - * - * @param[in] lineSensorValues The line sensor values as array. - * @param[in] length The number of line sensor values. - * - * @return If right angle curve to the left is detected, it will return true otherwise false. - */ - bool isRightAngleCurveToLeft(const uint16_t* lineSensorValues, uint8_t length) const; - - /** - * Is right angle curve to the right detected? - * - * @param[in] lineSensorValues The line sensor values as array. - * @param[in] length The number of line sensor values. - * - * @return If right angle curve to the right is detected, it will return true otherwise false. - */ - bool isRightAngleCurveToRight(const uint16_t* lineSensorValues, uint8_t length) const; - - /** - * Is a sharp left curve detected? - * - * @param[in] lineSensorValues The line sensor values as array. - * @param[in] length The number of line sensor values. - * @param[in] position3 The position calculated with the inner line sensors in digits. - * - * @return If sharp left curve is detected, it will return true otherwise false. - */ - bool isSharpLeftCurveDetected(const uint16_t* lineSensorValues, uint8_t length, int16_t position3) const; - - /** - * Is a sharp right curve detected? - * - * @param[in] lineSensorValues The line sensor values as array. - * @param[in] length The number of line sensor values. - * @param[in] position3 The position calculated with the inner line sensors in digits. - * - * @return If sharp right curve is detected, it will return true otherwise false. - */ - bool isSharpRightCurveDetected(const uint16_t* lineSensorValues, uint8_t length, int16_t position3) const; - - /** - * Process the situation and decide which measures to take. - * Measures will influence the position or whether its allowed to have - * a negative motor speed. - * - * @param[in, out] position The position calculated with all sensors in digits. - * @param[out] allowNegativeMotorSpeed Allow negative motor speed or not. - * @param[in] trackStatus The evaluated track status. - * @param[in] position3 The position calculated with the inner sensors in digits. - */ - void processSituation(int16_t& position, bool& allowNegativeMotorSpeed, TrackStatus trackStatus, int16_t position3); - - /** - * Adapt driving by using a PID algorithm, depended on the position - * input. - * - * @param[in] position Position in digits - * @param[in] allowNegativeMotorSpeed Allow negative motor speed. - */ - void adaptDriving(int16_t position, bool allowNegativeMotorSpeed); - - /** - * Check the abort conditions while driving the challenge. - * - * @return If abort is required, it will return true otherwise false. - */ - bool isAbortRequired(); - -#ifdef DEBUG_ALGORITHM - /** - * Required for debugging purposes. - * - * @param[in] trackStatus The track status. - */ - friend void logCsvDataTrackStatus(TrackStatus trackStatus); -#endif /* DEBUG_ALGORITHM */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* DRIVING_STATE_H */ -/** @} */ diff --git a/lib/APPReinforcementLearning/src/ErrorState.cpp b/lib/APPReinforcementLearning/src/ErrorState.cpp deleted file mode 100644 index 7ef95773..00000000 --- a/lib/APPReinforcementLearning/src/ErrorState.cpp +++ /dev/null @@ -1,139 +0,0 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Error state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ErrorState.h" -#include -#include -#include "StartupState.h" -#include -#include -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/** - * Error logging tag. - */ -LOG_TAG("EState"); - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void ErrorState::entry() -{ - IBoard& board = Board::getInstance(); - IDisplay& display = board.getDisplay(); - - /* Stop the motors in any case! */ - DifferentialDrive::getInstance().disable(); - - display.clear(); - display.print("A: CONT"); - display.gotoXY(0, 1); - - if ('\0' == m_errorMsg[0]) - { - display.print("ERR"); - } - else - { - display.print(m_errorMsg); - } - - LOG_ERROR_VAL("Error: ", m_errorMsg); -} - -void ErrorState::process(StateMachine& sm) -{ - IButton& buttonA = Board::getInstance().getButtonA(); - - /* Restart calibration? */ - if (true == Util::isButtonTriggered(buttonA, m_isButtonAPressed)) - { - sm.setState(&StartupState::getInstance()); - } -} - -void ErrorState::exit() -{ - /* Nothing to do. */ -} - -void ErrorState::setErrorMsg(const char* msg) -{ - if (nullptr == msg) - { - m_errorMsg[0] = '\0'; - } - else - { - strncpy(m_errorMsg, msg, ERROR_MSG_SIZE - 1); - m_errorMsg[ERROR_MSG_SIZE - 1] = '\0'; - } -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ diff --git a/lib/APPReinforcementLearning/src/ErrorState.h b/lib/APPReinforcementLearning/src/ErrorState.h deleted file mode 100644 index 78215fd3..00000000 --- a/lib/APPReinforcementLearning/src/ErrorState.h +++ /dev/null @@ -1,149 +0,0 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Error state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef ERROR_STATE_H -#define ERROR_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The system error state. */ -class ErrorState : public IState -{ -public: - /** - * Get state instance. - * - * @return State instance. - */ - static ErrorState& getInstance() - { - static ErrorState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - - /** - * Set error message, which to show on the display. - * - * @param[in] msg Error message - */ - void setErrorMsg(const char* msg); - -protected: -private: - /** - * The error message string size in bytes, which - * includes the terminating character. - */ - static const size_t ERROR_MSG_SIZE = 20; - - char m_errorMsg[ERROR_MSG_SIZE]; /**< Error message, which to show. */ - bool m_isButtonAPressed; /**< Is the button A pressed (last time)? */ - - /** - * Default constructor. - */ - ErrorState() : m_errorMsg(), m_isButtonAPressed(false) - { - m_errorMsg[0] = '\0'; - } - - /** - * Default destructor. - */ - ~ErrorState() - { - } - - /** - * Copy construction of an instance. - * Not allowed. - * - * @param[in] state Source instance. - */ - ErrorState(const ErrorState& state); - - /** - * Assignment of an instance. - * Not allowed. - * - * @param[in] state Source instance. - * - * @returns Reference to ErrorState instance. - */ - ErrorState& operator=(const ErrorState& state); -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* ERROR_STATE_H */ -/** @} */ diff --git a/lib/APPReinforcementLearning/src/IBoard.h b/lib/APPReinforcementLearning/src/IBoard.h deleted file mode 100644 index 1ffdf518..00000000 --- a/lib/APPReinforcementLearning/src/IBoard.h +++ /dev/null @@ -1,205 +0,0 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Board interface, which abstracts the physical board - * @author Andreas Merkle - * - * @addtogroup HALInterfaces - * - * @{ - */ - -#ifndef IBOARD_H -#define IBOARD_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#ifdef DEBUG_ODOMETRY -#include -#endif /* DEBUG_ODOMETRY */ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** - * Abstracts the physical board interface. - */ -class IBoard -{ -public: - - /** - * Destroys the board interface. - */ - virtual ~IBoard() - { - } - - /** - * Initialize the hardware. - */ - virtual void init() = 0; - - /** - * Get button A driver. - * - * @return Button A driver. - */ - virtual IButton& getButtonA() = 0; - - /** - * Get button B driver. - * - * @return Button B driver. - */ - virtual IButton& getButtonB() = 0; - - /** - * Get button C driver. - * - * @return Button C driver. - */ - virtual IButton& getButtonC() = 0; - - /** - * Get buzzer driver. - * - * @return Buzzer driver. - */ - virtual IBuzzer& getBuzzer() = 0; - - /** - * Get LCD driver. - * - * @return LCD driver. - */ - virtual IDisplay& getDisplay() = 0; - - /** - * Get encoders driver. - * - * @return Encoders driver. - */ - virtual IEncoders& getEncoders() = 0; - - /** - * Get line sensors driver. - * - * @return Line sensor driver. - */ - virtual ILineSensors& getLineSensors() = 0; - - /** - * Get motor driver. - * - * @return Motor driver. - */ - virtual IMotors& getMotors() = 0; - - /** - * Get red LED driver. - * - * @return Red LED driver. - */ - virtual ILed& getRedLed() = 0; - - /** - * Get yellow LED driver. - * - * @return Yellow LED driver. - */ - virtual ILed& getYellowLed() = 0; - - /** - * Get green LED driver. - * - * @return Green LED driver. - */ - virtual ILed& getGreenLed() = 0; - - /** - * Get the settings. - * - * @return Settings - */ - virtual ISettings& getSettings() = 0; - -#ifdef DEBUG_ODOMETRY - - /** - * Get the sender driver, used to send data to the webots supervisor. - * - * @return Sender driver - */ - virtual ISender& getSender() = 0; - -#endif /* DEBUG_ODOMETRY */ - - /** - * Process actuators and sensors. - */ - virtual void process() = 0; - -protected: - - /** - * Constructs the board interface. - */ - IBoard() - { - } - -private: - -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* IBOARD_H */ -/** @} */ diff --git a/lib/APPReinforcementLearning/src/LineSensorsCalibrationState.cpp b/lib/APPReinforcementLearning/src/LineSensorsCalibrationState.cpp deleted file mode 100644 index 4a498a79..00000000 --- a/lib/APPReinforcementLearning/src/LineSensorsCalibrationState.cpp +++ /dev/null @@ -1,213 +0,0 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Line sensors calibration state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "LineSensorsCalibrationState.h" -#include -#include -#include -#include -#include -#include "ReadyState.h" -#include "ErrorState.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void LineSensorsCalibrationState::entry() -{ - IDisplay& display = Board::getInstance().getDisplay(); - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - Odometry& odometry = Odometry::getInstance(); - ILineSensors& lineSensors = Board::getInstance().getLineSensors(); - - display.clear(); - display.print("Run"); - display.gotoXY(0, 1); - display.print("LCAL"); - - /* Prepare calibration drive. */ - m_calibrationSpeed = diffDrive.getMaxMotorSpeed() / 4; - m_orientation = odometry.getOrientation(); - - /* Mandatory for each new calibration. */ - lineSensors.resetCalibration(); - - /* Wait some time, before starting the calibration drive. */ - m_phase = PHASE_1_WAIT; - m_timer.start(WAIT_TIME); -} - -void LineSensorsCalibrationState::process(StateMachine& sm) -{ - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - - switch (m_phase) - { - case PHASE_1_WAIT: - if (true == m_timer.isTimeout()) - { - m_phase = PHASE_2_TURN_LEFT; - diffDrive.setLinearSpeed(-m_calibrationSpeed, m_calibrationSpeed); - } - break; - - case PHASE_2_TURN_LEFT: - if (true == turnAndCalibrate(CALIB_ANGLE, true)) - { - m_phase = PHASE_3_TURN_RIGHT; - diffDrive.setLinearSpeed(m_calibrationSpeed, -m_calibrationSpeed); - } - break; - - case PHASE_3_TURN_RIGHT: - if (true == turnAndCalibrate(-CALIB_ANGLE, false)) - { - m_phase = PHASE_4_TURN_ORIG; - diffDrive.setLinearSpeed(-m_calibrationSpeed, m_calibrationSpeed); - } - break; - - case PHASE_4_TURN_ORIG: - if (true == turnAndCalibrate(0, true)) - { - m_phase = PHASE_5_FINISHED; - diffDrive.setLinearSpeed(0, 0); - finishCalibration(sm); - } - break; - - case PHASE_5_FINISHED: - /* fallthrough */ - default: - break; - } -} - -void LineSensorsCalibrationState::exit() -{ - m_timer.stop(); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -bool LineSensorsCalibrationState::turnAndCalibrate(int32_t calibAlpha, bool isGreaterEqual) -{ - ILineSensors& lineSensors = Board::getInstance().getLineSensors(); - Odometry& odometry = Odometry::getInstance(); - int32_t alpha = odometry.getOrientation() - m_orientation; /* [mrad] */ - bool isSuccesful = false; - - /* Continously calibrate the line sensors. */ - lineSensors.calibrate(); - - /* Is the goal that the current angle shall be lower or equal than the destination calibration angle? */ - if (false == isGreaterEqual) - { - /* Is alpha lower or equal than the destination calibration angle? */ - if (calibAlpha >= alpha) - { - isSuccesful = true; - } - } - else - { - /* Is alpha greater or equal than the destination calibration angle? */ - if (calibAlpha <= alpha) - { - isSuccesful = true; - } - } - - return isSuccesful; -} - -void LineSensorsCalibrationState::finishCalibration(StateMachine& sm) -{ - ILineSensors& lineSensors = Board::getInstance().getLineSensors(); - - if (false == lineSensors.isCalibrationSuccessful()) - { - char str[10]; - char valueStr[10]; - - Util::uintToStr(valueStr, sizeof(valueStr), lineSensors.getCalibErrorInfo()); - - strncpy(str, "ELCAL ", sizeof(str) - 1); - str[sizeof(str) - 1] = '\0'; - - strncat(str, valueStr, sizeof(str) - strlen(valueStr) - 1); - - ErrorState::getInstance().setErrorMsg(str); - sm.setState(&ErrorState::getInstance()); - } - else - { - sm.setState(&ReadyState::getInstance()); - } -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ diff --git a/lib/APPReinforcementLearning/src/LineSensorsCalibrationState.h b/lib/APPReinforcementLearning/src/LineSensorsCalibrationState.h deleted file mode 100644 index 3569fe72..00000000 --- a/lib/APPReinforcementLearning/src/LineSensorsCalibrationState.h +++ /dev/null @@ -1,175 +0,0 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Line sensors calibration state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef LINE_SENSORS_CALIBRATION_STATE_H -#define LINE_SENSORS_CALIBRATION_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The line sensors calibration state. */ -class LineSensorsCalibrationState : public IState -{ -public: - /** - * Get state instance. - * - * @return State instance. - */ - static LineSensorsCalibrationState& getInstance() - { - static LineSensorsCalibrationState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - -protected: -private: - /** Calibration phases */ - enum Phase - { - PHASE_1_WAIT = 0, /**< Wait a short time before starting the calibration. */ - PHASE_2_TURN_LEFT, /**< Turn line sensors left over the line. */ - PHASE_3_TURN_RIGHT, /**< Turn line sensor right over the line. */ - PHASE_4_TURN_ORIG, /**< Turn back to origin. */ - PHASE_5_FINISHED /**< Calibration is finished. */ - }; - - /** - * Duration in ms about to wait, until the calibration drive starts. - */ - static const uint32_t WAIT_TIME = 1000; - - /** - * Calibration turn angle in mrad (corresponds to 72°). - */ - static const int32_t CALIB_ANGLE = (FP_2PI() / 5); - - SimpleTimer m_timer; /**< Timer used to wait, until the calibration drive starts. */ - Phase m_phase; /**< Current calibration phase */ - int16_t m_calibrationSpeed; /**< Speed in steps/s for the calibration drive. */ - int32_t m_orientation; /**< Orientation at the begin of the calibration in [mrad]. */ - - /** - * Default constructor. - */ - LineSensorsCalibrationState() : m_timer(), m_phase(PHASE_1_WAIT), m_calibrationSpeed(0), m_orientation(0) - { - } - - /** - * Default destructor. - */ - ~LineSensorsCalibrationState() - { - } - - /** - * Copy construction of an instance. - * Not allowed. - * - * @param[in] state Source instance. - */ - LineSensorsCalibrationState(const LineSensorsCalibrationState& state); - - /** - * Assignment of an instance. - * Not allowed. - * - * @param[in] state Source instance. - * - * @returns Reference to LineSensorsCalibrationState. - */ - LineSensorsCalibrationState& operator=(const LineSensorsCalibrationState& state); - - /** - * Turn and calibrate the line sensors. - * - * @param[in] calibAlpha Destination calibration angle in [mrad] - * @param[in] isGreaterEqual Configure true if angle shall be greater or equal than the destination. - * - * @return If destination angle reached, it will return true otherwise false. - */ - bool turnAndCalibrate(int32_t calibAlpha, bool isGreaterEqual); - - /** - * Finish the calibration and determine next state. - * - * @param[in] sm State machine - */ - void finishCalibration(StateMachine& sm); -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* LINE_SENSORS_CALIBRATION_STATE_H */ -/** @} */ diff --git a/lib/APPReinforcementLearning/src/MotorSpeedCalibrationState.cpp b/lib/APPReinforcementLearning/src/MotorSpeedCalibrationState.cpp deleted file mode 100644 index f9547662..00000000 --- a/lib/APPReinforcementLearning/src/MotorSpeedCalibrationState.cpp +++ /dev/null @@ -1,245 +0,0 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Motor speed calibration state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "MotorSpeedCalibrationState.h" -#include -#include -#include -#include -#include -#include -#include "LineSensorsCalibrationState.h" -#include "ErrorState.h" -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/** - * Logging source. - */ -LOG_TAG("MSCState"); - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void MotorSpeedCalibrationState::entry() -{ - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - IDisplay& display = Board::getInstance().getDisplay(); - - display.clear(); - display.print("Run"); - display.gotoXY(0, 1); - display.print("MCAL"); - - /* Disable differential drive to avoid any bad influence. */ - diffDrive.disable(); - - /* Setup relative encoders */ - m_relEncoders.clear(); - - /* Set the max. speeds to maximum of datatype, because during calibration - * the max. speed is determined by the lowest motor speed. - */ - m_maxSpeedLeft = INT16_MAX; - m_maxSpeedRight = INT16_MAX; - - /* Wait some time, before starting the calibration drive. */ - m_phase = PHASE_1_BACK; - m_timer.start(WAIT_TIME); -} - -void MotorSpeedCalibrationState::process(StateMachine& sm) -{ - if (true == m_timer.isTimeout()) - { - /* Control motors directly and not via differential drive control, - * because the differential drive control needs first to be updated - * regarding the max. possible motor speed in [steps/s] which is - * determined by this calibration. - */ - IMotors& motors = Board::getInstance().getMotors(); - - switch (m_phase) - { - case PHASE_1_BACK: - /* Drive full back. */ - motors.setSpeeds(-motors.getMaxSpeed(), -motors.getMaxSpeed()); - - m_timer.start(CALIB_DURATION); - m_phase = PHASE_2_FORWARD; - break; - - case PHASE_2_FORWARD: - motors.setSpeeds(0, 0); - determineMaxMotorSpeed(); - - /* Drive full forward. */ - motors.setSpeeds(motors.getMaxSpeed(), motors.getMaxSpeed()); - - m_timer.restart(); - m_phase = PHASE_3_FINISHED; - break; - - case PHASE_3_FINISHED: - motors.setSpeeds(0, 0); - determineMaxMotorSpeed(); - - m_timer.stop(); - finishCalibration(sm); - break; - - default: - break; - } - } -} - -void MotorSpeedCalibrationState::exit() -{ - /* Nothing to do. */ -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -void MotorSpeedCalibrationState::determineMaxMotorSpeed() -{ - int32_t stepsLeft = 0; - int32_t stepsRight = 0; - - /* Determine max. speed backward. */ - stepsLeft = abs(m_relEncoders.getCountsLeft()); - stepsRight = abs(m_relEncoders.getCountsRight()); - - /* Convert number of steps to [steps/s] */ - stepsLeft *= 1000; - stepsLeft /= CALIB_DURATION; - stepsRight *= 1000; - stepsRight /= CALIB_DURATION; - - if (INT16_MAX >= stepsLeft) - { - /* Use lower speed to ensure that motor speed can be reached in both - * directions. - */ - if (stepsLeft < m_maxSpeedLeft) - { - m_maxSpeedLeft = static_cast(stepsLeft); - } - } - - if (INT16_MAX >= stepsRight) - { - /* Use lower speed to ensure that motor speed can be reached in both - * directions. - */ - if (stepsRight < m_maxSpeedRight) - { - m_maxSpeedRight = static_cast(stepsRight); - } - } - - /* Clear relative encoders */ - m_relEncoders.clear(); -} - -void MotorSpeedCalibrationState::finishCalibration(StateMachine& sm) -{ - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - ISettings& settings = Board::getInstance().getSettings(); - - /* Set the lower speed as max. motor speed to ensure that both motors - * can reach the same max. speed. - */ - int16_t maxSpeed = (m_maxSpeedLeft < m_maxSpeedRight) ? m_maxSpeedLeft : m_maxSpeedRight; - - /* Store calibrated max. motor speed in the settings. */ - settings.setMaxSpeed(maxSpeed); - - /* With setting the max. motor speed in [steps/s] the differential drive control - * can now be used. - */ - diffDrive.setMaxMotorSpeed(maxSpeed); - - /* Differential drive can now be used. */ - diffDrive.enable(); - - if (0 == maxSpeed) - { - ErrorState::getInstance().setErrorMsg("EMCAL 0"); - sm.setState(&ErrorState::getInstance()); - } - else - { - int32_t maxSpeed32 = static_cast(maxSpeed) * 1000 / static_cast(RobotConstants::ENCODER_STEPS_PER_M); - - LOG_INFO_VAL("Calibrated max. speed (steps/s): ", maxSpeed); - LOG_INFO_VAL("Calibrated max. speed (mm/s): ", maxSpeed32); - - sm.setState(&LineSensorsCalibrationState::getInstance()); - } -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ diff --git a/lib/APPReinforcementLearning/src/MotorSpeedCalibrationState.h b/lib/APPReinforcementLearning/src/MotorSpeedCalibrationState.h deleted file mode 100644 index 33e820aa..00000000 --- a/lib/APPReinforcementLearning/src/MotorSpeedCalibrationState.h +++ /dev/null @@ -1,179 +0,0 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Motor speed calibration state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef MOTOR_SPEED_CALIBRATION_STATE_H -#define MOTOR_SPEED_CALIBRATION_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The motor speed calibration state. */ -class MotorSpeedCalibrationState : public IState -{ -public: - /** - * Get state instance. - * - * @return State instance. - */ - static MotorSpeedCalibrationState& getInstance() - { - static MotorSpeedCalibrationState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - -protected: -private: - /** Calibration phases */ - enum Phase - { - PHASE_1_BACK, /**< Drive with max. speed backwards. */ - PHASE_2_FORWARD, /**< Drive with max. speed forwards. */ - PHASE_3_FINISHED /**< Calibration is finished. */ - }; - - /** - * Duration in ms about to wait, until the calibration drive starts. - */ - static const uint32_t WAIT_TIME = 1000; - - /** - * Calibration drive duration in ms. - * It means how long the robot is driven with max. speed forward/backward. - */ - static const uint32_t CALIB_DURATION = 1000; - - SimpleTimer m_timer; /**< Timer used to wait, until the calibration drive starts and for drive duration. */ - Phase m_phase; /**< Current calibration phase */ - int16_t m_maxSpeedLeft; /**< Max. determined left motor speed [steps/s]. */ - int16_t m_maxSpeedRight; /**< Max. determined right motor speed [steps/s]. */ - RelativeEncoders m_relEncoders; /**< Relative encoders left/right. */ - - /** - * Default constructor. - */ - MotorSpeedCalibrationState() : - m_timer(), - m_phase(PHASE_1_BACK), - m_maxSpeedLeft(0), - m_maxSpeedRight(0), - m_relEncoders(Board::getInstance().getEncoders()) - { - } - - /** - * Default destructor. - */ - ~MotorSpeedCalibrationState() - { - } - - /** - * Copy construction of an instance. - * Not allowed. - * - * @param[in] state Source instance. - */ - MotorSpeedCalibrationState(const MotorSpeedCalibrationState& state); - - /** - * Assignment of an instance. - * Not allowed. - * - * @param[in] state Source instance. - * - * @returns Reference to MotorSpeedCalibrationStateinstance. - */ - MotorSpeedCalibrationState& operator=(const MotorSpeedCalibrationState& state); - - /** - * Determine the max. motor speed, considering both driving directions. - * There are two steps necessary: - * - Drive full backward and call this method to determine. - * - Drive full forward and call this method to determine. - */ - void determineMaxMotorSpeed(); - - /** - * Finish the calibration and determine next state. - * - * @param[in] sm State machine - */ - void finishCalibration(StateMachine& sm); -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* MOTOR_SPEED_CALIBRATION_STATE_H */ -/** @} */ diff --git a/lib/APPReinforcementLearning/src/ParameterSets.cpp b/lib/APPReinforcementLearning/src/ParameterSets.cpp deleted file mode 100644 index 050d8501..00000000 --- a/lib/APPReinforcementLearning/src/ParameterSets.cpp +++ /dev/null @@ -1,150 +0,0 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Calibration state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ParameterSets.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void ParameterSets::choose(uint8_t setId) -{ - if (MAX_SETS > setId) - { - m_currentSetId = setId; - } -} - -void ParameterSets::next() -{ - ++m_currentSetId; - m_currentSetId %= MAX_SETS; -} - -uint8_t ParameterSets::getCurrentSetId() const -{ - return m_currentSetId; -} - -const ParameterSets::ParameterSet& ParameterSets::getParameterSet() const -{ - return m_parSets[m_currentSetId]; -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -ParameterSets::ParameterSets() : m_currentSetId(0), m_parSets() -{ - m_parSets[0] = { - "PD VF", /* Name - VF: very fast */ - 4000, /* Top speed in steps/s */ - 4, /* Kp Numerator */ - 1, /* Kp Denominator */ - 0, /* Ki Numerator */ - 1, /* Ki Denominator */ - 60, /* Kd Numerator */ - 1 /* Kd Denominator */ - }; - - m_parSets[1] = { - "PD F", /* Name - F: fast */ - 3000, /* Top speed in steps/s */ - 2, /* Kp Numerator */ - 1, /* Kp Denominator */ - 0, /* Ki Numerator */ - 1, /* Ki Denominator */ - 50, /* Kd Numerator */ - 1 /* Kd Denominator */ - }; - - m_parSets[2] = { - "PD S", /* Name - S: slow */ - 2000, /* Top speed in steps/s */ - 2, /* Kp Numerator */ - 1, /* Kp Denominator */ - 0, /* Ki Numerator */ - 1, /* Ki Denominator */ - 40, /* Kd Numerator */ - 1 /* Kd Denominator */ - }; - - m_parSets[3] = { - "PD VS", /* Name - VS: very slow */ - 1000, /* Top speed in steps/s */ - 3, /* Kp Numerator */ - 2, /* Kp Denominator */ - 0, /* Ki Numerator */ - 1, /* Ki Denominator */ - 30, /* Kd Numerator */ - 1 /* Kd Denominator */ - }; -} - -ParameterSets::~ParameterSets() -{ -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ diff --git a/lib/APPReinforcementLearning/src/ParameterSets.h b/lib/APPReinforcementLearning/src/ParameterSets.h deleted file mode 100644 index 56fd9b81..00000000 --- a/lib/APPReinforcementLearning/src/ParameterSets.h +++ /dev/null @@ -1,159 +0,0 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Parameter state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef PARAMETER_SETS_H -#define PARAMETER_SETS_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** Parameter set with different driving configurations. */ -class ParameterSets -{ -public: - /** - * A single parameter set. - */ - struct ParameterSet - { - const char* name; /**< Name of the parameter set */ - int16_t topSpeed; /**< Top speed in steps/s */ - int16_t kPNumerator; /**< Kp numerator value */ - int16_t kPDenominator; /**< Kp denominator value */ - int16_t kINumerator; /**< Ki numerator value */ - int16_t kIDenominator; /**< Ki denominator value */ - int16_t kDNumerator; /**< Kd numerator value */ - int16_t kDDenominator; /**< Kd denominator value */ - }; - - /** - * Get parameter set instance. - * - * @return Parameter set instance. - */ - static ParameterSets& getInstance() - { - static ParameterSets instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * Choose a specific parameter set. - * If a invalid set id is given, nothing changes. - * - * @param[in] setId Parameter set id - */ - void choose(uint8_t setId); - - /** - * Change to next parameter set. - * After the last set, the first will be choosen. - */ - void next(); - - /** - * Get current set id. - * - * @return Parameter set id. - */ - uint8_t getCurrentSetId() const; - - /** - * Get selected parameter set. - * - * @return Parameter set - */ - const ParameterSet& getParameterSet() const; - - /** Max. number of parameter sets. */ - static const uint8_t MAX_SETS = 4U; - -protected: -private: - uint8_t m_currentSetId; /**< Set id of current selected set. */ - ParameterSet m_parSets[MAX_SETS]; /**< All parameter sets */ - - /** - * Default constructor. - */ - ParameterSets(); - - /** - * Default destructor. - */ - ~ParameterSets(); - - /** - * Copy construction of an instance. - * Not allowed. - * - * @param[in] set Source instance. - */ - ParameterSets(const ParameterSets& set); - - /** - * Assignment of an instance. - * Not allowed. - * - * @param[in] set Source instance. - * - * @returns Reference to ParameterSets. - */ - ParameterSets& operator=(const ParameterSets& set); -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* PARAMETER_SET_H */ -/** @} */ diff --git a/lib/APPReinforcementLearning/src/ReadyState.cpp b/lib/APPReinforcementLearning/src/ReadyState.cpp deleted file mode 100644 index 0e29141e..00000000 --- a/lib/APPReinforcementLearning/src/ReadyState.cpp +++ /dev/null @@ -1,168 +0,0 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Ready state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ReadyState.h" -#include -#include -#include "ReleaseTrackState.h" -#include -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/** - * Logging source. - */ -LOG_TAG("RState"); - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void ReadyState::entry() -{ - IDisplay& display = Board::getInstance().getDisplay(); - const int32_t SENSOR_VALUE_OUT_PERIOD = 1000; /* ms */ - - display.clear(); - display.print("A: Go"); - - if (true == m_isLapTimeAvailable) - { - display.gotoXY(0, 1); - display.print(m_lapTime); - display.print("ms"); - - LOG_INFO_VAL("Lap time: ", m_lapTime); - } - - /* The line sensor value shall be output on console cyclic. */ - m_timer.start(SENSOR_VALUE_OUT_PERIOD); -} - -void ReadyState::process(StateMachine& sm) -{ - IButton& buttonA = Board::getInstance().getButtonA(); - - /* Shall track be released? */ - if (true == Util::isButtonTriggered(buttonA, m_isButtonAPressed)) - { - sm.setState(&ReleaseTrackState::getInstance()); - } - - /* Shall the line sensor values be printed out on console? */ - if (true == m_timer.isTimeout()) - { - ILineSensors& lineSensors = Board::getInstance().getLineSensors(); - uint8_t index = 0; - int16_t position = lineSensors.readLine(); - const uint16_t* sensorValues = lineSensors.getSensorValues(); - char valueStr[10]; - - LOG_DEBUG_HEAD(); - - /* Print line sensor value on console for debug purposes. */ - for (index = 0; index < lineSensors.getNumLineSensors(); ++index) - { - if (0 < index) - { - LOG_DEBUG_MSG(" / "); - } - - Util::uintToStr(valueStr, sizeof(valueStr), sensorValues[index]); - - LOG_DEBUG_MSG(valueStr); - } - - LOG_DEBUG_MSG(" -> "); - - Util::intToStr(valueStr, sizeof(valueStr), position); - LOG_DEBUG_MSG(valueStr); - - LOG_DEBUG_TAIL(); - - m_timer.restart(); - } - else - { - /* Nothing to do. */ - ; - } -} - -void ReadyState::exit() -{ - m_timer.stop(); - m_isLapTimeAvailable = false; -} - -void ReadyState::setLapTime(uint32_t lapTime) -{ - m_isLapTimeAvailable = true; - m_lapTime = lapTime; -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ diff --git a/lib/APPReinforcementLearning/src/ReadyState.h b/lib/APPReinforcementLearning/src/ReadyState.h deleted file mode 100644 index ce1a8049..00000000 --- a/lib/APPReinforcementLearning/src/ReadyState.h +++ /dev/null @@ -1,144 +0,0 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Ready state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef READY_STATE_H -#define READY_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The system ready state. */ -class ReadyState : public IState -{ -public: - /** - * Get state instance. - * - * @return State instance. - */ - static ReadyState& getInstance() - { - static ReadyState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - - /** - * Set lap time, which to show on the display. - * - * @param[in] lapTime Lap time in ms - */ - void setLapTime(uint32_t lapTime); - -protected: -private: - SimpleTimer m_timer; /**< Timer used for cyclic debug output. */ - bool m_isLapTimeAvailable; /**< Is set (true), if a lap time is available. */ - uint32_t m_lapTime; /**< Lap time in ms of the last successful driven round. */ - bool m_isButtonAPressed; /**< Is the button A pressed (last time)? */ - - /** - * Default constructor. - */ - ReadyState() : m_timer(), m_isLapTimeAvailable(false), m_lapTime(0), m_isButtonAPressed(false) - { - } - - /** - * Default destructor. - */ - ~ReadyState() - { - } - - /** - * Copy construction of an instance. - * Not allowed. - * - * @param[in] state Source instance. - */ - ReadyState(const ReadyState& state); - - /** - * Assignment of an instance. - * Not allowed. - * - * @param[in] state Source instance. - * - * @returns Reference to ErrorState instance. - */ - ReadyState& operator=(const ReadyState& state); -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* READY_STATE_H */ -/** @} */ diff --git a/lib/APPReinforcementLearning/src/ReleaseTrackState.cpp b/lib/APPReinforcementLearning/src/ReleaseTrackState.cpp deleted file mode 100644 index e520e634..00000000 --- a/lib/APPReinforcementLearning/src/ReleaseTrackState.cpp +++ /dev/null @@ -1,129 +0,0 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Release track state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ReleaseTrackState.h" -#include -#include -#include "DrivingState.h" -#include "ParameterSets.h" -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void ReleaseTrackState::entry() -{ - /* Start challenge after specific time. */ - m_releaseTimer.start(TRACK_RELEASE_DURATION); - - /* Choose parameter set 0 by default. */ - ParameterSets::getInstance().choose(0); - showParSet(); -} - -void ReleaseTrackState::process(StateMachine& sm) -{ - IButton& buttonA = Board::getInstance().getButtonA(); - - /* Change parameter set? */ - if (true == Util::isButtonTriggered(buttonA, m_isButtonAPressed)) - { - /* Choose next parameter set (round-robin). */ - ParameterSets::getInstance().next(); - showParSet(); - - m_releaseTimer.restart(); - } - - /* Release track after specific time. */ - if (true == m_releaseTimer.isTimeout()) - { - sm.setState(&DrivingState::getInstance()); - } -} - -void ReleaseTrackState::exit() -{ - m_releaseTimer.stop(); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -void ReleaseTrackState::showParSet() const -{ - IDisplay& display = Board::getInstance().getDisplay(); - uint8_t parSetId = ParameterSets::getInstance().getCurrentSetId(); - const char* parSetName = ParameterSets::getInstance().getParameterSet().name; - - display.clear(); - display.print("A: CHG SET"); - display.gotoXY(0, 1); - display.print(parSetName); - display.print(parSetId); -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ diff --git a/lib/APPReinforcementLearning/src/ReleaseTrackState.h b/lib/APPReinforcementLearning/src/ReleaseTrackState.h deleted file mode 100644 index 41c5d892..00000000 --- a/lib/APPReinforcementLearning/src/ReleaseTrackState.h +++ /dev/null @@ -1,144 +0,0 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Release track state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef RELEASE_TRACK_STATE_H -#define RELEASE_TRACK_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The system release track state. */ -class ReleaseTrackState : public IState -{ -public: - /** - * Get state instance. - * - * @return State instance. - */ - static ReleaseTrackState& getInstance() - { - static ReleaseTrackState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - -protected: -private: - /** Track release timer duration in ms. */ - static const uint32_t TRACK_RELEASE_DURATION = 5000; - - SimpleTimer m_releaseTimer; /**< Track release timer */ - bool m_isButtonAPressed; /**< Is the button A pressed (last time)? */ - - /** - * Default constructor. - */ - ReleaseTrackState() : m_releaseTimer(), m_isButtonAPressed(false) - { - } - - /** - * Default destructor. - */ - ~ReleaseTrackState() - { - } - - /** - * Copy construction of an instance. - * Not allowed. - * - * @param[in] state Source instance. - */ - ReleaseTrackState(const ReleaseTrackState& state); - - /** - * Assignment of an instance. - * Not allowed. - * - * @param[in] state Source instance. - * - * @returns Reference to ReleaseTrackState. - */ - ReleaseTrackState& operator=(const ReleaseTrackState& state); - - /** - * Show choosen parameter set on LCD. - */ - void showParSet() const; -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* RELEASE_TRACK_STATE_H */ -/** @} */ diff --git a/lib/APPReinforcementLearning/src/StartupState.cpp b/lib/APPReinforcementLearning/src/StartupState.cpp deleted file mode 100644 index fbe9f82d..00000000 --- a/lib/APPReinforcementLearning/src/StartupState.cpp +++ /dev/null @@ -1,200 +0,0 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Startup state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "StartupState.h" -#include -#include -#include "MotorSpeedCalibrationState.h" -#include "LineSensorsCalibrationState.h" -#include "Sound.h" -#include -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void StartupState::entry() -{ - Board& board = Board::getInstance(); - ISettings& settings = board.getSettings(); - int16_t maxMotorSpeed = settings.getMaxSpeed(); - - /* If max. motor speed calibration value is available, the differential - * drive will be enabled. - */ - if (0 < maxMotorSpeed) - { - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - - /* With setting the max. motor speed in [steps/s] the differential drive control - * can now be used. - */ - diffDrive.setMaxMotorSpeed(maxMotorSpeed); - - /* Differential drive can now be used. */ - diffDrive.enable(); - - m_isMaxMotorSpeedCalibAvailable = true; - } - - showUserInfo(m_userInfoState); -} - -void StartupState::process(StateMachine& sm) -{ - Board& board = Board::getInstance(); - IButton& buttonA = board.getButtonA(); - - /* Start line sensor calibration? */ - if (true == Util::isButtonTriggered(buttonA, m_isButtonAPressed)) - { - sm.setState(&LineSensorsCalibrationState::getInstance()); - } - - /* If the max. motor speed calibration is done, it will be possible to - * start the line sensor calibration immediately. - */ - if (true == m_isMaxMotorSpeedCalibAvailable) - { - IButton& buttonB = board.getButtonB(); - - /* Start line sensor calibration? */ - if (true == Util::isButtonTriggered(buttonB, m_isButtonBPressed)) - { - sm.setState(&LineSensorsCalibrationState::getInstance()); - } - } - - /* Periodically change the user info on the display. */ - if (true == m_timer.isTimeout()) - { - int8_t next = m_userInfoState + 1; - - if (USER_INFO_COUNT <= next) - { - next = 0; - } - - showUserInfo(static_cast(next)); - m_timer.restart(); - } -} - -void StartupState::exit() -{ - /* Next time start again from begin with the info. */ - m_userInfoState = USER_INFO_TEAM_NAME; -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -void StartupState::showUserInfo(UserInfo next) -{ - Board& board = Board::getInstance(); - IDisplay& display = board.getDisplay(); - ISettings& settings = board.getSettings(); - - display.clear(); - - switch (next) - { - case USER_INFO_TEAM_NAME: - display.print(TEAM_NAME_LINE_1); - display.gotoXY(0, 1); - display.print(TEAM_NAME_LINE_2); - break; - - case USER_INFO_MAX_MOTOR_SPEED: - display.print("maxSpeed"); - display.gotoXY(0, 1); - display.print(settings.getMaxSpeed()); - break; - - case USER_INFO_UI: - display.print("A: MCAL"); - - if (true == m_isMaxMotorSpeedCalibAvailable) - { - display.gotoXY(0, 1); - display.print("B: LCAL"); - } - break; - - case USER_INFO_COUNT: - /* fallthrough */ - default: - display.print("?"); - next = USER_INFO_TEAM_NAME; - break; - } - - m_userInfoState = next; - - m_timer.start(INFO_DURATION); -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ diff --git a/lib/APPReinforcementLearning/src/StartupState.h b/lib/APPReinforcementLearning/src/StartupState.h deleted file mode 100644 index bc3d8000..00000000 --- a/lib/APPReinforcementLearning/src/StartupState.h +++ /dev/null @@ -1,169 +0,0 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Startup state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef STARTUP_STATE_H -#define STARTUP_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The system startup state. */ -class StartupState : public IState -{ -public: - /** - * Get state instance. - * - * @return State instance. - */ - static StartupState& getInstance() - { - static StartupState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - -protected: -private: - /** - * This type defines different kind of information, which will be shown - * to the user in the same order as defined. - */ - enum UserInfo - { - USER_INFO_TEAM_NAME = 0, /**< Show the team name. */ - USER_INFO_MAX_MOTOR_SPEED, /**< Show the max. motor speed. */ - USER_INFO_UI, /**< Show the user interface. */ - USER_INFO_COUNT /**< Number of user infos. */ - }; - - /** - * Duration in ms how long a info on the display shall be shown, until - * the next info appears. - */ - static const uint32_t INFO_DURATION = 2000; - - bool m_isMaxMotorSpeedCalibAvailable; /**< Is max. motor speed calibration value available? */ - SimpleTimer m_timer; /**< Used to show information for a certain time before changing to the next info. */ - UserInfo m_userInfoState; /**< Current user info state. */ - bool m_isButtonAPressed; /**< Is the button A pressed (last time)? */ - bool m_isButtonBPressed; /**< Is the button B pressed (last time)? */ - - /** - * Default constructor. - */ - StartupState() : - m_isMaxMotorSpeedCalibAvailable(false), - m_timer(), - m_userInfoState(USER_INFO_TEAM_NAME), - m_isButtonAPressed(false), - m_isButtonBPressed(false) - { - } - - /** - * Default destructor. - */ - ~StartupState() - { - } - - /** - * Copy construction of an instance. - * Not allowed. - * - * @param[in] state Source instance. - */ - StartupState(const StartupState& state); - - /** - * Assignment of an instance. - * Not allowed. - * - * @param[in] state Source instance. - * - * @returns Reference to StartupState instance. - */ - StartupState& operator=(const StartupState& state); - - /** - * Show next user info. - * - * @param[in] next Next user info which to show. - */ - void showUserInfo(UserInfo next); -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* STARTUP_STATE_H */ -/** @} */ From 2afc924d4b78955f6c0165eef462478ff43b785d Mon Sep 17 00:00:00 2001 From: Akram Date: Fri, 26 Jul 2024 11:27:12 +0200 Subject: [PATCH 24/58] Move cpp and header files to src folder / replace STATUS channel to receive commands with CMD channel --- lib/APPReinforcementLearning/src/App.cpp | 273 ++++++++++ lib/APPReinforcementLearning/{ => src}/App.h | 295 ++++++----- .../{ => src}/DrivingState.cpp | 239 ++++----- .../{ => src}/DrivingState.h | 288 +++++----- .../{ => src}/ErrorState.cpp | 261 +++++----- .../{ => src}/ErrorState.h | 269 +++++----- .../{ => src}/IBoard.h | 410 +++++++-------- .../{ => src}/LineSensorsCalibrationState.cpp | 428 +++++++-------- .../{ => src}/LineSensorsCalibrationState.h | 322 ++++++------ .../{ => src}/MotorSpeedCalibrationState.h | 330 ++++++------ .../{ => src}/MotorSpeedCalibrationstate.cpp | 490 +++++++++--------- .../{ => src}/ReadyState.cpp | 373 ++++++------- .../{ => src}/ReadyState.h | 427 +++++++-------- .../{ => src}/SerialMuxchannels.h | 285 +++++----- .../{ => src}/StartupState.cpp | 396 +++++++------- .../{ => src}/StartupState.h | 297 +++++------ 16 files changed, 2840 insertions(+), 2543 deletions(-) create mode 100644 lib/APPReinforcementLearning/src/App.cpp rename lib/APPReinforcementLearning/{ => src}/App.h (87%) rename lib/APPReinforcementLearning/{ => src}/DrivingState.cpp (97%) rename lib/APPReinforcementLearning/{ => src}/DrivingState.h (96%) rename lib/APPReinforcementLearning/{ => src}/ErrorState.cpp (95%) rename lib/APPReinforcementLearning/{ => src}/ErrorState.h (95%) rename lib/APPReinforcementLearning/{ => src}/IBoard.h (95%) rename lib/APPReinforcementLearning/{ => src}/LineSensorsCalibrationState.cpp (97%) rename lib/APPReinforcementLearning/{ => src}/LineSensorsCalibrationState.h (97%) rename lib/APPReinforcementLearning/{ => src}/MotorSpeedCalibrationState.h (97%) rename lib/APPReinforcementLearning/{ => src}/MotorSpeedCalibrationstate.cpp (97%) rename lib/APPReinforcementLearning/{ => src}/ReadyState.cpp (89%) rename lib/APPReinforcementLearning/{ => src}/ReadyState.h (92%) rename lib/APPReinforcementLearning/{ => src}/SerialMuxchannels.h (87%) rename lib/APPReinforcementLearning/{ => src}/StartupState.cpp (94%) rename lib/APPReinforcementLearning/{ => src}/StartupState.h (90%) diff --git a/lib/APPReinforcementLearning/src/App.cpp b/lib/APPReinforcementLearning/src/App.cpp new file mode 100644 index 00000000..77ce9b60 --- /dev/null +++ b/lib/APPReinforcementLearning/src/App.cpp @@ -0,0 +1,273 @@ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief LineFollower application with Reinforcement Learning + * @author Akram Bziouech + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "App.h" +#include "DrivingState.h" +#include "StartupState.h" +#include +#include +#include +#include "ErrorState.h" +#include +#include +#include +#include +#include +#include +#include "TrainingWaitState.h" +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + + +/****************************************************************************** + * Prototypes + *****************************************************************************/ +static void App_motorSpeedSetpointsChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData); +static void App_cmdChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData); + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + + +void App::setup() +{ + Serial.begin(SERIAL_BAUDRATE); + /* Initialize HAL */ + Board::getInstance().init(); + Logging::disable(); + if (false == setupSerialMuxProt()) + { + ErrorState::getInstance().setErrorMsg("SMP=0"); + m_systemStateMachine.setState(&ErrorState::getInstance()); + } + else + { + /* Setup the state machine with the first state. */ + m_systemStateMachine.setState(&StartupState::getInstance()); + m_statusTimer.start(SEND_STATUS_TIMER_INTERVAL); + m_sendLineSensorsDataInterval.start(SEND_LINE_SENSORS_DATA_PERIOD); + /* Setup the periodically processing of robot control. */ + m_controlInterval.start(DIFFERENTIAL_DRIVE_CONTROL_PERIOD); + } + +#ifdef DEBUG_ODOMETRY + /* Reset supervisor which set its observed position and orientation to 0. */ + Board::getInstance().getSender().send("RST"); +#endif /* DEBUG_ODOMETRY */ + + /* Surprise the audience. */ + Sound::playMelody(Sound::MELODY_WELCOME); +} + +void App::loop() +{ + Board::getInstance().process(); + Speedometer::getInstance().process(); + + if (true == m_controlInterval.isTimeout()) + { + /* The differential drive control needs the measured speed of the + * left and right wheel. Therefore it shall be processed after + * the speedometer. + */ + DifferentialDrive::getInstance().process(DIFFERENTIAL_DRIVE_CONTROL_PERIOD); + + /* The odometry unit needs to detect motor speed changes to be able to + * calculate correct values. Therefore it shall be processed right after + * the differential drive control. + */ + Odometry::getInstance().process(); + +#ifdef DEBUG_ODOMETRY + { + Odometry& odo = Odometry::getInstance(); + int32_t posX = 0; + int32_t posY = 0; + int32_t orientation = odo.getOrientation(); + const size_t BUFFER_SIZE = 128U; + char buffer[BUFFER_SIZE]; + + odo.getPosition(posX, posY); + + snprintf(buffer, BUFFER_SIZE, "ODO,%d,%d,%d", posX, posY, orientation); + + Board::getInstance().getSender().send(buffer); + } +#endif /* DEBUG_ODOMETRY */ + + m_controlInterval.restart(); + } + + if ((true == m_statusTimer.isTimeout()) && (true == m_smpServer.isSynced())&& (&DrivingState::getInstance() == m_systemStateMachine.getState())) + { + Status payload = {SMPChannelPayload::Status::NOT_DONE}; + + if (DrivingState::getInstance().isAbortRequired() == true) + { + payload = {SMPChannelPayload::Status::DONE}; + } + (void)m_smpServer.sendData(m_serialMuxProtChannelIdStatus, &payload, sizeof(payload)); + m_statusTimer.restart(); + } + + /* Send periodically line sensor data. */ + if (true == m_sendLineSensorsDataInterval.isTimeout() && (&DrivingState::getInstance() == m_systemStateMachine.getState()) ) + { + sendLineSensorsData(); + + m_sendLineSensorsDataInterval.restart(); + } + + /* Send Mode selected to The Supervisor. */ + if (&ReadyState::getInstance() == m_systemStateMachine.getState() && (!m_sentmode)) + { + int modeSelection = ReadyState::getInstance().setSelectedMode(); + if(modeSelection > 0) + { + SMPChannelPayload::Mode payload = (modeSelection == 1) ? SMPChannelPayload::Mode::DRIVING_MODE : SMPChannelPayload::Mode::TRAINING_MODE; + (void)m_smpServer.sendData(m_serialMuxProtChannelIdMode, &payload, sizeof(payload)); + m_sentmode = true; + } + } + + + m_smpServer.process(millis()); + + m_systemStateMachine.process(); +} + +void App::handleRemoteCommand(const Command& cmd) +{ + + switch (cmd.commandId) + { + case SMPChannelPayload::CmdId::CMD_ID_IDLE: + /* Nothing to do. */ + break; + + case SMPChannelPayload::CmdId::CMD_ID_SET_READY_STATE: + + m_systemStateMachine.setState(&ReadyState::getInstance()); + m_sentmode = false; + break; + + default: + /* Nothing to do. */ + break; + } +} + + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +void App::sendLineSensorsData() const +{ + ILineSensors& lineSensors = Board::getInstance().getLineSensors(); + uint8_t maxLineSensors = lineSensors.getNumLineSensors(); + const uint16_t* lineSensorValues = lineSensors.getSensorValues(); + uint8_t lineSensorIdx = 0U; + LineSensorData payload; + + if (LINE_SENSOR_CHANNEL_DLC == maxLineSensors * sizeof(uint16_t)) + { + while (maxLineSensors > lineSensorIdx) + { + payload.lineSensorData[lineSensorIdx] = lineSensorValues[lineSensorIdx]; + + ++lineSensorIdx; + } + } + + (void)m_smpServer.sendData(m_serialMuxProtChannelIdLineSensors, &payload, sizeof(payload)); +} + +bool App::setupSerialMuxProt() +{ + bool isSuccessful = false; + /* Channel subscription. */ + m_smpServer.subscribeToChannel(SPEED_SETPOINT_CHANNEL_NAME, App_motorSpeedSetpointsChannelCallback); + m_smpServer.subscribeToChannel(COMMAND_CHANNEL_NAME,App_cmdChannelCallback); + /* Channel creation. */ + m_serialMuxProtChannelIdStatus = m_smpServer.createChannel(STATUS_CHANNEL_NAME, STATUS_CHANNEL_DLC); + m_serialMuxProtChannelIdLineSensors = m_smpServer.createChannel(LINE_SENSOR_CHANNEL_NAME, LINE_SENSOR_CHANNEL_DLC); + m_serialMuxProtChannelIdMode = m_smpServer.createChannel(MODE_CHANNEL_NAME, MODE_CHANNEL_DLC); + + + /* Channels succesfully created? */ + if ((0U != m_serialMuxProtChannelIdStatus) && (0U != m_serialMuxProtChannelIdLineSensors) && (0U != m_serialMuxProtChannelIdMode)) + { + isSuccessful = true; + } + + return isSuccessful; +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ +/** + * Receives remote control commands over SerialMuxProt channel. + * + * @param[in] payload Command id + * @param[in] payloadSize Size of command id + * @param[in] userData User data + */ +static void App_cmdChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData) +{ + if ((nullptr != payload) && (COMMAND_CHANNEL_DLC == payloadSize) && (nullptr != userData)) + { + App* application = reinterpret_cast(userData); + const Command cmd = *reinterpret_cast(payload); + application->handleRemoteCommand(cmd); + } +} + +/** + * Receives motor speed setpoints over SerialMuxProt channel. + * + * @param[in] payload Motor speed left/right + * @param[in] payloadSize Size of twice motor speeds + * @param[in] userData User data + */ +void App_motorSpeedSetpointsChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData) +{ + (void)userData; + if ((nullptr != payload) && (SPEED_SETPOINT_CHANNEL_DLC == payloadSize)) + { + const SpeedData* motorSpeedData = reinterpret_cast(payload); + DrivingState::getInstance().setTargetSpeeds(motorSpeedData->left, motorSpeedData->right); + } +} diff --git a/lib/APPReinforcementLearning/App.h b/lib/APPReinforcementLearning/src/App.h similarity index 87% rename from lib/APPReinforcementLearning/App.h rename to lib/APPReinforcementLearning/src/App.h index 32b40730..995d8d5b 100644 --- a/lib/APPReinforcementLearning/App.h +++ b/lib/APPReinforcementLearning/src/App.h @@ -1,149 +1,146 @@ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief LineFollower application - * @author Akram Bziouech - * - * @addtogroup Application - * - * @{ - */ - -#ifndef APP_H -#define APP_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "SerialMuxChannels.h" -#include -#include -#include - - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The line follower application. */ -class App -{ -public: - /** - * Construct the line follower application. - */ - App() : - m_systemStateMachine(), - m_controlInterval(), - m_serialMuxProtChannelIdStatus(0U), - m_serialMuxProtChannelIdLineSensors(0U), - m_serialMuxProtChannelIdMode(0U), - m_statusTimer(), - m_sendLineSensorsDataInterval(), - m_sentmode(false), - m_smpServer(Serial, nullptr) - { - appInstance = this; - } - - /** - * Destroy the line follower application. - */ - ~App() - { - } - - /** - * Setup the application. - */ - void setup(); - - /** - * Process the application periodically. - */ - void loop(); - /** - * System Status callback. - * - * @param[in] status System status - */ - void systemStatusCallback(SMPChannelPayload::Status status); -private: - /** Differential drive control period in ms. */ - static const uint32_t DIFFERENTIAL_DRIVE_CONTROL_PERIOD = 5U; - - /** Baudrate for Serial Communication */ - static const uint32_t SERIAL_BAUDRATE = 115200U; - - /** Saving the APP instance */ - static App* appInstance; - - /** The system state machine. */ - StateMachine m_systemStateMachine; - - /** Timer used for differential drive control processing. */ - SimpleTimer m_controlInterval; - - /** - * Timer for sending system status to DCS. - */ - SimpleTimer m_statusTimer; - - /** Timer used for sending data periodically. */ - SimpleTimer m_sendLineSensorsDataInterval; - - /** Send status timer interval in ms. */ - static const uint32_t SEND_STATUS_TIMER_INTERVAL = 1000U; - - /** Sending Data period in ms. */ - static const uint32_t SEND_LINE_SENSORS_DATA_PERIOD = 20U; - - /** SerialMuxProt Channel id for sending system status. */ - uint8_t m_serialMuxProtChannelIdStatus; - - /** SerialMuxProt Channel id for sending LineSensors. */ - uint8_t m_serialMuxProtChannelIdLineSensors; - - /** SerialMuxProt Channel id for sending Mode Selected. */ - uint8_t m_serialMuxProtChannelIdMode; - - /** SerialMuxProt Server Instance. */ - SMPServer m_smpServer; - - /* Ensue that the mode is only sent once*/ - bool m_sentmode; - - /** - * Setup the SerialMuxProt channels. - * - * @return If successful returns true, otherwise false. - */ - bool setupSerialMuxProt(); - - /** - * Send line sensors data via SerialMuxProt. - */ - void sendLineSensorsData() const; - - static void App_statusChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData); - /* Not allowed. */ - App(const App& app); /**< Copy construction of an instance. */ - App& operator=(const App& app); /**< Assignment of an instance. */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* APP_H */ -/** @} */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief LineFollower application + * @author Akram Bziouech + * + * @addtogroup Application + * + * @{ + */ + +#ifndef APP_H +#define APP_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "SerialMuxChannels.h" +#include +#include +#include + + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The line follower application. */ +class App +{ +public: + /** + * Construct the line follower application. + */ + App() : + m_systemStateMachine(), + m_controlInterval(), + m_serialMuxProtChannelIdStatus(0U), + m_serialMuxProtChannelIdLineSensors(0U), + m_serialMuxProtChannelIdMode(0U), + m_statusTimer(), + m_sendLineSensorsDataInterval(), + m_sentmode(false), + m_smpServer(Serial, this) + { + } + + /** + * Destroy the line follower application. + */ + ~App() + { + } + + /** + * Setup the application. + */ + void setup(); + + /** + * Process the application periodically. + */ + void loop(); + + /** + * Handle remote command received via SerialMuxProt. + * + * @param[in] cmd Command to handle. + */ + void handleRemoteCommand(const Command& cmd); + +private: + /** Differential drive control period in ms. */ + static const uint32_t DIFFERENTIAL_DRIVE_CONTROL_PERIOD = 5U; + + /** Baudrate for Serial Communication */ + static const uint32_t SERIAL_BAUDRATE = 115200U; + + /** The system state machine. */ + StateMachine m_systemStateMachine; + + /** Timer used for differential drive control processing. */ + SimpleTimer m_controlInterval; + + /** + * Timer for sending system status to DCS. + */ + SimpleTimer m_statusTimer; + + /** Timer used for sending data periodically. */ + SimpleTimer m_sendLineSensorsDataInterval; + + /** Send status timer interval in ms. */ + static const uint32_t SEND_STATUS_TIMER_INTERVAL = 1000U; + + /** Sending Data period in ms. */ + static const uint32_t SEND_LINE_SENSORS_DATA_PERIOD = 20U; + + /** SerialMuxProt Channel id for sending system status. */ + uint8_t m_serialMuxProtChannelIdStatus; + + /** SerialMuxProt Channel id for sending LineSensors. */ + uint8_t m_serialMuxProtChannelIdLineSensors; + + /** SerialMuxProt Channel id for sending Mode Selected. */ + uint8_t m_serialMuxProtChannelIdMode; + + /** SerialMuxProt Server Instance. */ + SMPServer m_smpServer; + + /* Ensue that the mode is only sent once*/ + bool m_sentmode; + + /** + * Setup the SerialMuxProt channels. + * + * @return If successful returns true, otherwise false. + */ + bool setupSerialMuxProt(); + + /** + * Send line sensors data via SerialMuxProt. + */ + void sendLineSensorsData() const; + + /* Not allowed. */ + App(const App& app); /**< Copy construction of an instance. */ + App& operator=(const App& app); /**< Assignment of an instance. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* APP_H */ +/** @} */ diff --git a/lib/APPReinforcementLearning/DrivingState.cpp b/lib/APPReinforcementLearning/src/DrivingState.cpp similarity index 97% rename from lib/APPReinforcementLearning/DrivingState.cpp rename to lib/APPReinforcementLearning/src/DrivingState.cpp index 635e91c7..33515246 100644 --- a/lib/APPReinforcementLearning/DrivingState.cpp +++ b/lib/APPReinforcementLearning/src/DrivingState.cpp @@ -1,119 +1,120 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Driving state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "DrivingState.h" -#include -#include -#include -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void DrivingState::entry() -{ - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - diffDrive.setLinearSpeed(0, 0); - m_observationTimer.start(OBSERVATION_DURATION); -} - -void DrivingState::process(StateMachine& sm) -{ - /* Nothing to do. */ - (void)sm; -} - -void DrivingState::exit() -{ - m_observationTimer.stop(); -} - -void DrivingState::setTargetSpeeds(int16_t leftMotor, int16_t rightMotor) -{ - DifferentialDrive::getInstance().setLinearSpeed(leftMotor, rightMotor); -} - -bool DrivingState::isAbortRequired() -{ - bool isAbort = false; - - if (true == m_observationTimer.isTimeout()) - { - isAbort = true; - } - return isAbort; -} - - - - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ -DrivingState::DrivingState() : - m_observationTimer() -{ -} -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Driving state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "DrivingState.h" +#include +#include +#include +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void DrivingState::entry() +{ + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + diffDrive.setLinearSpeed(0, 0); + m_observationTimer.start(OBSERVATION_DURATION); +} + +void DrivingState::process(StateMachine& sm) +{ + /* Nothing to do. */ + (void)sm; +} + +void DrivingState::exit() +{ + m_observationTimer.stop(); +} + + +void DrivingState::setTargetSpeeds(int16_t leftMotor, int16_t rightMotor) +{ + DifferentialDrive::getInstance().setLinearSpeed(leftMotor, rightMotor); +} + +bool DrivingState::isAbortRequired() +{ + bool isAbort = false; + + if (true == m_observationTimer.isTimeout()) + { + isAbort = true; + } + return isAbort; +} + + + + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ +DrivingState::DrivingState() : + m_observationTimer() +{ +} +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPReinforcementLearning/DrivingState.h b/lib/APPReinforcementLearning/src/DrivingState.h similarity index 96% rename from lib/APPReinforcementLearning/DrivingState.h rename to lib/APPReinforcementLearning/src/DrivingState.h index 3d39e696..09d1c842 100644 --- a/lib/APPReinforcementLearning/DrivingState.h +++ b/lib/APPReinforcementLearning/src/DrivingState.h @@ -1,144 +1,144 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Driving state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef DRIVING_STATE_H -#define DRIVING_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The system driving state. */ -class DrivingState : public IState -{ -public: - /** - * Get state instance. - * - * @return State instance. - */ - static DrivingState& getInstance() - { - static DrivingState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - - /** - * Set target motor speeds. - * - * @param[in] leftMotor Left motor speed. [steps/s] - * @param[in] rightMotor Right motor speed. [steps/s] - */ - void setTargetSpeeds(int16_t leftMotor, int16_t rightMotor); - - /** - * Check the abort conditions while driving the challenge. - * - * @return If abort is required, it will return true otherwise false. - */ - bool isAbortRequired(); - -protected: -private: - - - /** Observation duration in ms. This is the max. time within the robot must be finished its drive. */ - static const uint32_t OBSERVATION_DURATION = 3000000; - - /**< Observation timer to observe the max. time per challenge. */ - - SimpleTimer m_observationTimer; - - /** - * Default constructor. - */ - DrivingState(); - - /** - * Default destructor. - */ - ~DrivingState() - { - } - - /* Not allowed. */ - DrivingState(const DrivingState& state); /**< Copy construction of an instance. */ - DrivingState& operator=(const DrivingState& state); /**< Assignment of an instance. */ - - - -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* DRIVING_STATE_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Driving state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef DRIVING_STATE_H +#define DRIVING_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The system driving state. */ +class DrivingState : public IState +{ +public: + /** + * Get state instance. + * + * @return State instance. + */ + static DrivingState& getInstance() + { + static DrivingState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + + /** + * Set target motor speeds. + * + * @param[in] leftMotor Left motor speed. [steps/s] + * @param[in] rightMotor Right motor speed. [steps/s] + */ + void setTargetSpeeds(int16_t leftMotor, int16_t rightMotor); + + /** + * Check the abort conditions while driving the challenge. + * + * @return If abort is required, it will return true otherwise false. + */ + bool isAbortRequired(); + +protected: +private: + + + /** Observation duration in ms. This is the max. time within the robot must be finished its drive. */ + static const uint32_t OBSERVATION_DURATION = 3000000; + + /**< Observation timer to observe the max. time per challenge. */ + + SimpleTimer m_observationTimer; + + /** + * Default constructor. + */ + DrivingState(); + + /** + * Default destructor. + */ + ~DrivingState() + { + } + + /* Not allowed. */ + DrivingState(const DrivingState& state); /**< Copy construction of an instance. */ + DrivingState& operator=(const DrivingState& state); /**< Assignment of an instance. */ + + + +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* DRIVING_STATE_H */ +/** @} */ diff --git a/lib/APPReinforcementLearning/ErrorState.cpp b/lib/APPReinforcementLearning/src/ErrorState.cpp similarity index 95% rename from lib/APPReinforcementLearning/ErrorState.cpp rename to lib/APPReinforcementLearning/src/ErrorState.cpp index 2af70585..c2bbfe3a 100644 --- a/lib/APPReinforcementLearning/ErrorState.cpp +++ b/lib/APPReinforcementLearning/src/ErrorState.cpp @@ -1,131 +1,130 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Error state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ErrorState.h" -#include -#include -#include "StartupState.h" -#include -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void ErrorState::entry() -{ - IDisplay& display = Board::getInstance().getDisplay(); - - DifferentialDrive::getInstance().disable(); - - display.clear(); - display.print("A: CONT"); - display.gotoXY(0, 1); - - if ('\0' == m_errorMsg[0]) - { - display.print("ERR"); - } - else - { - display.print(m_errorMsg); - } - -} - -void ErrorState::process(StateMachine& sm) -{ - IButton& buttonA = Board::getInstance().getButtonA(); - - /* Restart calibration? */ - if (true == buttonA.isPressed()) - { - buttonA.waitForRelease(); - sm.setState(&StartupState::getInstance()); - } -} - -void ErrorState::exit() -{ - /* Nothing to do. */ -} - -void ErrorState::setErrorMsg(const char* msg) -{ - if (nullptr == msg) - { - m_errorMsg[0] = '\0'; - } - else - { - strncpy(m_errorMsg, msg, ERROR_MSG_SIZE - 1); - m_errorMsg[ERROR_MSG_SIZE - 1] = '\0'; - } -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Error state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ErrorState.h" +#include +#include +#include "StartupState.h" +#include +#include +#include +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void ErrorState::entry() +{ + IDisplay& display = Board::getInstance().getDisplay(); + + DifferentialDrive::getInstance().disable(); + + display.clear(); + display.print("A: CONT"); + display.gotoXY(0, 1); + + if ('\0' == m_errorMsg[0]) + { + display.print("ERR"); + } + else + { + display.print(m_errorMsg); + } + +} + +void ErrorState::process(StateMachine& sm) +{ + IButton& buttonA = Board::getInstance().getButtonA(); + + /* Restart calibration? */ + if (true == Util::isButtonTriggered(buttonA, m_isButtonAPressed)) + { + sm.setState(&StartupState::getInstance()); + } +} + +void ErrorState::exit() +{ + /* Nothing to do. */ +} + +void ErrorState::setErrorMsg(const char* msg) +{ + if (nullptr == msg) + { + m_errorMsg[0] = '\0'; + } + else + { + strncpy(m_errorMsg, msg, ERROR_MSG_SIZE - 1); + m_errorMsg[ERROR_MSG_SIZE - 1] = '\0'; + } +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPReinforcementLearning/ErrorState.h b/lib/APPReinforcementLearning/src/ErrorState.h similarity index 95% rename from lib/APPReinforcementLearning/ErrorState.h rename to lib/APPReinforcementLearning/src/ErrorState.h index 74095203..e0f29a0f 100644 --- a/lib/APPReinforcementLearning/ErrorState.h +++ b/lib/APPReinforcementLearning/src/ErrorState.h @@ -1,134 +1,135 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Error state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef ERROR_STATE_H -#define ERROR_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The system error state. */ -class ErrorState : public IState -{ -public: - /** - * Get state instance. - * - * @return State instance. - */ - static ErrorState& getInstance() - { - static ErrorState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - - /** - * Set error message, which to show on the display. - * - * @param[in] msg Error message - */ - void setErrorMsg(const char* msg); - -protected: -private: - /** - * The error message string size in bytes, which - * includes the terminating character. - */ - static const size_t ERROR_MSG_SIZE = 20; - - char m_errorMsg[ERROR_MSG_SIZE]; /**< Error message, which to show. */ - - /** - * Default constructor. - */ - ErrorState() : m_errorMsg() - { - m_errorMsg[0] = '\0'; - } - - /** - * Default destructor. - */ - ~ErrorState() - { - } - - /* Not allowed. */ - ErrorState(const ErrorState& state); /**< Copy construction of an instance. */ - ErrorState& operator=(const ErrorState& state); /**< Assignment of an instance. */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* ERROR_STATE_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Error state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef ERROR_STATE_H +#define ERROR_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The system error state. */ +class ErrorState : public IState +{ +public: + /** + * Get state instance. + * + * @return State instance. + */ + static ErrorState& getInstance() + { + static ErrorState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + + /** + * Set error message, which to show on the display. + * + * @param[in] msg Error message + */ + void setErrorMsg(const char* msg); + +protected: +private: + /** + * The error message string size in bytes, which + * includes the terminating character. + */ + static const size_t ERROR_MSG_SIZE = 20; + + char m_errorMsg[ERROR_MSG_SIZE]; /**< Error message, which to show. */ + bool m_isButtonAPressed; /**< Is the button A pressed (last time)? */ + + /** + * Default constructor. + */ + ErrorState() : m_errorMsg(), m_isButtonAPressed(false) + { + m_errorMsg[0] = '\0'; + } + + /** + * Default destructor. + */ + ~ErrorState() + { + } + + /* Not allowed. */ + ErrorState(const ErrorState& state); /**< Copy construction of an instance. */ + ErrorState& operator=(const ErrorState& state); /**< Assignment of an instance. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* ERROR_STATE_H */ +/** @} */ diff --git a/lib/APPReinforcementLearning/IBoard.h b/lib/APPReinforcementLearning/src/IBoard.h similarity index 95% rename from lib/APPReinforcementLearning/IBoard.h rename to lib/APPReinforcementLearning/src/IBoard.h index 68c58215..1ffdf518 100644 --- a/lib/APPReinforcementLearning/IBoard.h +++ b/lib/APPReinforcementLearning/src/IBoard.h @@ -1,205 +1,205 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Board interface, which abstracts the physical board - * @author Andreas Merkle - * - * @addtogroup HALInterfaces - * - * @{ - */ - -#ifndef IBOARD_H -#define IBOARD_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#ifdef DEBUG_ODOMETRY -#include -#endif /* DEBUG_ODOMETRY */ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** - * Abstracts the physical board interface. - */ -class IBoard -{ -public: - - /** - * Destroys the board interface. - */ - virtual ~IBoard() - { - } - - /** - * Initialize the hardware. - */ - virtual void init() = 0; - - /** - * Get button A driver. - * - * @return Button A driver. - */ - virtual IButton& getButtonA() = 0; - - /** - * Get button B driver. - * - * @return Button B driver. - */ - virtual IButton& getButtonB() = 0; - - /** - * Get button C driver. - * - * @return Button C driver. - */ - virtual IButton& getButtonC() = 0; - - /** - * Get buzzer driver. - * - * @return Buzzer driver. - */ - virtual IBuzzer& getBuzzer() = 0; - - /** - * Get LCD driver. - * - * @return LCD driver. - */ - virtual IDisplay& getDisplay() = 0; - - /** - * Get encoders driver. - * - * @return Encoders driver. - */ - virtual IEncoders& getEncoders() = 0; - - /** - * Get line sensors driver. - * - * @return Line sensor driver. - */ - virtual ILineSensors& getLineSensors() = 0; - - /** - * Get motor driver. - * - * @return Motor driver. - */ - virtual IMotors& getMotors() = 0; - - /** - * Get red LED driver. - * - * @return Red LED driver. - */ - virtual ILed& getRedLed() = 0; - - /** - * Get yellow LED driver. - * - * @return Yellow LED driver. - */ - virtual ILed& getYellowLed() = 0; - - /** - * Get green LED driver. - * - * @return Green LED driver. - */ - virtual ILed& getGreenLed() = 0; - - /** - * Get the settings. - * - * @return Settings - */ - virtual ISettings& getSettings() = 0; - -#ifdef DEBUG_ODOMETRY - - /** - * Get the sender driver, used to send data to the webots supervisor. - * - * @return Sender driver - */ - virtual ISender& getSender() = 0; - -#endif /* DEBUG_ODOMETRY */ - - /** - * Process actuators and sensors. - */ - virtual void process() = 0; - -protected: - - /** - * Constructs the board interface. - */ - IBoard() - { - } - -private: - -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* IBOARD_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Board interface, which abstracts the physical board + * @author Andreas Merkle + * + * @addtogroup HALInterfaces + * + * @{ + */ + +#ifndef IBOARD_H +#define IBOARD_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef DEBUG_ODOMETRY +#include +#endif /* DEBUG_ODOMETRY */ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** + * Abstracts the physical board interface. + */ +class IBoard +{ +public: + + /** + * Destroys the board interface. + */ + virtual ~IBoard() + { + } + + /** + * Initialize the hardware. + */ + virtual void init() = 0; + + /** + * Get button A driver. + * + * @return Button A driver. + */ + virtual IButton& getButtonA() = 0; + + /** + * Get button B driver. + * + * @return Button B driver. + */ + virtual IButton& getButtonB() = 0; + + /** + * Get button C driver. + * + * @return Button C driver. + */ + virtual IButton& getButtonC() = 0; + + /** + * Get buzzer driver. + * + * @return Buzzer driver. + */ + virtual IBuzzer& getBuzzer() = 0; + + /** + * Get LCD driver. + * + * @return LCD driver. + */ + virtual IDisplay& getDisplay() = 0; + + /** + * Get encoders driver. + * + * @return Encoders driver. + */ + virtual IEncoders& getEncoders() = 0; + + /** + * Get line sensors driver. + * + * @return Line sensor driver. + */ + virtual ILineSensors& getLineSensors() = 0; + + /** + * Get motor driver. + * + * @return Motor driver. + */ + virtual IMotors& getMotors() = 0; + + /** + * Get red LED driver. + * + * @return Red LED driver. + */ + virtual ILed& getRedLed() = 0; + + /** + * Get yellow LED driver. + * + * @return Yellow LED driver. + */ + virtual ILed& getYellowLed() = 0; + + /** + * Get green LED driver. + * + * @return Green LED driver. + */ + virtual ILed& getGreenLed() = 0; + + /** + * Get the settings. + * + * @return Settings + */ + virtual ISettings& getSettings() = 0; + +#ifdef DEBUG_ODOMETRY + + /** + * Get the sender driver, used to send data to the webots supervisor. + * + * @return Sender driver + */ + virtual ISender& getSender() = 0; + +#endif /* DEBUG_ODOMETRY */ + + /** + * Process actuators and sensors. + */ + virtual void process() = 0; + +protected: + + /** + * Constructs the board interface. + */ + IBoard() + { + } + +private: + +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* IBOARD_H */ +/** @} */ diff --git a/lib/APPReinforcementLearning/LineSensorsCalibrationState.cpp b/lib/APPReinforcementLearning/src/LineSensorsCalibrationState.cpp similarity index 97% rename from lib/APPReinforcementLearning/LineSensorsCalibrationState.cpp rename to lib/APPReinforcementLearning/src/LineSensorsCalibrationState.cpp index 378d0998..f2aaa849 100644 --- a/lib/APPReinforcementLearning/LineSensorsCalibrationState.cpp +++ b/lib/APPReinforcementLearning/src/LineSensorsCalibrationState.cpp @@ -1,214 +1,214 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Line sensors calibration state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "LineSensorsCalibrationState.h" -#include -#include -#include -#include -#include -#include "ReadyState.h" -#include "ErrorState.h" -#include "DrivingState.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void LineSensorsCalibrationState::entry() -{ - IDisplay& display = Board::getInstance().getDisplay(); - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - Odometry& odometry = Odometry::getInstance(); - ILineSensors& lineSensors = Board::getInstance().getLineSensors(); - - display.clear(); - display.print("Run"); - display.gotoXY(0, 1); - display.print("LCAL"); - - /* Prepare calibration drive. */ - m_calibrationSpeed = diffDrive.getMaxMotorSpeed() / 4; - m_orientation = odometry.getOrientation(); - - /* Mandatory for each new calibration. */ - lineSensors.resetCalibration(); - - /* Wait some time, before starting the calibration drive. */ - m_phase = PHASE_1_WAIT; - m_timer.start(WAIT_TIME); -} - -void LineSensorsCalibrationState::process(StateMachine& sm) -{ - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - - switch (m_phase) - { - case PHASE_1_WAIT: - if (true == m_timer.isTimeout()) - { - m_phase = PHASE_2_TURN_LEFT; - diffDrive.setLinearSpeed(-m_calibrationSpeed, m_calibrationSpeed); - } - break; - - case PHASE_2_TURN_LEFT: - if (true == turnAndCalibrate(CALIB_ANGLE, true)) - { - m_phase = PHASE_3_TURN_RIGHT; - diffDrive.setLinearSpeed(m_calibrationSpeed, -m_calibrationSpeed); - } - break; - - case PHASE_3_TURN_RIGHT: - if (true == turnAndCalibrate(-CALIB_ANGLE, false)) - { - m_phase = PHASE_4_TURN_ORIG; - diffDrive.setLinearSpeed(-m_calibrationSpeed, m_calibrationSpeed); - } - break; - - case PHASE_4_TURN_ORIG: - if (true == turnAndCalibrate(0, true)) - { - m_phase = PHASE_5_FINISHED; - diffDrive.setLinearSpeed(0, 0); - finishCalibration(sm); - } - break; - - case PHASE_5_FINISHED: - /* fallthrough */ - default: - break; - } -} - -void LineSensorsCalibrationState::exit() -{ - m_timer.stop(); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -bool LineSensorsCalibrationState::turnAndCalibrate(int32_t calibAlpha, bool isGreaterEqual) -{ - ILineSensors& lineSensors = Board::getInstance().getLineSensors(); - Odometry& odometry = Odometry::getInstance(); - int32_t alpha = odometry.getOrientation() - m_orientation; /* [mrad] */ - bool isSuccesful = false; - - /* Continously calibrate the line sensors. */ - lineSensors.calibrate(); - - /* Is the goal that the current angle shall be lower or equal than the destination calibration angle? */ - if (false == isGreaterEqual) - { - /* Is alpha lower or equal than the destination calibration angle? */ - if (calibAlpha >= alpha) - { - isSuccesful = true; - } - } - else - { - /* Is alpha greater or equal than the destination calibration angle? */ - if (calibAlpha <= alpha) - { - isSuccesful = true; - } - } - - return isSuccesful; -} - -void LineSensorsCalibrationState::finishCalibration(StateMachine& sm) -{ - ILineSensors& lineSensors = Board::getInstance().getLineSensors(); - - if (false == lineSensors.isCalibrationSuccessful()) - { - char str[10]; - char valueStr[10]; - - Util::uintToStr(valueStr, sizeof(valueStr), lineSensors.getCalibErrorInfo()); - - strncpy(str, "ELCAL ", sizeof(str) - 1); - str[sizeof(str) - 1] = '\0'; - - strncat(str, valueStr, sizeof(str) - strlen(valueStr) - 1); - - ErrorState::getInstance().setErrorMsg(str); - sm.setState(&ErrorState::getInstance()); - } - else - { - sm.setState(&ReadyState::getInstance()); - } -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Line sensors calibration state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "LineSensorsCalibrationState.h" +#include +#include +#include +#include +#include +#include "ReadyState.h" +#include "ErrorState.h" +#include "DrivingState.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void LineSensorsCalibrationState::entry() +{ + IDisplay& display = Board::getInstance().getDisplay(); + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + Odometry& odometry = Odometry::getInstance(); + ILineSensors& lineSensors = Board::getInstance().getLineSensors(); + + display.clear(); + display.print("Run"); + display.gotoXY(0, 1); + display.print("LCAL"); + + /* Prepare calibration drive. */ + m_calibrationSpeed = diffDrive.getMaxMotorSpeed() / 4; + m_orientation = odometry.getOrientation(); + + /* Mandatory for each new calibration. */ + lineSensors.resetCalibration(); + + /* Wait some time, before starting the calibration drive. */ + m_phase = PHASE_1_WAIT; + m_timer.start(WAIT_TIME); +} + +void LineSensorsCalibrationState::process(StateMachine& sm) +{ + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + + switch (m_phase) + { + case PHASE_1_WAIT: + if (true == m_timer.isTimeout()) + { + m_phase = PHASE_2_TURN_LEFT; + diffDrive.setLinearSpeed(-m_calibrationSpeed, m_calibrationSpeed); + } + break; + + case PHASE_2_TURN_LEFT: + if (true == turnAndCalibrate(CALIB_ANGLE, true)) + { + m_phase = PHASE_3_TURN_RIGHT; + diffDrive.setLinearSpeed(m_calibrationSpeed, -m_calibrationSpeed); + } + break; + + case PHASE_3_TURN_RIGHT: + if (true == turnAndCalibrate(-CALIB_ANGLE, false)) + { + m_phase = PHASE_4_TURN_ORIG; + diffDrive.setLinearSpeed(-m_calibrationSpeed, m_calibrationSpeed); + } + break; + + case PHASE_4_TURN_ORIG: + if (true == turnAndCalibrate(0, true)) + { + m_phase = PHASE_5_FINISHED; + diffDrive.setLinearSpeed(0, 0); + finishCalibration(sm); + } + break; + + case PHASE_5_FINISHED: + /* fallthrough */ + default: + break; + } +} + +void LineSensorsCalibrationState::exit() +{ + m_timer.stop(); +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +bool LineSensorsCalibrationState::turnAndCalibrate(int32_t calibAlpha, bool isGreaterEqual) +{ + ILineSensors& lineSensors = Board::getInstance().getLineSensors(); + Odometry& odometry = Odometry::getInstance(); + int32_t alpha = odometry.getOrientation() - m_orientation; /* [mrad] */ + bool isSuccesful = false; + + /* Continously calibrate the line sensors. */ + lineSensors.calibrate(); + + /* Is the goal that the current angle shall be lower or equal than the destination calibration angle? */ + if (false == isGreaterEqual) + { + /* Is alpha lower or equal than the destination calibration angle? */ + if (calibAlpha >= alpha) + { + isSuccesful = true; + } + } + else + { + /* Is alpha greater or equal than the destination calibration angle? */ + if (calibAlpha <= alpha) + { + isSuccesful = true; + } + } + + return isSuccesful; +} + +void LineSensorsCalibrationState::finishCalibration(StateMachine& sm) +{ + ILineSensors& lineSensors = Board::getInstance().getLineSensors(); + + if (false == lineSensors.isCalibrationSuccessful()) + { + char str[10]; + char valueStr[10]; + + Util::uintToStr(valueStr, sizeof(valueStr), lineSensors.getCalibErrorInfo()); + + strncpy(str, "ELCAL ", sizeof(str) - 1); + str[sizeof(str) - 1] = '\0'; + + strncat(str, valueStr, sizeof(str) - strlen(valueStr) - 1); + + ErrorState::getInstance().setErrorMsg(str); + sm.setState(&ErrorState::getInstance()); + } + else + { + sm.setState(&ReadyState::getInstance()); + } +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPReinforcementLearning/LineSensorsCalibrationState.h b/lib/APPReinforcementLearning/src/LineSensorsCalibrationState.h similarity index 97% rename from lib/APPReinforcementLearning/LineSensorsCalibrationState.h rename to lib/APPReinforcementLearning/src/LineSensorsCalibrationState.h index 564e7508..e9336431 100644 --- a/lib/APPReinforcementLearning/LineSensorsCalibrationState.h +++ b/lib/APPReinforcementLearning/src/LineSensorsCalibrationState.h @@ -1,161 +1,161 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Line sensors calibration state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef LINE_SENSORS_CALIBRATION_STATE_H -#define LINE_SENSORS_CALIBRATION_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The line sensors calibration state. */ -class LineSensorsCalibrationState : public IState -{ -public: - /** - * Get state instance. - * - * @return State instance. - */ - static LineSensorsCalibrationState& getInstance() - { - static LineSensorsCalibrationState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - -protected: -private: - /** Calibration phases */ - enum Phase - { - PHASE_1_WAIT = 0, /**< Wait a short time before starting the calibration. */ - PHASE_2_TURN_LEFT, /**< Turn line sensors left over the line. */ - PHASE_3_TURN_RIGHT, /**< Turn line sensor right over the line. */ - PHASE_4_TURN_ORIG, /**< Turn back to origin. */ - PHASE_5_FINISHED /**< Calibration is finished. */ - }; - - /** - * Duration in ms about to wait, until the calibration drive starts. - */ - static const uint32_t WAIT_TIME = 1000; - - /** - * Calibration turn angle in mrad (corresponds to 72°). - */ - static const int32_t CALIB_ANGLE = (FP_2PI() / 5); - - SimpleTimer m_timer; /**< Timer used to wait, until the calibration drive starts. */ - Phase m_phase; /**< Current calibration phase */ - int16_t m_calibrationSpeed; /**< Speed in steps/s for the calibration drive. */ - int32_t m_orientation; /**< Orientation at the begin of the calibration in [mrad]. */ - - /** - * Default constructor. - */ - LineSensorsCalibrationState() : m_timer(), m_phase(PHASE_1_WAIT), m_calibrationSpeed(0), m_orientation(0) - { - } - - /** - * Default destructor. - */ - ~LineSensorsCalibrationState() - { - } - - /* Not allowed. */ - LineSensorsCalibrationState(const LineSensorsCalibrationState& state); /**< Copy construction of an instance. */ - LineSensorsCalibrationState& operator=(const LineSensorsCalibrationState& state); /**< Assignment of an instance. */ - - /** - * Turn and calibrate the line sensors. - * - * @param[in] calibAlpha Destination calibration angle in [mrad] - * @param[in] isGreaterEqual Configure true if angle shall be greater or equal than the destination. - * - * @return If destination angle reached, it will return true otherwise false. - */ - bool turnAndCalibrate(int32_t calibAlpha, bool isGreaterEqual); - - /** - * Finish the calibration and determine next state. - * - * @param[in] sm State machine - */ - void finishCalibration(StateMachine& sm); -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* LINE_SENSORS_CALIBRATION_STATE_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Line sensors calibration state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef LINE_SENSORS_CALIBRATION_STATE_H +#define LINE_SENSORS_CALIBRATION_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The line sensors calibration state. */ +class LineSensorsCalibrationState : public IState +{ +public: + /** + * Get state instance. + * + * @return State instance. + */ + static LineSensorsCalibrationState& getInstance() + { + static LineSensorsCalibrationState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + +protected: +private: + /** Calibration phases */ + enum Phase + { + PHASE_1_WAIT = 0, /**< Wait a short time before starting the calibration. */ + PHASE_2_TURN_LEFT, /**< Turn line sensors left over the line. */ + PHASE_3_TURN_RIGHT, /**< Turn line sensor right over the line. */ + PHASE_4_TURN_ORIG, /**< Turn back to origin. */ + PHASE_5_FINISHED /**< Calibration is finished. */ + }; + + /** + * Duration in ms about to wait, until the calibration drive starts. + */ + static const uint32_t WAIT_TIME = 1000; + + /** + * Calibration turn angle in mrad (corresponds to 72°). + */ + static const int32_t CALIB_ANGLE = (FP_2PI() / 5); + + SimpleTimer m_timer; /**< Timer used to wait, until the calibration drive starts. */ + Phase m_phase; /**< Current calibration phase */ + int16_t m_calibrationSpeed; /**< Speed in steps/s for the calibration drive. */ + int32_t m_orientation; /**< Orientation at the begin of the calibration in [mrad]. */ + + /** + * Default constructor. + */ + LineSensorsCalibrationState() : m_timer(), m_phase(PHASE_1_WAIT), m_calibrationSpeed(0), m_orientation(0) + { + } + + /** + * Default destructor. + */ + ~LineSensorsCalibrationState() + { + } + + /* Not allowed. */ + LineSensorsCalibrationState(const LineSensorsCalibrationState& state); /**< Copy construction of an instance. */ + LineSensorsCalibrationState& operator=(const LineSensorsCalibrationState& state); /**< Assignment of an instance. */ + + /** + * Turn and calibrate the line sensors. + * + * @param[in] calibAlpha Destination calibration angle in [mrad] + * @param[in] isGreaterEqual Configure true if angle shall be greater or equal than the destination. + * + * @return If destination angle reached, it will return true otherwise false. + */ + bool turnAndCalibrate(int32_t calibAlpha, bool isGreaterEqual); + + /** + * Finish the calibration and determine next state. + * + * @param[in] sm State machine + */ + void finishCalibration(StateMachine& sm); +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* LINE_SENSORS_CALIBRATION_STATE_H */ +/** @} */ diff --git a/lib/APPReinforcementLearning/MotorSpeedCalibrationState.h b/lib/APPReinforcementLearning/src/MotorSpeedCalibrationState.h similarity index 97% rename from lib/APPReinforcementLearning/MotorSpeedCalibrationState.h rename to lib/APPReinforcementLearning/src/MotorSpeedCalibrationState.h index 097cf12e..f4ea0bcc 100644 --- a/lib/APPReinforcementLearning/MotorSpeedCalibrationState.h +++ b/lib/APPReinforcementLearning/src/MotorSpeedCalibrationState.h @@ -1,165 +1,165 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Motor speed calibration state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef MOTOR_SPEED_CALIBRATION_STATE_H -#define MOTOR_SPEED_CALIBRATION_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The motor speed calibration state. */ -class MotorSpeedCalibrationState : public IState -{ -public: - /** - * Get state instance. - * - * @return State instance. - */ - static MotorSpeedCalibrationState& getInstance() - { - static MotorSpeedCalibrationState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - -protected: -private: - /** Calibration phases */ - enum Phase - { - PHASE_1_BACK, /**< Drive with max. speed backwards. */ - PHASE_2_FORWARD, /**< Drive with max. speed forwards. */ - PHASE_3_FINISHED /**< Calibration is finished. */ - }; - - /** - * Duration in ms about to wait, until the calibration drive starts. - */ - static const uint32_t WAIT_TIME = 1000; - - /** - * Calibration drive duration in ms. - * It means how long the robot is driven with max. speed forward/backward. - */ - static const uint32_t CALIB_DURATION = 1000; - - SimpleTimer m_timer; /**< Timer used to wait, until the calibration drive starts and for drive duration. */ - Phase m_phase; /**< Current calibration phase */ - int16_t m_maxSpeedLeft; /**< Max. determined left motor speed [steps/s]. */ - int16_t m_maxSpeedRight; /**< Max. determined right motor speed [steps/s]. */ - RelativeEncoders m_relEncoders; /**< Relative encoders left/right. */ - - /** - * Default constructor. - */ - MotorSpeedCalibrationState() : - m_timer(), - m_phase(PHASE_1_BACK), - m_maxSpeedLeft(0), - m_maxSpeedRight(0), - m_relEncoders(Board::getInstance().getEncoders()) - { - } - - /** - * Default destructor. - */ - ~MotorSpeedCalibrationState() - { - } - - /* Not allowed. */ - MotorSpeedCalibrationState(const MotorSpeedCalibrationState& state); /**< Copy construction of an instance. */ - MotorSpeedCalibrationState& operator=(const MotorSpeedCalibrationState& state); /**< Assignment of an instance. */ - - /** - * Determine the max. motor speed, considering both driving directions. - * There are two steps necessary: - * - Drive full backward and call this method to determine. - * - Drive full forward and call this method to determine. - */ - void determineMaxMotorSpeed(); - - /** - * Finish the calibration and determine next state. - * - * @param[in] sm State machine - */ - void finishCalibration(StateMachine& sm); -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* MOTOR_SPEED_CALIBRATION_STATE_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Motor speed calibration state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef MOTOR_SPEED_CALIBRATION_STATE_H +#define MOTOR_SPEED_CALIBRATION_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The motor speed calibration state. */ +class MotorSpeedCalibrationState : public IState +{ +public: + /** + * Get state instance. + * + * @return State instance. + */ + static MotorSpeedCalibrationState& getInstance() + { + static MotorSpeedCalibrationState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + +protected: +private: + /** Calibration phases */ + enum Phase + { + PHASE_1_BACK, /**< Drive with max. speed backwards. */ + PHASE_2_FORWARD, /**< Drive with max. speed forwards. */ + PHASE_3_FINISHED /**< Calibration is finished. */ + }; + + /** + * Duration in ms about to wait, until the calibration drive starts. + */ + static const uint32_t WAIT_TIME = 1000; + + /** + * Calibration drive duration in ms. + * It means how long the robot is driven with max. speed forward/backward. + */ + static const uint32_t CALIB_DURATION = 1000; + + SimpleTimer m_timer; /**< Timer used to wait, until the calibration drive starts and for drive duration. */ + Phase m_phase; /**< Current calibration phase */ + int16_t m_maxSpeedLeft; /**< Max. determined left motor speed [steps/s]. */ + int16_t m_maxSpeedRight; /**< Max. determined right motor speed [steps/s]. */ + RelativeEncoders m_relEncoders; /**< Relative encoders left/right. */ + + /** + * Default constructor. + */ + MotorSpeedCalibrationState() : + m_timer(), + m_phase(PHASE_1_BACK), + m_maxSpeedLeft(0), + m_maxSpeedRight(0), + m_relEncoders(Board::getInstance().getEncoders()) + { + } + + /** + * Default destructor. + */ + ~MotorSpeedCalibrationState() + { + } + + /* Not allowed. */ + MotorSpeedCalibrationState(const MotorSpeedCalibrationState& state); /**< Copy construction of an instance. */ + MotorSpeedCalibrationState& operator=(const MotorSpeedCalibrationState& state); /**< Assignment of an instance. */ + + /** + * Determine the max. motor speed, considering both driving directions. + * There are two steps necessary: + * - Drive full backward and call this method to determine. + * - Drive full forward and call this method to determine. + */ + void determineMaxMotorSpeed(); + + /** + * Finish the calibration and determine next state. + * + * @param[in] sm State machine + */ + void finishCalibration(StateMachine& sm); +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* MOTOR_SPEED_CALIBRATION_STATE_H */ +/** @} */ diff --git a/lib/APPReinforcementLearning/MotorSpeedCalibrationstate.cpp b/lib/APPReinforcementLearning/src/MotorSpeedCalibrationstate.cpp similarity index 97% rename from lib/APPReinforcementLearning/MotorSpeedCalibrationstate.cpp rename to lib/APPReinforcementLearning/src/MotorSpeedCalibrationstate.cpp index 5dec5290..f9547662 100644 --- a/lib/APPReinforcementLearning/MotorSpeedCalibrationstate.cpp +++ b/lib/APPReinforcementLearning/src/MotorSpeedCalibrationstate.cpp @@ -1,245 +1,245 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Motor speed calibration state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "MotorSpeedCalibrationState.h" -#include -#include -#include -#include -#include -#include -#include "LineSensorsCalibrationState.h" -#include "ErrorState.h" -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/** - * Logging source. - */ -LOG_TAG("MSCState"); - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void MotorSpeedCalibrationState::entry() -{ - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - IDisplay& display = Board::getInstance().getDisplay(); - - display.clear(); - display.print("Run"); - display.gotoXY(0, 1); - display.print("MCAL"); - - /* Disable differential drive to avoid any bad influence. */ - diffDrive.disable(); - - /* Setup relative encoders */ - m_relEncoders.clear(); - - /* Set the max. speeds to maximum of datatype, because during calibration - * the max. speed is determined by the lowest motor speed. - */ - m_maxSpeedLeft = INT16_MAX; - m_maxSpeedRight = INT16_MAX; - - /* Wait some time, before starting the calibration drive. */ - m_phase = PHASE_1_BACK; - m_timer.start(WAIT_TIME); -} - -void MotorSpeedCalibrationState::process(StateMachine& sm) -{ - if (true == m_timer.isTimeout()) - { - /* Control motors directly and not via differential drive control, - * because the differential drive control needs first to be updated - * regarding the max. possible motor speed in [steps/s] which is - * determined by this calibration. - */ - IMotors& motors = Board::getInstance().getMotors(); - - switch (m_phase) - { - case PHASE_1_BACK: - /* Drive full back. */ - motors.setSpeeds(-motors.getMaxSpeed(), -motors.getMaxSpeed()); - - m_timer.start(CALIB_DURATION); - m_phase = PHASE_2_FORWARD; - break; - - case PHASE_2_FORWARD: - motors.setSpeeds(0, 0); - determineMaxMotorSpeed(); - - /* Drive full forward. */ - motors.setSpeeds(motors.getMaxSpeed(), motors.getMaxSpeed()); - - m_timer.restart(); - m_phase = PHASE_3_FINISHED; - break; - - case PHASE_3_FINISHED: - motors.setSpeeds(0, 0); - determineMaxMotorSpeed(); - - m_timer.stop(); - finishCalibration(sm); - break; - - default: - break; - } - } -} - -void MotorSpeedCalibrationState::exit() -{ - /* Nothing to do. */ -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -void MotorSpeedCalibrationState::determineMaxMotorSpeed() -{ - int32_t stepsLeft = 0; - int32_t stepsRight = 0; - - /* Determine max. speed backward. */ - stepsLeft = abs(m_relEncoders.getCountsLeft()); - stepsRight = abs(m_relEncoders.getCountsRight()); - - /* Convert number of steps to [steps/s] */ - stepsLeft *= 1000; - stepsLeft /= CALIB_DURATION; - stepsRight *= 1000; - stepsRight /= CALIB_DURATION; - - if (INT16_MAX >= stepsLeft) - { - /* Use lower speed to ensure that motor speed can be reached in both - * directions. - */ - if (stepsLeft < m_maxSpeedLeft) - { - m_maxSpeedLeft = static_cast(stepsLeft); - } - } - - if (INT16_MAX >= stepsRight) - { - /* Use lower speed to ensure that motor speed can be reached in both - * directions. - */ - if (stepsRight < m_maxSpeedRight) - { - m_maxSpeedRight = static_cast(stepsRight); - } - } - - /* Clear relative encoders */ - m_relEncoders.clear(); -} - -void MotorSpeedCalibrationState::finishCalibration(StateMachine& sm) -{ - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - ISettings& settings = Board::getInstance().getSettings(); - - /* Set the lower speed as max. motor speed to ensure that both motors - * can reach the same max. speed. - */ - int16_t maxSpeed = (m_maxSpeedLeft < m_maxSpeedRight) ? m_maxSpeedLeft : m_maxSpeedRight; - - /* Store calibrated max. motor speed in the settings. */ - settings.setMaxSpeed(maxSpeed); - - /* With setting the max. motor speed in [steps/s] the differential drive control - * can now be used. - */ - diffDrive.setMaxMotorSpeed(maxSpeed); - - /* Differential drive can now be used. */ - diffDrive.enable(); - - if (0 == maxSpeed) - { - ErrorState::getInstance().setErrorMsg("EMCAL 0"); - sm.setState(&ErrorState::getInstance()); - } - else - { - int32_t maxSpeed32 = static_cast(maxSpeed) * 1000 / static_cast(RobotConstants::ENCODER_STEPS_PER_M); - - LOG_INFO_VAL("Calibrated max. speed (steps/s): ", maxSpeed); - LOG_INFO_VAL("Calibrated max. speed (mm/s): ", maxSpeed32); - - sm.setState(&LineSensorsCalibrationState::getInstance()); - } -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Motor speed calibration state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "MotorSpeedCalibrationState.h" +#include +#include +#include +#include +#include +#include +#include "LineSensorsCalibrationState.h" +#include "ErrorState.h" +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/** + * Logging source. + */ +LOG_TAG("MSCState"); + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void MotorSpeedCalibrationState::entry() +{ + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + IDisplay& display = Board::getInstance().getDisplay(); + + display.clear(); + display.print("Run"); + display.gotoXY(0, 1); + display.print("MCAL"); + + /* Disable differential drive to avoid any bad influence. */ + diffDrive.disable(); + + /* Setup relative encoders */ + m_relEncoders.clear(); + + /* Set the max. speeds to maximum of datatype, because during calibration + * the max. speed is determined by the lowest motor speed. + */ + m_maxSpeedLeft = INT16_MAX; + m_maxSpeedRight = INT16_MAX; + + /* Wait some time, before starting the calibration drive. */ + m_phase = PHASE_1_BACK; + m_timer.start(WAIT_TIME); +} + +void MotorSpeedCalibrationState::process(StateMachine& sm) +{ + if (true == m_timer.isTimeout()) + { + /* Control motors directly and not via differential drive control, + * because the differential drive control needs first to be updated + * regarding the max. possible motor speed in [steps/s] which is + * determined by this calibration. + */ + IMotors& motors = Board::getInstance().getMotors(); + + switch (m_phase) + { + case PHASE_1_BACK: + /* Drive full back. */ + motors.setSpeeds(-motors.getMaxSpeed(), -motors.getMaxSpeed()); + + m_timer.start(CALIB_DURATION); + m_phase = PHASE_2_FORWARD; + break; + + case PHASE_2_FORWARD: + motors.setSpeeds(0, 0); + determineMaxMotorSpeed(); + + /* Drive full forward. */ + motors.setSpeeds(motors.getMaxSpeed(), motors.getMaxSpeed()); + + m_timer.restart(); + m_phase = PHASE_3_FINISHED; + break; + + case PHASE_3_FINISHED: + motors.setSpeeds(0, 0); + determineMaxMotorSpeed(); + + m_timer.stop(); + finishCalibration(sm); + break; + + default: + break; + } + } +} + +void MotorSpeedCalibrationState::exit() +{ + /* Nothing to do. */ +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +void MotorSpeedCalibrationState::determineMaxMotorSpeed() +{ + int32_t stepsLeft = 0; + int32_t stepsRight = 0; + + /* Determine max. speed backward. */ + stepsLeft = abs(m_relEncoders.getCountsLeft()); + stepsRight = abs(m_relEncoders.getCountsRight()); + + /* Convert number of steps to [steps/s] */ + stepsLeft *= 1000; + stepsLeft /= CALIB_DURATION; + stepsRight *= 1000; + stepsRight /= CALIB_DURATION; + + if (INT16_MAX >= stepsLeft) + { + /* Use lower speed to ensure that motor speed can be reached in both + * directions. + */ + if (stepsLeft < m_maxSpeedLeft) + { + m_maxSpeedLeft = static_cast(stepsLeft); + } + } + + if (INT16_MAX >= stepsRight) + { + /* Use lower speed to ensure that motor speed can be reached in both + * directions. + */ + if (stepsRight < m_maxSpeedRight) + { + m_maxSpeedRight = static_cast(stepsRight); + } + } + + /* Clear relative encoders */ + m_relEncoders.clear(); +} + +void MotorSpeedCalibrationState::finishCalibration(StateMachine& sm) +{ + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + ISettings& settings = Board::getInstance().getSettings(); + + /* Set the lower speed as max. motor speed to ensure that both motors + * can reach the same max. speed. + */ + int16_t maxSpeed = (m_maxSpeedLeft < m_maxSpeedRight) ? m_maxSpeedLeft : m_maxSpeedRight; + + /* Store calibrated max. motor speed in the settings. */ + settings.setMaxSpeed(maxSpeed); + + /* With setting the max. motor speed in [steps/s] the differential drive control + * can now be used. + */ + diffDrive.setMaxMotorSpeed(maxSpeed); + + /* Differential drive can now be used. */ + diffDrive.enable(); + + if (0 == maxSpeed) + { + ErrorState::getInstance().setErrorMsg("EMCAL 0"); + sm.setState(&ErrorState::getInstance()); + } + else + { + int32_t maxSpeed32 = static_cast(maxSpeed) * 1000 / static_cast(RobotConstants::ENCODER_STEPS_PER_M); + + LOG_INFO_VAL("Calibrated max. speed (steps/s): ", maxSpeed); + LOG_INFO_VAL("Calibrated max. speed (mm/s): ", maxSpeed32); + + sm.setState(&LineSensorsCalibrationState::getInstance()); + } +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPReinforcementLearning/ReadyState.cpp b/lib/APPReinforcementLearning/src/ReadyState.cpp similarity index 89% rename from lib/APPReinforcementLearning/ReadyState.cpp rename to lib/APPReinforcementLearning/src/ReadyState.cpp index 5b79418b..28cf0f43 100644 --- a/lib/APPReinforcementLearning/ReadyState.cpp +++ b/lib/APPReinforcementLearning/src/ReadyState.cpp @@ -1,186 +1,187 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Ready state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ReadyState.h" -#include -#include -#include -#include "DrivingState.h" -#include "ErrorState.h" -#include -#include -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ -const uint16_t ReadyState::SENSOR_VALUE_MAX = Board::getInstance().getLineSensors().getSensorValueMax(); -/* Initialize the required sensor IDs to be generic. */ -const uint8_t ReadyState::SENSOR_ID_MOST_LEFT = 0U; -const uint8_t ReadyState::SENSOR_ID_MIDDLE = Board::getInstance().getLineSensors().getNumLineSensors() / 2U; -const uint8_t ReadyState::SENSOR_ID_MOST_RIGHT = Board::getInstance().getLineSensors().getNumLineSensors() - 1U; - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void ReadyState::entry() -{ - IDisplay& display = Board::getInstance().getDisplay(); - display.clear(); - display.print("A: TMD"); - display.gotoXY(0, 1); - display.print("B: DMD"); - - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - diffDrive.setLinearSpeed(0, 0); - LastStatus = false; - - if (true == m_isLapTimeAvailable) - { - display.gotoXY(0, 2); - display.print(m_lapTime); - display.print("ms"); - } - m_modeTimeoutTimer.start(mode_selected_period); -} - -void ReadyState::process(StateMachine& sm) -{ - IButton& buttonA = Board::getInstance().getButtonA(); - IButton& buttonB = Board::getInstance().getButtonB(); - ILineSensors& lineSensors = Board::getInstance().getLineSensors(); - uint8_t maxLineSensors = lineSensors.getNumLineSensors(); - const uint16_t* lineSensorValues = lineSensors.getSensorValues(); - m_mode = IDLE; - /* Shall the driving mode be released? */ - if (true == buttonA.isPressed()) - { - buttonA.waitForRelease(); - m_mode = DRIVING_MODE; - } - /* Shall the Training mode be released? */ - else if ((true == buttonB.isPressed()) ) - { - buttonB.waitForRelease(); - m_mode = TRAINING_MODE; - } - else if (true == m_modeTimeoutTimer.isTimeout() && (m_mode == IDLE)) - { - m_mode = TRAINING_MODE; - m_modeTimeoutTimer.restart(); - } - else - { - /* Nothing to do. */ - ; - } - /**Drive forward until START LINE is crossed */ - DriveUntilStartLinecross(); - if ((isStartStopLineDetected(lineSensorValues, maxLineSensors) == false) && (LastStatus == true)) - { - sm.setState(&DrivingState::getInstance()); - } - else - { - LastStatus = isStartStopLineDetected(lineSensorValues, maxLineSensors); - } - - -} - -void ReadyState::exit() -{ - m_isLapTimeAvailable = false; -} - -void ReadyState::setLapTime(uint32_t lapTime) -{ - m_isLapTimeAvailable = true; - m_lapTime = lapTime; -} - -uint8_t ReadyState::setSelectedMode() -{ - return (m_mode); -} - -/**Drive forward until START LINE is crossed */ -void ReadyState :: DriveUntilStartLinecross() -{ - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - int16_t top_speed = 2000; - int16_t leftMotor = top_speed / 2U; - int16_t rightMotor = top_speed / 2U; - diffDrive.setLinearSpeed(leftMotor, rightMotor); -} - -bool ReadyState::isStartStopLineDetected(const uint16_t* lineSensorValues, uint8_t length) const -{ - bool isDetected = false; - const uint32_t LINE_MAX_30 = (SENSOR_VALUE_MAX * 3U) / 10U; /* 30 % of max. value */ - const uint32_t LINE_MAX_70 = (SENSOR_VALUE_MAX * 7U) / 10U; /* 70 % of max. value */ - - /* - * === = === - * + + + + + - * L M R - */ - if ((LINE_MAX_30 <= lineSensorValues[SENSOR_ID_MOST_LEFT]) && - (LINE_MAX_70 > lineSensorValues[SENSOR_ID_MIDDLE - 1U]) && - (LINE_MAX_70 <= lineSensorValues[SENSOR_ID_MIDDLE]) && - (LINE_MAX_70 > lineSensorValues[SENSOR_ID_MIDDLE + 1U]) && - (LINE_MAX_30 <= lineSensorValues[SENSOR_ID_MOST_RIGHT])) - { - isDetected = true; - } - - return isDetected; -} +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Ready state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ReadyState.h" +#include +#include +#include +#include "DrivingState.h" +#include "ErrorState.h" +#include +#include +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ +const uint16_t ReadyState::SENSOR_VALUE_MAX = Board::getInstance().getLineSensors().getSensorValueMax(); + +/* Initialize the required sensor IDs to be generic. */ +const uint8_t ReadyState::SENSOR_ID_MOST_LEFT = 0U; +const uint8_t ReadyState::SENSOR_ID_MIDDLE = Board::getInstance().getLineSensors().getNumLineSensors() / 2U; +const uint8_t ReadyState::SENSOR_ID_MOST_RIGHT = Board::getInstance().getLineSensors().getNumLineSensors() - 1U; + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void ReadyState::entry() +{ + IDisplay& display = Board::getInstance().getDisplay(); + display.clear(); + display.print("A: TMD"); + display.gotoXY(0, 1); + display.print("B: DMD"); + + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + diffDrive.setLinearSpeed(0, 0); + LastLineStatus = false; + + if (true == m_isLapTimeAvailable) + { + display.gotoXY(0, 2); + display.print(m_lapTime); + display.print("ms"); + } + m_modeTimeoutTimer.start(mode_selected_period); +} + +void ReadyState::process(StateMachine& sm) +{ + IButton& buttonA = Board::getInstance().getButtonA(); + IButton& buttonB = Board::getInstance().getButtonB(); + ILineSensors& lineSensors = Board::getInstance().getLineSensors(); + uint8_t maxLineSensors = lineSensors.getNumLineSensors(); + const uint16_t* lineSensorValues = lineSensors.getSensorValues(); + m_mode = IDLE; + + /* Shall the driving mode be released? */ + if (true == Util::isButtonTriggered(buttonA, m_isButtonAPressed)) + { + m_mode = DRIVING_MODE; + } + /* Shall the Training mode be released? */ + else if (true == Util::isButtonTriggered(buttonB, m_isButtonBPressed)) + { + m_mode = TRAINING_MODE; + } + else if (true == m_modeTimeoutTimer.isTimeout() && (m_mode == IDLE)) + { + m_mode = TRAINING_MODE; + m_modeTimeoutTimer.restart(); + } + else + { + /* Nothing to do. */ + ; + } + + /**Drive forward until START LINE is crossed */ + DriveUntilStartLineisCrossed(); + + if ((isStartStopLineDetected(lineSensorValues, maxLineSensors) == false) && (LastLineStatus == true)) + { + printf("StartStopLineDetected"); + sm.setState(&DrivingState::getInstance()); + } + else + { + LastLineStatus = isStartStopLineDetected(lineSensorValues, maxLineSensors); + } +} + +void ReadyState::exit() +{ + m_isLapTimeAvailable = false; +} + +void ReadyState::setLapTime(uint32_t lapTime) +{ + m_isLapTimeAvailable = true; + m_lapTime = lapTime; +} + +uint8_t ReadyState::setSelectedMode() +{ + return (m_mode); +} + +/**Drive forward until START LINE is crossed */ +void ReadyState :: DriveUntilStartLineisCrossed() +{ + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + int16_t top_speed = 2000; /*Set a top speed of 2000 */ + int16_t leftMotor = top_speed / 2U; /* Drive at half speed */ + int16_t rightMotor = top_speed / 2U; /* Drive at half speed */ + diffDrive.setLinearSpeed(leftMotor, rightMotor); +} + +bool ReadyState::isStartStopLineDetected(const uint16_t* lineSensorValues, uint8_t length) const +{ + bool isDetected = false; + const uint32_t LINE_MAX_30 = (SENSOR_VALUE_MAX * 3U) / 10U; /* 30 % of max. value */ + const uint32_t LINE_MAX_70 = (SENSOR_VALUE_MAX * 7U) / 10U; /* 70 % of max. value */ + + /* + * === = === + * + + + + + + * L M R + */ + if ((LINE_MAX_30 <= lineSensorValues[SENSOR_ID_MOST_LEFT]) && + (LINE_MAX_70 > lineSensorValues[SENSOR_ID_MIDDLE - 1U]) && + (LINE_MAX_70 <= lineSensorValues[SENSOR_ID_MIDDLE]) && + (LINE_MAX_70 > lineSensorValues[SENSOR_ID_MIDDLE + 1U]) && + (LINE_MAX_30 <= lineSensorValues[SENSOR_ID_MOST_RIGHT])) + { + isDetected = true; + } + + return isDetected; +} diff --git a/lib/APPReinforcementLearning/ReadyState.h b/lib/APPReinforcementLearning/src/ReadyState.h similarity index 92% rename from lib/APPReinforcementLearning/ReadyState.h rename to lib/APPReinforcementLearning/src/ReadyState.h index dced9196..e592113a 100644 --- a/lib/APPReinforcementLearning/ReadyState.h +++ b/lib/APPReinforcementLearning/src/ReadyState.h @@ -1,212 +1,217 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Ready state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef READY_STATE_H -#define READY_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The system ready state. */ -class ReadyState : public IState -{ -public: - - /** - * Get state instance. - * - * @return State instance. - */ - static ReadyState& getInstance() - { - static ReadyState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - - /** - * Set lap time, which to show on the display. - * - * @param[in] lapTime Lap time in ms - */ - void setLapTime(uint32_t lapTime); - - /** - * Set the selected mode. - * - * @return It returns 1 if DrivingMode is selected or 2 if TrainingMode is selected. - */ - - uint8_t setSelectedMode( ); - -protected: -private: - - /** Duration of the selected mode in ms. This is the maximum time to select a mode. */ - static const uint32_t mode_selected_period = 500U; - - /** - * The line sensor threshold (normalized) used to detect the track. - * The track is detected in case the received value is greater or equal than - * the threshold. - */ - static const uint16_t LINE_SENSOR_ON_TRACK_MIN_VALUE = 200U; - - /** - * ID of most left sensor. - */ - static const uint8_t SENSOR_ID_MOST_LEFT; - - /** - * ID of middle sensor. - */ - static const uint8_t SENSOR_ID_MIDDLE; - - /** - * ID of most right sensor. - */ - static const uint8_t SENSOR_ID_MOST_RIGHT; - - /** - * The max. normalized value of a sensor in digits. - */ - static const uint16_t SENSOR_VALUE_MAX; - - /** - * last Line Status - */ - bool LastStatus ; - - /** Timeout timer for the selected mode. */ - SimpleTimer m_modeTimeoutTimer; - - /** The system state machine. */ - StateMachine m_systemStateMachine; - - /**< Is set (true), if a lap time is available. */ - bool m_isLapTimeAvailable; - - /**< Lap time in ms of the last successful driven round. */ - uint32_t m_lapTime; - - /** - * The mode that can be selected. - */ - enum mode: uint8_t - { - IDLE = 0, /**< No mode has been selected*/ - DRIVING_MODE, /**< Driving Mode Selected. */ - TRAINING_MODE /**< Training Mode Selected. */ - }; - mode m_mode; - - ReadyState() : - m_isLapTimeAvailable(false), - m_modeTimeoutTimer(), - m_lapTime(0), - LastStatus(false), - m_mode(IDLE) - { - } - - /** - * Default destructor. - */ - ~ReadyState() - { - } - - /* Not allowed. */ - ReadyState(const ReadyState& state); /**< Copy construction of an instance. */ - ReadyState& operator=(const ReadyState& state); /**< Assignment of an instance. */ - - /* - * The robot moves from its current position - * until it crosses and leaves the start line. - */ - void DriveUntilStartLinecross(); - - /** - * Is the start/stop line detected? - * - * @param[in] lineSensorValues The line sensor values as array. - * @param[in] length The number of line sensor values. - * - * @return If start/stop line detected, it will return true otherwise false. - */ - bool isStartStopLineDetected(const uint16_t* lineSensorValues, uint8_t length) const; -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* READY_STATE_H */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Ready state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef READY_STATE_H +#define READY_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The system ready state. */ +class ReadyState : public IState +{ +public: + + /** + * Get state instance. + * + * @return State instance. + */ + static ReadyState& getInstance() + { + static ReadyState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + + /** + * Set lap time, which to show on the display. + * + * @param[in] lapTime Lap time in ms + */ + void setLapTime(uint32_t lapTime); + + /** + * Set the selected mode. + * + * @return It returns 1 if DrivingMode is selected or 2 if TrainingMode is selected. + */ + + uint8_t setSelectedMode( ); + +protected: +private: + + /** Duration of the selected mode in ms. This is the maximum time to select a mode. */ + static const uint32_t mode_selected_period = 100U; + + /** + * The line sensor threshold (normalized) used to detect the track. + * The track is detected in case the received value is greater or equal than + * the threshold. + */ + static const uint16_t LINE_SENSOR_ON_TRACK_MIN_VALUE = 200U; + + /** + * ID of most left sensor. + */ + static const uint8_t SENSOR_ID_MOST_LEFT; + + /** + * ID of middle sensor. + */ + static const uint8_t SENSOR_ID_MIDDLE; + + /** + * ID of most right sensor. + */ + static const uint8_t SENSOR_ID_MOST_RIGHT; + + /** + * The max. normalized value of a sensor in digits. + */ + static const uint16_t SENSOR_VALUE_MAX; + + /** + * last Line Status + */ + bool LastLineStatus ; + + /** Timeout timer for the selected mode. */ + SimpleTimer m_modeTimeoutTimer; + + /** The system state machine. */ + StateMachine m_systemStateMachine; + + /**< Is set (true), if a lap time is available. */ + bool m_isLapTimeAvailable; + + /**< Lap time in ms of the last successful driven round. */ + uint32_t m_lapTime; + + /** + * The mode that can be selected. + */ + enum mode: uint8_t + { + IDLE = 0, /**< No mode has been selected*/ + DRIVING_MODE, /**< Driving Mode Selected. */ + TRAINING_MODE /**< Training Mode Selected. */ + }; + + bool m_isButtonAPressed; /**< Is the button A pressed (last time)? */ + bool m_isButtonBPressed; /**< Is the button B pressed (last time)? */ + mode m_mode; + + ReadyState() : + m_isLapTimeAvailable(false), + m_isButtonAPressed(false), + m_isButtonBPressed(false), + m_modeTimeoutTimer(), + m_lapTime(0), + LastLineStatus(false), + m_mode(IDLE) + { + } + + /** + * Default destructor. + */ + ~ReadyState() + { + } + + /* Not allowed. */ + ReadyState(const ReadyState& state); /**< Copy construction of an instance. */ + ReadyState& operator=(const ReadyState& state); /**< Assignment of an instance. */ + + /* + * The robot moves from its current position + * until it crosses and leaves the start line. + */ + void DriveUntilStartLineisCrossed(); + + /** + * Is the start/stop line detected? + * + * @param[in] lineSensorValues The line sensor values as array. + * @param[in] length The number of line sensor values. + * + * @return If start/stop line detected, it will return true otherwise false. + */ + bool isStartStopLineDetected(const uint16_t* lineSensorValues, uint8_t length) const; +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* READY_STATE_H */ /** @} */ \ No newline at end of file diff --git a/lib/APPReinforcementLearning/SerialMuxchannels.h b/lib/APPReinforcementLearning/src/SerialMuxchannels.h similarity index 87% rename from lib/APPReinforcementLearning/SerialMuxchannels.h rename to lib/APPReinforcementLearning/src/SerialMuxchannels.h index 239aa823..3dbe7822 100644 --- a/lib/APPReinforcementLearning/SerialMuxchannels.h +++ b/lib/APPReinforcementLearning/src/SerialMuxchannels.h @@ -1,133 +1,154 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Channel structure definition for the SerialMuxProt. - * @author Gabryel Reyes - */ - -#ifndef SERIAL_MUX_CHANNELS_H_ -#define SERIAL_MUX_CHANNELS_H_ - -/****************************************************************************** - * Includes - *****************************************************************************/ - -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/** Maximum number of SerialMuxProt Channels. */ -#define MAX_CHANNELS (10U) - -/** Name of Channel to send system status to. */ -#define STATUS_CHANNEL_NAME "STATUS" - -/** DLC of Status Channel */ -#define STATUS_CHANNEL_DLC (sizeof(Status)) - -/** Name of Channel to receive Motor Speed Setpoints. */ -#define SPEED_SETPOINT_CHANNEL_NAME "SPEED_SET" - -/** DLC of Speedometer Channel */ -#define SPEED_SETPOINT_CHANNEL_DLC (sizeof(SpeedData)) - -/** Name of the Channel to receive Line Sensor Data from. */ -#define LINE_SENSOR_CHANNEL_NAME "LINE_SENS" - -/** DLC of Line Sensor Channel */ -#define LINE_SENSOR_CHANNEL_DLC (sizeof(LineSensorData)) - -/** Name of channel to send system Mode to */ -#define MODE_CHANNEL_NAME "MODE" - -/** DLC of Mode Channel */ -#define MODE_CHANNEL_DLC (sizeof(Mode)) - - - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** SerialMuxProt Server with fixed template argument. */ -typedef SerialMuxProtServer SMPServer; - -/** Channel payload constants. */ -namespace SMPChannelPayload -{ - - /** Status flags. */ - typedef enum : uint8_t - { - NOT_DONE = 0, /**< End Line Not Detected. */ - DONE /**< End Line Detected. */ - - } Status; /**< Status flag */ - - typedef enum : uint8_t - { - TRAINING_MODE = 0, /**< Driving Mode Selected. */ - DRIVING_MODE /**< Training Mode Selected. */ - - } Mode; /**< Status flag */ - -} - - -/** Struct of the "Speed" channel payload. */ -typedef struct _SpeedData -{ - int16_t left; /**< Left motor speed [steps/s] */ - int16_t right; /**< Right motor speed [steps/s] */ -} __attribute__((packed)) SpeedData; - -/** Struct of the "Mode" channel payload. */ -typedef struct _Mode -{ - SMPChannelPayload::Mode mode; /**< Mode */ -} __attribute__((packed)) Mode; - -/** Struct of the "Status" channel payload. */ -typedef struct _Status -{ - SMPChannelPayload::Status status; /**< Status */ -} __attribute__((packed)) Status; - -/** Struct of the "Line Sensor" channel payload. */ -typedef struct _LineSensorData -{ - uint16_t lineSensorData[5U]; /**< Line sensor data [digits] normalized to max 1000 digits. */ -} __attribute__((packed)) LineSensorData; - -/****************************************************************************** - * Functions - *****************************************************************************/ - +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Channel structure definition for the SerialMuxProt. + * @author Gabryel Reyes + */ + +#ifndef SERIAL_MUX_CHANNELS_H_ +#define SERIAL_MUX_CHANNELS_H_ + +/****************************************************************************** + * Includes + *****************************************************************************/ + +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/** Maximum number of SerialMuxProt Channels. */ +#define MAX_CHANNELS (10U) + +/** Name of Channel to send Commands to. */ +#define COMMAND_CHANNEL_NAME "CMD" + +/** DLC of Command Channel. */ +#define COMMAND_CHANNEL_DLC (sizeof(Command)) + +/** Name of Channel to send system status to. */ +#define STATUS_CHANNEL_NAME "STATUS" + +/** DLC of Status Channel */ +#define STATUS_CHANNEL_DLC (sizeof(Status)) + +/** Name of Channel to receive Motor Speed Setpoints. */ +#define SPEED_SETPOINT_CHANNEL_NAME "SPEED_SET" + +/** DLC of Speedometer Channel */ +#define SPEED_SETPOINT_CHANNEL_DLC (sizeof(SpeedData)) + +/** Name of the Channel to receive Line Sensor Data from. */ +#define LINE_SENSOR_CHANNEL_NAME "LINE_SENS" + +/** DLC of Line Sensor Channel */ +#define LINE_SENSOR_CHANNEL_DLC (sizeof(LineSensorData)) + +/** Name of channel to send system Mode to */ +#define MODE_CHANNEL_NAME "MODE" + +/** DLC of Mode Channel */ +#define MODE_CHANNEL_DLC (sizeof(Mode)) + + + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** SerialMuxProt Server with fixed template argument. */ +typedef SerialMuxProtServer SMPServer; + +/** Channel payload constants. */ +namespace SMPChannelPayload +{ + /** Remote control commands. */ + typedef enum : uint8_t + { + CMD_ID_IDLE = 0, /**< Nothing to do. */ + CMD_ID_SET_READY_STATE, /**< Set Ready State. */ + CMD_ID_SET_TRAINING_WAIT_STATE, /**< Set Training Wait State. */ + + } CmdId; /**< Command ID */ + + /** Status flags. */ + typedef enum : uint8_t + { + NOT_DONE = 0, /**< End Line Not Detected. */ + DONE /**< End Line Detected. */ + + } Status; /**< Status flag */ + + typedef enum : uint8_t + { + TRAINING_MODE = 0, /**< Driving Mode Selected. */ + DRIVING_MODE /**< Training Mode Selected. */ + + } Mode; /**< Status flag */ + +} + + +/** Struct of the "Speed" channel payload. */ +typedef struct _SpeedData +{ + int16_t left; /**< Left motor speed [steps/s] */ + int16_t right; /**< Right motor speed [steps/s] */ +} __attribute__((packed)) SpeedData; + +/** Struct of the "Mode" channel payload. */ +typedef struct _Mode +{ + SMPChannelPayload::Mode mode; /**< Mode */ +} __attribute__((packed)) Mode; + +/** Struct of the "Status" channel payload. */ +typedef struct _Status +{ + SMPChannelPayload::Status status; /**< Status */ +} __attribute__((packed)) Status; + +/** Struct of the "Command" channel payload. */ +typedef struct _Command +{ + SMPChannelPayload::CmdId commandId; /**< Command ID */ + +} __attribute__((packed)) Command; + +/** Struct of the "Line Sensor" channel payload. */ +typedef struct _LineSensorData +{ + uint16_t lineSensorData[5U]; /**< Line sensor data [digits] normalized to max 1000 digits. */ +} __attribute__((packed)) LineSensorData; + +/****************************************************************************** + * Functions + *****************************************************************************/ + #endif /* SERIAL_MUX_CHANNELS_H_ */ \ No newline at end of file diff --git a/lib/APPReinforcementLearning/StartupState.cpp b/lib/APPReinforcementLearning/src/StartupState.cpp similarity index 94% rename from lib/APPReinforcementLearning/StartupState.cpp rename to lib/APPReinforcementLearning/src/StartupState.cpp index 7c1a7377..b7c1b09b 100644 --- a/lib/APPReinforcementLearning/StartupState.cpp +++ b/lib/APPReinforcementLearning/src/StartupState.cpp @@ -1,199 +1,197 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Startup state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "StartupState.h" -#include -#include -#include "MotorSpeedCalibrationState.h" -#include "LineSensorsCalibrationState.h" -#include "Sound.h" -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void StartupState::entry() -{ - Board& board = Board::getInstance(); - ISettings& settings = board.getSettings(); - int16_t maxMotorSpeed = settings.getMaxSpeed(); - - /* If max. motor speed calibration value is available, the differential - * drive will be enabled. - */ - if (0 < maxMotorSpeed) - { - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - - /* With setting the max. motor speed in [steps/s] the differential drive control - * can now be used. - */ - diffDrive.setMaxMotorSpeed(maxMotorSpeed); - - /* Differential drive can now be used. */ - diffDrive.enable(); - - m_isMaxMotorSpeedCalibAvailable = true; - } - showUserInfo(m_userInfoState); -} - -void StartupState::process(StateMachine& sm) -{ - Board& board = Board::getInstance(); - IButton& buttonA = board.getButtonA(); - - /* Start max. motor speed calibration? */ - if (true == buttonA.isPressed()) - { - buttonA.waitForRelease(); - sm.setState(&MotorSpeedCalibrationState::getInstance()); - } - - /* If the max. motor speed calibration is done, it will be possible to - * start the line sensor calibration immediately. - */ - if (true == m_isMaxMotorSpeedCalibAvailable) - { - IButton& buttonB = board.getButtonB(); - - /* Start line sensor calibration? */ - if (true == buttonB.isPressed()) - { - buttonB.waitForRelease(); - sm.setState(&LineSensorsCalibrationState::getInstance()); - } - } - - /* Periodically change the user info on the display. */ - if (true == m_timer.isTimeout()) - { - int8_t next = m_userInfoState + 1; - - if (USER_INFO_COUNT <= next) - { - next = 0; - } - - showUserInfo(static_cast(next)); - m_timer.restart(); - } -} - -void StartupState::exit() -{ - /* Next time start again from begin with the info. */ - m_userInfoState = USER_INFO_TEAM_NAME; -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -void StartupState::showUserInfo(UserInfo next) -{ - Board& board = Board::getInstance(); - IDisplay& display = board.getDisplay(); - ISettings& settings = board.getSettings(); - - display.clear(); - - switch (next) - { - case USER_INFO_TEAM_NAME: - display.print(TEAM_NAME_LINE_1); - display.gotoXY(0, 1); - display.print(TEAM_NAME_LINE_2); - break; - - case USER_INFO_MAX_MOTOR_SPEED: - display.print("maxSpeed"); - display.gotoXY(0, 1); - display.print(settings.getMaxSpeed()); - break; - - case USER_INFO_UI: - display.print("A: MCAL"); - if (true == m_isMaxMotorSpeedCalibAvailable) - { - display.gotoXY(0, 1); - display.print("B: LCAL"); - } - break; - - case USER_INFO_COUNT: - /* fallthrough */ - default: - display.print("?"); - next = USER_INFO_TEAM_NAME; - break; - } - - m_userInfoState = next; - - m_timer.start(INFO_DURATION); -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Startup state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "StartupState.h" +#include +#include +#include "MotorSpeedCalibrationState.h" +#include "LineSensorsCalibrationState.h" +#include "Sound.h" +#include +#include +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void StartupState::entry() +{ + Board& board = Board::getInstance(); + ISettings& settings = board.getSettings(); + int16_t maxMotorSpeed = settings.getMaxSpeed(); + + /* If max. motor speed calibration value is available, the differential + * drive will be enabled. + */ + if (0 < maxMotorSpeed) + { + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + + /* With setting the max. motor speed in [steps/s] the differential drive control + * can now be used. + */ + diffDrive.setMaxMotorSpeed(maxMotorSpeed); + + /* Differential drive can now be used. */ + diffDrive.enable(); + + m_isMaxMotorSpeedCalibAvailable = true; + } + showUserInfo(m_userInfoState); +} + +void StartupState::process(StateMachine& sm) +{ + Board& board = Board::getInstance(); + IButton& buttonA = board.getButtonA(); + + /* Start max. motor speed calibration? */ + if (true == Util::isButtonTriggered(buttonA, m_isButtonAPressed)) + { + sm.setState(&MotorSpeedCalibrationState::getInstance()); + } + + /* If the max. motor speed calibration is done, it will be possible to + * start the line sensor calibration immediately. + */ + if (true == m_isMaxMotorSpeedCalibAvailable) + { + IButton& buttonB = board.getButtonB(); + + /* Start line sensor calibration? */ + if (true == Util::isButtonTriggered(buttonB, m_isButtonBPressed)) + { + sm.setState(&LineSensorsCalibrationState::getInstance()); + } + } + + /* Periodically change the user info on the display. */ + if (true == m_timer.isTimeout()) + { + int8_t next = m_userInfoState + 1; + + if (USER_INFO_COUNT <= next) + { + next = 0; + } + + showUserInfo(static_cast(next)); + m_timer.restart(); + } +} + +void StartupState::exit() +{ + /* Next time start again from begin with the info. */ + m_userInfoState = USER_INFO_TEAM_NAME; +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +void StartupState::showUserInfo(UserInfo next) +{ + Board& board = Board::getInstance(); + IDisplay& display = board.getDisplay(); + ISettings& settings = board.getSettings(); + + display.clear(); + + switch (next) + { + case USER_INFO_TEAM_NAME: + display.print(TEAM_NAME_LINE_1); + display.gotoXY(0, 1); + display.print(TEAM_NAME_LINE_2); + break; + + case USER_INFO_MAX_MOTOR_SPEED: + display.print("maxSpeed"); + display.gotoXY(0, 1); + display.print(settings.getMaxSpeed()); + break; + + case USER_INFO_UI: + display.print("A: MCAL"); + if (true == m_isMaxMotorSpeedCalibAvailable) + { + display.gotoXY(0, 1); + display.print("B: LCAL"); + } + break; + + case USER_INFO_COUNT: + /* fallthrough */ + default: + display.print("?"); + next = USER_INFO_TEAM_NAME; + break; + } + + m_userInfoState = next; + + m_timer.start(INFO_DURATION); +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPReinforcementLearning/StartupState.h b/lib/APPReinforcementLearning/src/StartupState.h similarity index 90% rename from lib/APPReinforcementLearning/StartupState.h rename to lib/APPReinforcementLearning/src/StartupState.h index f718292a..e464693e 100644 --- a/lib/APPReinforcementLearning/StartupState.h +++ b/lib/APPReinforcementLearning/src/StartupState.h @@ -1,148 +1,149 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Startup state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef STARTUP_STATE_H -#define STARTUP_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The system startup state. */ -class StartupState : public IState -{ -public: - /** - * Get state instance. - * - * @return State instance. - */ - static StartupState& getInstance() - { - static StartupState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - -protected: -private: - /** - * This type defines different kind of information, which will be shown - * to the user in the same order as defined. - */ - enum UserInfo - { - USER_INFO_TEAM_NAME = 0, /**< Show the team name. */ - USER_INFO_MAX_MOTOR_SPEED, /**< Show the max. motor speed. */ - USER_INFO_UI, /**< Show the user interface. */ - USER_INFO_COUNT /**< Number of user infos. */ - }; - - /** - * Duration in ms how long a info on the display shall be shown, until - * the next info appears. - */ - static const uint32_t INFO_DURATION = 2000; - - bool m_isMaxMotorSpeedCalibAvailable; /**< Is max. motor speed calibration value available? */ - SimpleTimer m_timer; /**< Used to show information for a certain time before changing to the next info. */ - UserInfo m_userInfoState; /**< Current user info state. */ - - /** - * Default constructor. - */ - StartupState() : m_isMaxMotorSpeedCalibAvailable(false), m_timer(), m_userInfoState(USER_INFO_TEAM_NAME) - { - } - - /** - * Default destructor. - */ - ~StartupState() - { - } - - /* Not allowed. */ - StartupState(const StartupState& state); /**< Copy construction of an instance. */ - StartupState& operator=(const StartupState& state); /**< Assignment of an instance. */ - - /** - * Show next user info. - * - * @param[in] next Next user info which to show. - */ - void showUserInfo(UserInfo next); -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* STARTUP_STATE_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Startup state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef STARTUP_STATE_H +#define STARTUP_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The system startup state. */ +class StartupState : public IState +{ +public: + /** + * Get state instance. + * + * @return State instance. + */ + static StartupState& getInstance() + { + static StartupState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + +protected: +private: + /** + * This type defines different kind of information, which will be shown + * to the user in the same order as defined. + */ + enum UserInfo + { + USER_INFO_TEAM_NAME = 0, /**< Show the team name. */ + USER_INFO_MAX_MOTOR_SPEED, /**< Show the max. motor speed. */ + USER_INFO_UI, /**< Show the user interface. */ + USER_INFO_COUNT /**< Number of user infos. */ + }; + + /** + * Duration in ms how long a info on the display shall be shown, until + * the next info appears. + */ + static const uint32_t INFO_DURATION = 2000; + + bool m_isMaxMotorSpeedCalibAvailable; /**< Is max. motor speed calibration value available? */ + SimpleTimer m_timer; /**< Used to show information for a certain time before changing to the next info. */ + UserInfo m_userInfoState; /**< Current user info state. */ + bool m_isButtonAPressed; /**< Is the button A pressed (last time)? */ + bool m_isButtonBPressed; /**< Is the button B pressed (last time)? */ + /** + * Default constructor. + */ + StartupState() : m_isMaxMotorSpeedCalibAvailable(false), m_timer(), m_userInfoState(USER_INFO_TEAM_NAME),m_isButtonAPressed(false),m_isButtonBPressed(false) + { + } + + /** + * Default destructor. + */ + ~StartupState() + { + } + + /* Not allowed. */ + StartupState(const StartupState& state); /**< Copy construction of an instance. */ + StartupState& operator=(const StartupState& state); /**< Assignment of an instance. */ + + /** + * Show next user info. + * + * @param[in] next Next user info which to show. + */ + void showUserInfo(UserInfo next); +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* STARTUP_STATE_H */ +/** @} */ From 12cdd39de808faea94e2615ba221aba8fe3599d7 Mon Sep 17 00:00:00 2001 From: Akram Date: Fri, 26 Jul 2024 11:40:54 +0200 Subject: [PATCH 25/58] Use updated SerialMuxProt and replace status channel with CMS channel in supervisor --- .../RL_Supervisor/RL_Supervisor.py | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/webots/controllers/RL_Supervisor/RL_Supervisor.py b/webots/controllers/RL_Supervisor/RL_Supervisor.py index fc331140..f18bee55 100644 --- a/webots/controllers/RL_Supervisor/RL_Supervisor.py +++ b/webots/controllers/RL_Supervisor/RL_Supervisor.py @@ -3,7 +3,7 @@ """ import sys from Serial_webots import SerialWebots -from SerialMuxProt import SerialMuxProt +from SerialMuxProt import Server from controller import Supervisor import struct @@ -44,12 +44,13 @@ # SerialMuxProt Server Instance. S_client = SerialWebots(supervisor_com_Tx, supervisor_com_rx) -smp_server = SerialMuxProt(10, S_client) +smp_server = Server(10, S_client) #DLC of SPEED_SET Channel SPEED_SET_DLC = 4 + #DLC of STATUS Channel -STATUS_DLC = 1 +CMD_DLC = 1 # Counter for the number of times no line has been detected noLineDetectionCount = 0 @@ -76,7 +77,7 @@ def callback_Status(payload: bytearray) -> int: if payload[0] == 1: print("the max. time within the robot must be finished its drive is Done") smp_server.send_data("SPEED_SET", struct.pack('2H', 0, 0)) # Stop motors - smp_server.send_data("STATUS",struct.pack('B', 1)) + smp_server.send_data("CMD",struct.pack('B', 1)) reinitialize() @@ -97,10 +98,11 @@ def callback_LineSensors(payload: bytearray)-> None: if noLineDetectionCount >= 20 or all(value >= Line_Sensor_On_Track_Min_Value for value in sensor_data): smp_server.send_data("SPEED_SET", struct.pack('2H', 0, 0)) # Stop motors, maximum NO Line Detection Counter reached - smp_server.send_data("STATUS",struct.pack('B', 1)) # SEND STATUS DONE + smp_server.send_data("CMD",struct.pack('B', 1)) # SENDING A COMMAND ID SET READY STATUS Reset_Count += 1 else: smp_server.send_data("SPEED_SET", struct.pack('2H', 1000, 1000)) + def callback_Mode(payload: bytearray)-> None: """ Callback MODE Channel""" @@ -111,8 +113,11 @@ def callback_Mode(payload: bytearray)-> None: print("Train Mode Selected") +# Channel creation. m_serialMuxProtChannelIdSPEED_SET = smp_server.create_channel("SPEED_SET", SPEED_SET_DLC) -m_serialMuxProtChannelIdSTATUS = smp_server.create_channel("STATUS", STATUS_DLC) +m_serialMuxProtChannelIdCMD = smp_server.create_channel("CMD", CMD_DLC) + +# Channel subscription. smp_server.subscribe_to_channel("STATUS", callback_Status) smp_server.subscribe_to_channel("LINE_SENS",callback_LineSensors) smp_server.subscribe_to_channel("MODE",callback_Mode) @@ -127,7 +132,7 @@ def main_loop(): status = 0 m_elapsedTimeSinceReset = 0 - if m_serialMuxProtChannelIdSPEED_SET == 0 or m_serialMuxProtChannelIdSTATUS == 0 : + if m_serialMuxProtChannelIdSPEED_SET == 0 or m_serialMuxProtChannelIdCMD == 0 : print("Channel SPEED_SET not created.") else: if robot_node is None: From 9e8a3cf85cca5afcc5feaca6ef301a44bebc3638 Mon Sep 17 00:00:00 2001 From: Akram Date: Fri, 26 Jul 2024 11:50:18 +0200 Subject: [PATCH 26/58] Add new track and create RL linefollower world --- webots/protos/NTrack.png | Bin 0 -> 22053 bytes webots/protos/RL_LineFollowerTrack.proto | 40 ++++++++++ webots/worlds/RL_LineFollower.wbt | 92 +++++++++++------------ 3 files changed, 86 insertions(+), 46 deletions(-) create mode 100644 webots/protos/NTrack.png create mode 100644 webots/protos/RL_LineFollowerTrack.proto diff --git a/webots/protos/NTrack.png b/webots/protos/NTrack.png new file mode 100644 index 0000000000000000000000000000000000000000..760362f8aed39f5d3d71bcd61af32c4aac398aa8 GIT binary patch literal 22053 zcmeHPc}!GC7@q}PR}NXU6_i7Y2ev9K5s}md7Nk*P0b}cd7+h>bEeNaF78-OPqEh2o z+e)kGTK~|rQbbhPaL7X}v_V}PT8+L7tPKne zB;@RVtKR4lkRGL35Rpj8xGD5c60&%5J0VL7LV^R|Pl{?ap0y)#!mC908v zZi%bRp%RsDJQOjMl26g9Ip#pG&J44Z_9 zhGU5!f{Bc?2bT?k>m?z$Q4riAk!=7+LTW&e8Xz@*nSj&)sR6(MsR2?0bVN|^2~q>3 z1^@%}8Xz?wNDYu0&^-aY2Iw^a7$7x3Y5*`mYJk)LsR7*=(QAOz0KEouCLlFHY5*`m zYJk)LsR3OOks2U1_^(g{O>JRXlh5bVxD#pp{km^12YR?KUA_H)MZTf#>m?uhguZt_ zKA*_s6t+}U91G~h%)%6vyP9Gg8T|6nf_^s2#8ovAfjz*?6vZZSECCA$B7(CA9H_W# z;Ce}Lqre>!_5`E`NDYu0P@?=iYM?%>U4Bd=tU0-U#W7Boa)Rxmn&}XDMhmw29Y3?5 zckrJ8Q&;Z&HlVev*^xF&YJi(3O`E4pnxWgo6f3DhmZnYe&~0W4NY{Q<8c=j7d@fCM z8%Kd+M`?i5?r0jEBp);*opuCU^g6WJF(W1*RNn^*Vlj zMCserV&AdQw7$--Y{8Au^%Xtr$6Z1K7?>5)E~|+4WA)nIq2B2@XCGsXa5gKHHcvdD zOiFAe_sb(Q!tcLVL@` z_mw5@qNNBapA1#4!1w?=mgbXA*8M8w!sExA8G{6;tXQh$yz~;ng<6klRAwy`6<~XJ z&Z94LqTMnL#*bP~vC8DyLPGetW!k%Ap&O9%Fj$J-^wiJ9>;kY{6#6_JGOPL(F8QLu z8%j!Jx^3@_5oY3PmWu~aJ zVIxE1bQ!fSzo!)@n5yZ9RqKU3hm41|RH=zKBV4NfqN6^e3rv99p%1)Uc;G4c(InAC z4R80-g`0d-Q8>%n7ea5pW~;gVt&vR845KT`Q^ZaxbQPo)C!b-NPWvG!>9R>_EM{qq z|FAB^22`9F!WEAS&s`5xgM;;|9JKY7-l3Ia#oImuuoFD)D?LS6lA?oy!|_b)qEU-D$nFCVu}B`&(Pg#kesceoGfhhAUV6p&YW}< zaLVD70~oyMlxuIG2MFHtmK6mPX`TcpbY5uym46XC^pkz_Sq{AnC%Ns{+O`Zk^EzyA zXpug;y@j1)HSZ2P&bF6cXyCe7OMEhbS$f+TFGSsf?m#Nwf8_{s!ul`iv$QnLB@P^E zM0l$evDs}OTRN1Vi@90YH<6YQYKI+891cVh&XC#1&fn{Xy{%O*ikFbB;3A;aj?aU0 zPQWg5yt0S6hSu4Ek09=k@1o`f-Hj;BJ}NfgYbznV5KI&TJU}c<}8FxP?ilxgHSNA1L+S4P`cjS%nofjre*F*x&_&);-;hWbSVaptX4Y$-4fBBLv4t z>Ek^6B#)NYsFuD+IvwYCGvP>TMXnF)QxC(DW2_Fm6Q1W-oAFR|Z|K0V`YVE6)RQi# zxJ?Fm=cRELEz1*FTUl>VkCxxggSxtdf_*GQ2se|~+|U+1CC65m%!+-G9NA~a$L!pr zQC7o)1k;j$7xLNlQgWuLYBbfTR{|JZf@H+d^aWE;J8h>}a21_zvS-HcfTw=S&Ybs8 z5j{Vey`adDoAVn|a@lRPJ42A}D&rdd3}z!FEFA(^5?Da{0da>%Aj(tD7g6b|8pwSb z&7xYx4Q9XEdk-6i7hP0vEKMjv5WVougdjo?!CoT@u0#a4Gx(IAu@?#n+{cl?01WV3 z33LDe3`{huw;|j4I-ETe)`zvf)pSVVW&RV4e(3l*w2lmIEdN-rDBpRTduQz4hR}Yx O>V&)#7MvZV-u5@@2W2q; literal 0 HcmV?d00001 diff --git a/webots/protos/RL_LineFollowerTrack.proto b/webots/protos/RL_LineFollowerTrack.proto new file mode 100644 index 00000000..22d7cf74 --- /dev/null +++ b/webots/protos/RL_LineFollowerTrack.proto @@ -0,0 +1,40 @@ +#VRML_SIM R2023a utf8 + +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/floors/protos/RectangleArena.proto" + +PROTO RL_LineFollowerTrack [ + field SFString contactMaterial "default" # Is `Solid.contactMaterial`. +] +{ + RectangleArena { + contactMaterial IS contactMaterial + floorSize 2 2 + floorTileSize 4 4 + floorAppearance PBRAppearance { + baseColorMap ImageTexture { + url [ + "./NTrack.png" + ] + } + roughness 1 + roughnessMap ImageTexture { + url [ + "https://raw.githubusercontent.com/cyberbotics/webots/R2022a/projects/appearances/protos/textures/parquetry/chequered_parquetry_roughness.jpg" + ] + } + metalness 0 + normalMap ImageTexture { + url [ + "https://raw.githubusercontent.com/cyberbotics/webots/R2022a/projects/appearances/protos/textures/parquetry/chequered_parquetry_normal.jpg" + ] + } + occlusionMap ImageTexture { + url [ + "https://raw.githubusercontent.com/cyberbotics/webots/R2022a/projects/appearances/protos/textures/parquetry/chequered_parquetry_occlusion.jpg" + ] + } + } + wallThickness 0.001 + wallHeight 0.05 + } +} \ No newline at end of file diff --git a/webots/worlds/RL_LineFollower.wbt b/webots/worlds/RL_LineFollower.wbt index e3dfdac2..10c104da 100644 --- a/webots/worlds/RL_LineFollower.wbt +++ b/webots/worlds/RL_LineFollower.wbt @@ -1,46 +1,46 @@ -#VRML_SIM R2023b utf8 - -EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/backgrounds/protos/TexturedBackground.proto" -EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto" -EXTERNPROTO "../protos/Zumo32U4.proto" -EXTERNPROTO "../protos/LineFollowerTrack.proto" -IMPORTABLE EXTERNPROTO "../protos/Supervisor.proto" - -WorldInfo { - info [ - "Example uses of Track nodes, as caterpillar tracks on a robot, or as a conveyor belt." - ] - title "Track" - basicTimeStep 8 - contactProperties [ - ContactProperties { - material1 "rubber" - material2 "cardboard" - coulombFriction [ - 0.65 - ] - } - ] -} -Viewpoint { - orientation 0.3787583266277594 0.09714544028170935 -0.9203830145339561 2.677946076660944 - position 1.2569587324159737 0.7945022889765716 1.5111964909293643 -} -TexturedBackground { -} -TexturedBackgroundLight { -} -LineFollowerTrack { - contactMaterial "cardboard" -} -DEF ROBOT Zumo32U4 { - translation -0.24713614078815466 -0.04863962992854465 0.013994298332013683 - rotation -1.0564747468923541e-06 8.746699709178704e-07 0.9999999999990595 1.5880805820884731 - name "Zumo" - contactMaterial "rubber" -} -Supervisor { - name "RL_Supervisor" - controller "RL_Supervisor" - supervisor TRUE -} +#VRML_SIM R2023b utf8 + +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/backgrounds/protos/TexturedBackground.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto" +EXTERNPROTO "../protos/Zumo32U4.proto" +EXTERNPROTO "../protos/RL_LineFollowerTrack.proto" +IMPORTABLE EXTERNPROTO "../protos/Supervisor.proto" + +WorldInfo { + info [ + "Example uses of Track nodes, as caterpillar tracks on a robot, or as a conveyor belt." + ] + title "Track" + basicTimeStep 8 + contactProperties [ + ContactProperties { + material1 "rubber" + material2 "cardboard" + coulombFriction [ + 0.65 + ] + } + ] +} +Viewpoint { + orientation 0.3787583266277594 0.09714544028170935 -0.9203830145339561 2.677946076660944 + position 1.2569587324159737 0.7945022889765716 1.5111964909293643 +} +TexturedBackground { +} +TexturedBackgroundLight { +} +RL_LineFollowerTrack { + contactMaterial "cardboard" +} +DEF ROBOT Zumo32U4 { + translation -0.24713614078815466 -0.04863962992854465 0.013994298332013683 + rotation -1.0564747468923541e-06 8.746699709178704e-07 0.9999999999990595 1.5880805820884731 + name "Zumo" + contactMaterial "rubber" +} +Supervisor { + name "RL_Supervisor" + controller "RL_Supervisor" + supervisor TRUE +} From 71f3fb049a7ceb476279709ad5fa034bb805aa1f Mon Sep 17 00:00:00 2001 From: Akram Date: Fri, 26 Jul 2024 15:17:48 +0200 Subject: [PATCH 27/58] Import stream into SerialWebots and rename variables --- .../RL_Supervisor/Serial_webots.py | 104 ++++++++---------- 1 file changed, 46 insertions(+), 58 deletions(-) diff --git a/webots/controllers/RL_Supervisor/Serial_webots.py b/webots/controllers/RL_Supervisor/Serial_webots.py index 0863f06e..a0e27d9c 100644 --- a/webots/controllers/RL_Supervisor/Serial_webots.py +++ b/webots/controllers/RL_Supervisor/Serial_webots.py @@ -2,38 +2,35 @@ # Imports ################################################################################ from controller import device -################################################################################ -# Variables -################################################################################ +from SerialMuxProt import Stream ################################################################################ # Classes ################################################################################ - -class SerialWebots: +class SerialWebots(Stream): """ Serial Webots Communication Class """ - def __init__(self, Emitter: device, Receiver: device)-> None: - """ SerialWebots Constructor. + def __init__(self, emitter: device, receiver: device) -> None: + """ + SerialWebots Constructor. - Parameters + Parameters ---------- - Emitter : device + emitter : device Name of Emitter Device - Receiver : device + receiver : device Name of Receiver Device - """ - self.m_Emitter = Emitter - self.m_Receiver = Receiver - self.buffer = bytearray() + self.__emitter = emitter + self.__receiver = receiver + self.__buffer = bytearray() - - def write(self, payload : bytearray ) -> int: - """ Sends Data to the Server. + def write(self, payload: bytearray) -> int: + """ + Sends Data to the Server. Parameters ---------- @@ -42,69 +39,60 @@ def write(self, payload : bytearray ) -> int: Returns ---------- - Number of bytes sent + int + Number of bytes sent """ - self.m_Emitter.send(bytes(payload)) - bytes_sent= len(payload) - + self.__emitter.send(bytes(payload)) + bytes_sent = len(payload) return bytes_sent def available(self) -> int: - """ Check if there is anything available for reading + """ + Check if there is anything available for reading. Returns ---------- - Number of bytes that are available for reading. - + int + Number of bytes that are available for reading. """ - if len(self.buffer) > 0: - return len(self.buffer) - elif self.m_Receiver.getQueueLength() > 0: - return self.m_Receiver.getDataSize() - return 0 + if len(self.__buffer) > 0: + return len(self.__buffer) + elif self.__receiver.getQueueLength() > 0: + return self.__receiver.getDataSize() + return 0 def read_bytes(self, length: int) -> tuple[int, bytearray]: - """ Read a given number of Bytes from Serial. + """ + Read a given number of Bytes from Serial. Returns ---------- - Tuple: - - int: Number of bytes received. - - bytearray: Received data. + tuple[int, bytearray] + - int: Number of bytes received. + - bytearray: Received data. """ - read = 0 data = bytearray() - if len(self.buffer) > 0: - read = min(len(self.buffer), length) - data = self.buffer[:read] - self.buffer = self.buffer[read:] - elif self.m_Receiver.getQueueLength() > 0: - receivedData = self.m_Receiver.getBytes() - receivedData_size = self.m_Receiver.getDataSize() - self.m_Receiver.nextPacket() - - if receivedData_size > length: - data = receivedData[:length] - self.buffer = receivedData[length:] + if len(self.__buffer) > 0: + read = min(len(self.__buffer), length) + data = self.__buffer[:read] + self.__buffer = self.__buffer[read:] + elif self.__receiver.getQueueLength() > 0: + received_data = self.__receiver.getBytes() + received_data_size = self.__receiver.getDataSize() + self.__receiver.nextPacket() + + if received_data_size > length: + data = received_data[:length] + self.__buffer = received_data[length:] read = length else: - data = receivedData - read = receivedData_size + data = received_data + read = received_data_size return read, data - - - - - - - - - - ################################################################################ # Functions ################################################################################ From afd6b14d6e71e27dc5847f1944be84020fd24276 Mon Sep 17 00:00:00 2001 From: Akram Date: Fri, 26 Jul 2024 15:21:30 +0200 Subject: [PATCH 28/58] Rename variables and customize code formatting in the Supervisor class. --- .../RL_Supervisor/RL_Supervisor.py | 168 ++++++++++-------- 1 file changed, 91 insertions(+), 77 deletions(-) diff --git a/webots/controllers/RL_Supervisor/RL_Supervisor.py b/webots/controllers/RL_Supervisor/RL_Supervisor.py index f18bee55..7b888d2d 100644 --- a/webots/controllers/RL_Supervisor/RL_Supervisor.py +++ b/webots/controllers/RL_Supervisor/RL_Supervisor.py @@ -1,20 +1,30 @@ -"""Supervisor controller. - Details: https://github.com/cyberbotics/webots/blob/master/docs/guide/supervisor-programming.md """ +Supervisor controller. + +Details: https://github.com/cyberbotics/webots/blob/master/docs/guide/supervisor-programming.md +""" + import sys +import struct +from controller import Supervisor from Serial_webots import SerialWebots from SerialMuxProt import Server -from controller import Supervisor -import struct -# The PROTO DEF name must be given! +# Constants ROBOT_NAME = "ROBOT" - -# The supervisor receiver name. SUPERVISOR_RX_NAME = "serialComRx" - -# The supervisor Emitter name. SUPERVISOR_TX_NAME = "serialComTx" +COMMAND_CHANNEL_NAME = "CMD" +CMD_DLC = 1 +SPEED_SETPOINT_CHANNEL_NAME = "SPEED_SET" +SPEED_SET_DLC = 4 +LINE_SENSOR_CHANNEL_NAME = "LINE_SENS" +STATUS_CHANNEL_NAME = "STATUS" +MODE_CHANNEL_NAME = "MODE" +CMD_ID_SET_READY_STATE = 1 +LINE_SENSOR_ON_TRACK_MIN_VALUE = 200 +POSITION_DATA = [-0.24713614078815466, -0.04863962992854465, 0.013994298332013683] +ORIENTATION_DATA = [-1.0564747468923541e-06, 8.746699709178704e-07, 0.9999999999990595, 1.5880805820884731] # Create the Supervisor instance. supervisor = Supervisor() @@ -25,102 +35,98 @@ # Get the Supervisor Receiver Device supervisor_com_rx = supervisor.getDevice(SUPERVISOR_RX_NAME) -# Get the Supervisor Emitter device -supervisor_com_Tx = supervisor.getDevice(SUPERVISOR_TX_NAME) +# Get the Supervisor Emitter device +supervisor_com_tx = supervisor.getDevice(SUPERVISOR_TX_NAME) # Set serial rx/tx channels for communication with the Roboter. supervisor_com_rx.setChannel(2) -supervisor_com_Tx.setChannel(1) - -# Enable supervisor receiver to receive any message from the robot or other -# devices in the simulation. -if supervisor_com_rx is None: - print(f"No supervisor communication possible, because {SUPERVISOR_RX_NAME} not found.") -else: - supervisor_com_rx.enable(timestep) +supervisor_com_tx.setChannel(1) # Get robot node which to observe. robot_node = supervisor.getFromDef(ROBOT_NAME) # SerialMuxProt Server Instance. -S_client = SerialWebots(supervisor_com_Tx, supervisor_com_rx) -smp_server = Server(10, S_client) - -#DLC of SPEED_SET Channel -SPEED_SET_DLC = 4 +s_client = SerialWebots(supervisor_com_tx, supervisor_com_rx) +smp_server = Server(10, s_client) -#DLC of STATUS Channel -CMD_DLC = 1 +# Channel creation. +serialMuxProtChannelIdSPEED_SET = smp_server.create_channel(SPEED_SETPOINT_CHANNEL_NAME, SPEED_SET_DLC) +serialMuxProtChannelIdCMD = smp_server.create_channel(COMMAND_CHANNEL_NAME, CMD_DLC) # Counter for the number of times no line has been detected -noLineDetectionCount = 0 - -# The line sensor threshold. -Line_Sensor_On_Track_Min_Value = 200 +no_line_detection_count = 0 # The counter of how much it has been reset. -Reset_Count = 0 +reset_count = 0 + +# Enable supervisor receiver to receive any message from the robot or other +# devices in the simulation. +if supervisor_com_rx is None: + print(f"No supervisor communication possible, because {SUPERVISOR_RX_NAME} not found.") +else: + supervisor_com_rx.enable(timestep) def reinitialize (): - """ Re-initialization of position and orientation of The ROBOT """ - global robot_node,Reset_Count + """ Re-initialization of position and orientation of The ROBOT. + """ + global reset_count trans_field = robot_node.getField("translation") rot_field = robot_node.getField("rotation") - intial_position =[-0.24713614078815466, -0.04863962992854465, 0.013994298332013683] - initial_orientation =[-1.0564747468923541e-06, 8.746699709178704e-07, 0.9999999999990595, 1.5880805820884731] + intial_position = POSITION_DATA + initial_orientation = ORIENTATION_DATA trans_field.setSFVec3f(intial_position) rot_field.setSFRotation(initial_orientation) - Reset_Count = 0 + reset_count = 0 def callback_Status(payload: bytearray) -> int: - """ Callback Status Channel """ + """ Callback Status Channel. + """ if payload[0] == 1: - print("the max. time within the robot must be finished its drive is Done") - smp_server.send_data("SPEED_SET", struct.pack('2H', 0, 0)) # Stop motors - smp_server.send_data("CMD",struct.pack('B', 1)) - reinitialize() + print("the max. time within the robot must be finished its drive is Done") + smp_server.send_data(SPEED_SETPOINT_CHANNEL_NAME, struct.pack('2H', 0, 0)) # Stop motors + smp_server.send_data(COMMAND_CHANNEL_NAME,struct.pack('B', CMD_ID_SET_READY_STATE)) # SENDING A COMMAND_ID SET READY STATUS + reinitialize() def callback_LineSensors(payload: bytearray)-> None: - """ Callback LINE_SENS Channel""" - global noLineDetectionCount, Line_Sensor_On_Track_Min_Value, Reset_Count + """ Callback LINE_SENS Channel. + """ + global no_line_detection_count, reset_count sensor_data = struct.unpack('5H', payload) + for idx in range (5): - if idx == 4: - print(f"Sensor[{idx}] = {sensor_data[idx]}") - else : - print(f"Sensor[{idx}] = {sensor_data[idx]}, ", end="") + if idx == 4: + print(f"Sensor[{idx}] = {sensor_data[idx]}") + else: + print(f"Sensor[{idx}] = {sensor_data[idx]}, ", end="") if all(value == 0 for value in sensor_data): - noLineDetectionCount += 1 + no_line_detection_count += 1 else: - noLineDetectionCount = 0 - - if noLineDetectionCount >= 20 or all(value >= Line_Sensor_On_Track_Min_Value for value in sensor_data): - smp_server.send_data("SPEED_SET", struct.pack('2H', 0, 0)) # Stop motors, maximum NO Line Detection Counter reached - smp_server.send_data("CMD",struct.pack('B', 1)) # SENDING A COMMAND ID SET READY STATUS - Reset_Count += 1 + no_line_detection_count = 0 + + if no_line_detection_count >= 20 or all(value >= LINE_SENSOR_ON_TRACK_MIN_VALUE for value in sensor_data): + + # Stop motors, maximum NO Line Detection Counter reached + smp_server.send_data(SPEED_SETPOINT_CHANNEL_NAME, struct.pack('2H', 0, 0)) + + # SENDING A COMMAND ID SET READY STATUS + smp_server.send_data(COMMAND_CHANNEL_NAME,struct.pack('B', CMD_ID_SET_READY_STATE)) + reset_count += 1 + else: - smp_server.send_data("SPEED_SET", struct.pack('2H', 1000, 1000)) - + # Set left and right motors speed to 1000 + smp_server.send_data(SPEED_SETPOINT_CHANNEL_NAME, struct.pack('2H', 1000, 1000)) def callback_Mode(payload: bytearray)-> None: - """ Callback MODE Channel""" + """ Callback MODE Channel. + """ driving_mode = payload[0] + if driving_mode: - print("Driving Mode Selected") + print("Driving Mode Selected.") else: - print("Train Mode Selected") - - -# Channel creation. -m_serialMuxProtChannelIdSPEED_SET = smp_server.create_channel("SPEED_SET", SPEED_SET_DLC) -m_serialMuxProtChannelIdCMD = smp_server.create_channel("CMD", CMD_DLC) - -# Channel subscription. -smp_server.subscribe_to_channel("STATUS", callback_Status) -smp_server.subscribe_to_channel("LINE_SENS",callback_LineSensors) -smp_server.subscribe_to_channel("MODE",callback_Mode) + print("Train Mode Selected.") def main_loop(): """Main loop: @@ -130,20 +136,28 @@ def main_loop(): number: Status """ status = 0 - m_elapsedTimeSinceReset = 0 + elapsedTimeSinceReset = 0 # Elapsed time since reset [ms] + + # Channel subscription. + smp_server.subscribe_to_channel(STATUS_CHANNEL_NAME, callback_Status) + smp_server.subscribe_to_channel(LINE_SENSOR_CHANNEL_NAME, callback_LineSensors) + smp_server.subscribe_to_channel(MODE_CHANNEL_NAME, callback_Mode) - if m_serialMuxProtChannelIdSPEED_SET == 0 or m_serialMuxProtChannelIdCMD == 0 : + if serialMuxProtChannelIdSPEED_SET == 0 or serialMuxProtChannelIdCMD == 0 : print("Channel SPEED_SET not created.") else: + if robot_node is None: print(f"Robot DEF {ROBOT_NAME} not found.") - status = -1 + status = -1 + else: - while supervisor.step(timestep) != -1: - m_elapsedTimeSinceReset += timestep - smp_server.process(m_elapsedTimeSinceReset) - if Reset_Count != 0: - reinitialize() + while supervisor.step(timestep) != -1: + elapsedTimeSinceReset += timestep + smp_server.process(elapsedTimeSinceReset) + + if reset_count != 0: + reinitialize() return status sys.exit(main_loop()) \ No newline at end of file From afbc8b0cd95c5cd878cfa9346256cd42b305e4a7 Mon Sep 17 00:00:00 2001 From: Akram Date: Fri, 26 Jul 2024 15:53:42 +0200 Subject: [PATCH 29/58] Rename Track name to GaplessTrack --- webots/protos/{NTrack.png => GaplessTrack.png} | Bin webots/protos/RL_LineFollowerTrack.proto | 2 +- 2 files changed, 1 insertion(+), 1 deletion(-) rename webots/protos/{NTrack.png => GaplessTrack.png} (100%) diff --git a/webots/protos/NTrack.png b/webots/protos/GaplessTrack.png similarity index 100% rename from webots/protos/NTrack.png rename to webots/protos/GaplessTrack.png diff --git a/webots/protos/RL_LineFollowerTrack.proto b/webots/protos/RL_LineFollowerTrack.proto index 22d7cf74..928267b9 100644 --- a/webots/protos/RL_LineFollowerTrack.proto +++ b/webots/protos/RL_LineFollowerTrack.proto @@ -13,7 +13,7 @@ PROTO RL_LineFollowerTrack [ floorAppearance PBRAppearance { baseColorMap ImageTexture { url [ - "./NTrack.png" + "./GaplessTrack.png" ] } roughness 1 From 6e0dd8fc7148c7e8960173c42704f5f85c5128a4 Mon Sep 17 00:00:00 2001 From: Akram Date: Fri, 26 Jul 2024 16:08:25 +0200 Subject: [PATCH 30/58] Move the cpp and header board file to the src file and delete the cpp APPReinforcementLearning file. --- lib/APPReinforcementLearning/App.cpp | 265 ------------- lib/HALReinforcementLearningSim/Board.cpp | 134 ------- lib/HALReinforcementLearningSim/Board.h | 374 ------------------ lib/HALReinforcementLearningSim/src/Board.cpp | 3 + lib/HALReinforcementLearningSim/src/Board.h | 17 +- 5 files changed, 14 insertions(+), 779 deletions(-) delete mode 100644 lib/APPReinforcementLearning/App.cpp delete mode 100644 lib/HALReinforcementLearningSim/Board.cpp delete mode 100644 lib/HALReinforcementLearningSim/Board.h diff --git a/lib/APPReinforcementLearning/App.cpp b/lib/APPReinforcementLearning/App.cpp deleted file mode 100644 index 7c6a33cb..00000000 --- a/lib/APPReinforcementLearning/App.cpp +++ /dev/null @@ -1,265 +0,0 @@ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief LineFollower application with Reinforcement Learning - * @author Akram Bziouech - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "App.h" -#include "DrivingState.h" -#include "StartupState.h" -#include -#include -#include -#include "ErrorState.h" -#include -#include -#include -#include -#include -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - - -/****************************************************************************** - * Prototypes - *****************************************************************************/ -static void App_motorSpeedSetpointsChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData); - -/* Initialization of the static variable*/ -App* App::appInstance = nullptr; - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - - -void App::setup() -{ - Serial.begin(SERIAL_BAUDRATE); - /* Initialize HAL */ - Board::getInstance().init(); - Logging::disable(); - if (false == setupSerialMuxProt()) - { - ErrorState::getInstance().setErrorMsg("SMP=0"); - m_systemStateMachine.setState(&ErrorState::getInstance()); - } - else - { - /* Setup the state machine with the first state. */ - m_systemStateMachine.setState(&StartupState::getInstance()); - m_statusTimer.start(SEND_STATUS_TIMER_INTERVAL); - m_sendLineSensorsDataInterval.start(SEND_LINE_SENSORS_DATA_PERIOD); - /* Setup the periodically processing of robot control. */ - m_controlInterval.start(DIFFERENTIAL_DRIVE_CONTROL_PERIOD); - } - -#ifdef DEBUG_ODOMETRY - /* Reset supervisor which set its observed position and orientation to 0. */ - Board::getInstance().getSender().send("RST"); -#endif /* DEBUG_ODOMETRY */ - - /* Surprise the audience. */ - Sound::playMelody(Sound::MELODY_WELCOME); -} - -void App::loop() -{ - Board::getInstance().process(); - Speedometer::getInstance().process(); - - if (true == m_controlInterval.isTimeout()) - { - /* The differential drive control needs the measured speed of the - * left and right wheel. Therefore it shall be processed after - * the speedometer. - */ - DifferentialDrive::getInstance().process(DIFFERENTIAL_DRIVE_CONTROL_PERIOD); - - /* The odometry unit needs to detect motor speed changes to be able to - * calculate correct values. Therefore it shall be processed right after - * the differential drive control. - */ - Odometry::getInstance().process(); - -#ifdef DEBUG_ODOMETRY - { - Odometry& odo = Odometry::getInstance(); - int32_t posX = 0; - int32_t posY = 0; - int32_t orientation = odo.getOrientation(); - const size_t BUFFER_SIZE = 128U; - char buffer[BUFFER_SIZE]; - - odo.getPosition(posX, posY); - - snprintf(buffer, BUFFER_SIZE, "ODO,%d,%d,%d", posX, posY, orientation); - - Board::getInstance().getSender().send(buffer); - } -#endif /* DEBUG_ODOMETRY */ - - m_controlInterval.restart(); - } - - if ((true == m_statusTimer.isTimeout()) && (true == m_smpServer.isSynced())&& (&DrivingState::getInstance() == m_systemStateMachine.getState())) - { - Status payload = {SMPChannelPayload::Status::NOT_DONE}; - - if (DrivingState::getInstance().isAbortRequired() == true) - { - payload = {SMPChannelPayload::Status::DONE}; - } - (void)m_smpServer.sendData(m_serialMuxProtChannelIdStatus, &payload, sizeof(payload)); - m_statusTimer.restart(); - } - /* Send periodically line sensor data. */ - if (true == m_sendLineSensorsDataInterval.isTimeout() && (&DrivingState::getInstance() == m_systemStateMachine.getState()) ) - { - sendLineSensorsData(); - - m_sendLineSensorsDataInterval.restart(); - } - /* Send Mode selected to The Supervisor. */ - if (&ReadyState::getInstance() == m_systemStateMachine.getState() && (!m_sentmode)) - { - int modeSelection = ReadyState::getInstance().setSelectedMode(); - if(modeSelection > 0) - { - SMPChannelPayload::Mode payload = (modeSelection == 1) ? SMPChannelPayload::Mode::DRIVING_MODE : SMPChannelPayload::Mode::TRAINING_MODE; - (void)m_smpServer.sendData(m_serialMuxProtChannelIdMode, &payload, sizeof(payload)); - m_sentmode = true; - } - } - - - m_smpServer.process(millis()); - - m_systemStateMachine.process(); -} - - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -bool App::setupSerialMuxProt() -{ - bool isSuccessful = false; - m_smpServer.subscribeToChannel(SPEED_SETPOINT_CHANNEL_NAME, App_motorSpeedSetpointsChannelCallback); - m_smpServer.subscribeToChannel(STATUS_CHANNEL_NAME,App_statusChannelCallback); - /* Channel creation. */ - m_serialMuxProtChannelIdStatus = m_smpServer.createChannel(STATUS_CHANNEL_NAME, STATUS_CHANNEL_DLC); - m_serialMuxProtChannelIdLineSensors = m_smpServer.createChannel(LINE_SENSOR_CHANNEL_NAME, LINE_SENSOR_CHANNEL_DLC); - m_serialMuxProtChannelIdMode = m_smpServer.createChannel(MODE_CHANNEL_NAME, MODE_CHANNEL_DLC); - - - /* Channels succesfully created? */ - if ((0U != m_serialMuxProtChannelIdStatus) && (0U != m_serialMuxProtChannelIdLineSensors) && (0U != m_serialMuxProtChannelIdMode)) - { - isSuccessful = true; - } - - return isSuccessful; -} - -void App::sendLineSensorsData() const -{ - ILineSensors& lineSensors = Board::getInstance().getLineSensors(); - uint8_t maxLineSensors = lineSensors.getNumLineSensors(); - const uint16_t* lineSensorValues = lineSensors.getSensorValues(); - uint8_t lineSensorIdx = 0U; - LineSensorData payload; - - if (LINE_SENSOR_CHANNEL_DLC == maxLineSensors * sizeof(uint16_t)) - { - while (maxLineSensors > lineSensorIdx) - { - payload.lineSensorData[lineSensorIdx] = lineSensorValues[lineSensorIdx]; - - ++lineSensorIdx; - } - } - - (void)m_smpServer.sendData(m_serialMuxProtChannelIdLineSensors, &payload, sizeof(payload)); -} - -void App::systemStatusCallback(SMPChannelPayload::Status status) -{ - switch (status) - { - case SMPChannelPayload::NOT_DONE: - /* Nothing to do. All good. */ - break; - case SMPChannelPayload::DONE: - Odometry::getInstance().clearPosition(); - Odometry::getInstance().clearMileage(); - m_systemStateMachine.setState(&ReadyState::getInstance()); - m_sentmode = false; - - break; - - default: - break; - } -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ -/** - * Receives current status of the DCS over SerialMuxProt channel. - * - * @param[in] payload Status of the DCS. - * @param[in] payloadSize Size of the Status Flag - * @param[in] userData Instance of App class. - */ -void App::App_statusChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData) -{ - - if ((nullptr != payload) && (STATUS_CHANNEL_DLC == payloadSize) && (nullptr != App::appInstance)) - { - const Status* currentStatus = reinterpret_cast(payload); - App::appInstance->systemStatusCallback(currentStatus->status); - } -} - -void App_motorSpeedSetpointsChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData) -{ - if ((nullptr != payload) && (SPEED_SETPOINT_CHANNEL_DLC == payloadSize)) - { - const SpeedData* motorSpeedData = reinterpret_cast(payload); - //printf("Payload %d", motorSpeedData->left); - DrivingState::getInstance().setTargetSpeeds(motorSpeedData->left, motorSpeedData->right); - - } -} \ No newline at end of file diff --git a/lib/HALReinforcementLearningSim/Board.cpp b/lib/HALReinforcementLearningSim/Board.cpp deleted file mode 100644 index d2516f1c..00000000 --- a/lib/HALReinforcementLearningSim/Board.cpp +++ /dev/null @@ -1,134 +0,0 @@ -/* MIT License - * - * Copyright (c) 2019 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief The simulation robot board realization. - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void Board::init() -{ - m_encoders.init(); - m_lineSensors.init(); - m_motors.init(); - m_settings.init(); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -Board::Board() : - IBoard(), - m_robot(), - m_WebotsSerialDrv(m_robot.getEmitter(RobotDeviceNames::EMITTER_NAME_SERIAL), - m_robot.getReceiver(RobotDeviceNames::RECEIVER_NAME_SERIAL)), - m_simTime(m_robot), - m_keyboard(m_simTime, m_robot.getKeyboard()), - m_buttonA(m_keyboard), - m_buttonB(m_keyboard), - m_buttonC(m_keyboard), - m_buzzer(m_robot.getSpeaker(RobotDeviceNames::SPEAKER_NAME)), - m_display(m_robot.getDisplay(RobotDeviceNames::DISPLAY_NAME)), - m_encoders(m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME), - m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)), - m_lineSensors( - m_robot.getEmitter(RobotDeviceNames::EMITTER_0_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_1_NAME), - m_robot.getEmitter(RobotDeviceNames::EMITTER_2_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_3_NAME), - m_robot.getEmitter(RobotDeviceNames::EMITTER_4_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)), - m_motors(m_robot.getMotor(RobotDeviceNames::LEFT_MOTOR_NAME), m_robot.getMotor(RobotDeviceNames::RIGHT_MOTOR_NAME)), - m_ledRed(m_robot.getLED(RobotDeviceNames::LED_RED_NAME)), - m_ledYellow(m_robot.getLED(RobotDeviceNames::LED_YELLOW_NAME)), - m_ledGreen(m_robot.getLED(RobotDeviceNames::LED_GREEN_NAME)), - m_settings() -#ifdef DEBUG_ODOMETRY - , - m_sender(m_robot.getEmitter(SENDER_NAME)) -#endif /* DEBUG_ODOMETRY */ -{ -} - -void Board::enableSimulationDevices() -{ - const int timeStep = m_simTime.getTimeStep(); - - m_robot.getKeyboard()->enable(timeStep); - m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME)->enable(timeStep); - m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)->enable(timeStep); - m_robot.getReceiver(RobotDeviceNames::RECEIVER_NAME_SERIAL)->enable(timeStep); -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ \ No newline at end of file diff --git a/lib/HALReinforcementLearningSim/Board.h b/lib/HALReinforcementLearningSim/Board.h deleted file mode 100644 index 993f8f89..00000000 --- a/lib/HALReinforcementLearningSim/Board.h +++ /dev/null @@ -1,374 +0,0 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief The simulation robot board realization. - * @author Andreas Merkle - * - * @addtogroup HALSim - * - * @{ - */ -#ifndef BOARD_H -#define BOARD_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#ifdef DEBUG_ODOMETRY -#include -#endif /* DEBUG_ODOMETRY */ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** - * The concrete simulation robot board. - */ -class Board : public IBoard -{ -public: - /** - * Get board instance. - * - * @return Board instance - */ - static Board& getInstance() - { - static Board instance; /* idiom */ - - return instance; - } - - /** - * Initialize the hardware. - */ - void init() final; - - /** - * Get button A driver. - * - * @return Button A driver. - */ - IButton& getButtonA() final - { - return m_buttonA; - } - - /** - * Get button B driver. - * - * @return Button B driver. - */ - IButton& getButtonB() final - { - return m_buttonB; - } - - /** - * Get button C driver. - * - * @return Button C driver. - */ - IButton& getButtonC() final - { - return m_buttonC; - } - - /** - * Get buzzer driver. - * - * @return Buzzer driver. - */ - IBuzzer& getBuzzer() final - { - return m_buzzer; - } - - /** - * Get LCD driver. - * - * @return LCD driver. - */ - IDisplay& getDisplay() final - { - return m_display; - } - - /** - * Get encoders. - * - * @return Encoders driver. - */ - IEncoders& getEncoders() final - { - return m_encoders; - } - - /** - * Get line sensors driver. - * - * @return Line sensor driver. - */ - ILineSensors& getLineSensors() final - { - return m_lineSensors; - } - - /** - * Get motor driver. - * - * @return Motor driver. - */ - IMotors& getMotors() final - { - return m_motors; - } - - /** - * Get red LED driver. - * - * @return Red LED driver. - */ - ILed& getRedLed() final - { - return m_ledRed; - } - - /** - * Get yellow LED driver. - * - * @return Yellow LED driver. - */ - ILed& getYellowLed() final - { - return m_ledYellow; - } - - /** - * Get green LED driver. - * - * @return Green LED driver. - */ - ILed& getGreenLed() final - { - return m_ledGreen; - } - - /** - * Get the settings. - * - * @return Settings - */ - ISettings& getSettings() final - { - return m_settings; - } - - -#ifdef DEBUG_ODOMETRY - - /** - * Get the sender driver, used to send data to the webots supervisor. - * - * @return Sender driver - */ - ISender& getSender() final - { - return m_sender; - } - -#endif /* DEBUG_ODOMETRY */ - - /** - * Process actuators and sensors. - */ - void process() final - { - m_buzzer.process(); - } - WebotsSerialDrv* getSimSerial() - { - - return &m_WebotsSerialDrv; - - } - - -private: - - /** Simulated roboter instance. */ - webots::Robot m_robot; - - /** Simulation time handler */ - SimTime m_simTime; - - /** Own keyboard that wraps the webots keyboard. */ - Keyboard m_keyboard; - - /** Button A driver */ - ButtonA m_buttonA; - - /** Button B driver */ - ButtonB m_buttonB; - - /** Button C driver */ - ButtonC m_buttonC; - - /** Buzzer driver */ - Buzzer m_buzzer; - - /** Display driver */ - Display m_display; - - /** Encoders driver */ - Encoders m_encoders; - - /** Line sensors driver */ - LineSensors m_lineSensors; - - /** Motors driver */ - Motors m_motors; - - /** Red LED driver */ - LedRed m_ledRed; - - /** Red LED driver */ - LedYellow m_ledYellow; - - /** Red LED driver */ - LedGreen m_ledGreen; - - /** Settings */ - Settings m_settings; - - WebotsSerialDrv m_WebotsSerialDrv; - - - -#ifdef DEBUG_ODOMETRY - - /** Sender driver for supervisor communication. */ - Sender m_sender; - -#endif /* DEBUG_ODOMETRY */ - - /** - * Constructs the concrete board. - */ - Board(); - - /** - * Destroys the concrete board. - */ - ~Board() - { - } - /** - * Get the simulation time handler. - * - * @return Simulation time handler - */ - SimTime& getSimTime() - { - return m_simTime; - } - - /** - * Get the keyboard instance of the simulation. - * - * @return The keyboard. - */ - Keyboard& getKeyboard() - { - return m_keyboard; - } - - /** - * Get the simulation serial driver, which is connected within Webots. - * - * @return If serial driver is available, it will return a pointer to it, otherwise nullptr. - */ - - - - /** - * Enable all simulation devices. - * It is called by the main entry only. - * Devices must be enabled before they can be used, and a simulation step must be performed before the application - * initialization. - */ - void enableSimulationDevices(); - - /** - * The main entry needs access to the simulation robot instance. - * But all other application parts shall have no access, which is - * solved by this friend. - * - * @param[in] argc Number of arguments - * @param[in] argv Arguments - * - * @return Exit code - */ - friend int main(int argc, char** argv); -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* BOARD_H */ -/** @} */ diff --git a/lib/HALReinforcementLearningSim/src/Board.cpp b/lib/HALReinforcementLearningSim/src/Board.cpp index b1fd5e5d..ac66608d 100644 --- a/lib/HALReinforcementLearningSim/src/Board.cpp +++ b/lib/HALReinforcementLearningSim/src/Board.cpp @@ -100,6 +100,8 @@ Board::Board() : m_ledRed(m_robot.getLED(RobotDeviceNames::LED_RED_NAME)), m_ledYellow(m_robot.getLED(RobotDeviceNames::LED_YELLOW_NAME)), m_ledGreen(m_robot.getLED(RobotDeviceNames::LED_GREEN_NAME)), + m_serialDrv(m_robot.getEmitter(RobotDeviceNames::EMITTER_NAME_SERIAL), + m_robot.getReceiver(RobotDeviceNames::RECEIVER_NAME_SERIAL)), m_settings() #ifdef DEBUG_ODOMETRY , @@ -120,6 +122,7 @@ void Board::enableSimulationDevices() m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME)->enable(timeStep); m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME)->enable(timeStep); m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)->enable(timeStep); + m_robot.getReceiver(RobotDeviceNames::RECEIVER_NAME_SERIAL)->enable(timeStep); } /****************************************************************************** diff --git a/lib/HALReinforcementLearningSim/src/Board.h b/lib/HALReinforcementLearningSim/src/Board.h index 992e5147..90cb0911 100644 --- a/lib/HALReinforcementLearningSim/src/Board.h +++ b/lib/HALReinforcementLearningSim/src/Board.h @@ -218,6 +218,7 @@ class Board : public IBoard return m_settings; } + #ifdef DEBUG_ODOMETRY /** @@ -239,7 +240,7 @@ class Board : public IBoard { m_buzzer.process(); } - + private: /** Simulated roboter instance. */ @@ -287,6 +288,11 @@ class Board : public IBoard /** Settings */ Settings m_settings; + /** Simulation serial driver */ + WebotsSerialDrv m_serialDrv; + + + #ifdef DEBUG_ODOMETRY /** Sender driver for supervisor communication. */ @@ -299,13 +305,12 @@ class Board : public IBoard */ Board(); - /** + /** * Destroys the concrete board. */ - ~Board() + ~Board() { } - /** * Get the simulation time handler. * @@ -333,10 +338,10 @@ class Board : public IBoard */ WebotsSerialDrv* getSimSerial() { - /* Not supported. */ - return nullptr; + return &m_serialDrv; } + /** * Enable all simulation devices. * It is called by the main entry only. From 410582d2c796d3b1a66d4dbb9e951f32acdd7d01 Mon Sep 17 00:00:00 2001 From: Akram Date: Fri, 26 Jul 2024 16:20:43 +0200 Subject: [PATCH 31/58] Re-add missing license to cpp und Header APPReinforcementLearning file --- lib/APPReinforcementLearning/src/App.cpp | 25 ++++++++++++++++++++++-- lib/APPReinforcementLearning/src/App.h | 22 +++++++++++++++++++++ 2 files changed, 45 insertions(+), 2 deletions(-) diff --git a/lib/APPReinforcementLearning/src/App.cpp b/lib/APPReinforcementLearning/src/App.cpp index 77ce9b60..59eecbae 100644 --- a/lib/APPReinforcementLearning/src/App.cpp +++ b/lib/APPReinforcementLearning/src/App.cpp @@ -1,3 +1,25 @@ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ /******************************************************************************* DESCRIPTION @@ -22,8 +44,7 @@ #include #include #include -#include -#include "TrainingWaitState.h" +#include "ReadyState.h" /****************************************************************************** * Compiler Switches *****************************************************************************/ diff --git a/lib/APPReinforcementLearning/src/App.h b/lib/APPReinforcementLearning/src/App.h index 995d8d5b..a3a972cc 100644 --- a/lib/APPReinforcementLearning/src/App.h +++ b/lib/APPReinforcementLearning/src/App.h @@ -1,3 +1,25 @@ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ /******************************************************************************* DESCRIPTION From 3295a7e0e5af5108967cdd22c11b761e2024f47b Mon Sep 17 00:00:00 2001 From: Akram Date: Mon, 29 Jul 2024 11:01:44 +0200 Subject: [PATCH 32/58] Temporary renaming from Supervisor.py to temp.py --- webots/controllers/Supervisor/temp.py | 206 ++++++++++++++++++++++++++ 1 file changed, 206 insertions(+) create mode 100644 webots/controllers/Supervisor/temp.py diff --git a/webots/controllers/Supervisor/temp.py b/webots/controllers/Supervisor/temp.py new file mode 100644 index 00000000..10b7d06c --- /dev/null +++ b/webots/controllers/Supervisor/temp.py @@ -0,0 +1,206 @@ +"""Supervisor controller. + Details: https://github.com/cyberbotics/webots/blob/master/docs/guide/supervisor-programming.md +""" + +import math +import sys +from dataclasses import dataclass +from controller import Supervisor # pylint: disable=import-error +from robot_observer import RobotObserver, has_position_changed, has_orientation_changed + +# The PROTO DEF name must be given! +ROBOT_NAME = "ROBOT" + +# The supervisor receiver name. +SUPERVISOR_RX_NAME = "serialComRx" + + +def serial_com_read(serial_com_rx_device): + """Read from serial communication device. + + Args: + serial_com_rx_device (obj): Serial COM device + + Returns: + Union[str, None]: Received data or None. + """ + rx_data = None + + if serial_com_rx_device.getQueueLength() > 0: + rx_data = serial_com_rx_device.getString() + serial_com_rx_device.nextPacket() + + return rx_data + + +def rad_to_deg(angle_rad): + """Convert angle in rad to degree. + + Args: + angle_rad (float): Angle in rad + + Returns: + float: Angle in degree + """ + return angle_rad * 180 / math.pi + + +def convert_angle_to_2pi(angle): + """Convert angle from range [-PI; PI] in rad to [0; 2PI]. + + Args: + angle (float): Angle in rad, range [-PI; PI]. + + Returns: + float: Angle in rad, range [0; 2PI] + """ + if angle < 0: + angle += 2 * math.pi + + return angle + + +def convert_webots_angle_to_ru_angle(webots_angle): + """Convert Webots angle to RadonUlzer angle. + + Args: + webots_angle (float): Webots angle in rad, range [-PI; PI]. + + Returns: + float: Angle in rad, range [-2PI; 2PI] + """ + # Radon Ulzer + # Angle [-2PI; 2PI] + # North are 90° (PI/2) + # + # Webots angle [-PI; PI] + webots_angle += math.pi / 2 + + webots_angle_2pi = convert_angle_to_2pi(webots_angle) + + # TO DO Handling the negative range. + + return webots_angle_2pi + + +@dataclass +class OdometryData(): + """Odometry data container. + """ + x: int = 0 + y: int = 0 + yaw_angle: float = 0 + + def reset(self): + """Reset odometry data. + """ + self.x = 0 + self.y = 0 + self.yaw_angle = 0 + + +# pylint: disable=too-many-branches +# pylint: disable=too-many-statements +# pylint: disable=too-many-locals +def main_loop(): + """Main loop: + - Perform simulation steps until Webots is stopping the controller- + + Returns: + number: Status + """ + status = 0 + + # Create the Supervisor instance. + supervisor = Supervisor() + + # Get the time step of the current world. + timestep = int(supervisor.getBasicTimeStep()) + + # Enable supervisor receiver to receive any message from the robot or other + # devices in the simulation. + supervisor_com_rx = supervisor.getDevice(SUPERVISOR_RX_NAME) + + if supervisor_com_rx is None: + print( + f"No supervisor communication possible, because {SUPERVISOR_RX_NAME} not found.") + else: + supervisor_com_rx.enable(timestep) + + # Get robot node which to observe. + robot_node = supervisor.getFromDef(ROBOT_NAME) + + if robot_node is None: + + print(f"Robot DEF {ROBOT_NAME} not found.") + status = -1 + + else: + robot_observer = RobotObserver(robot_node) + + # Set current position and orientation in the map as reference. + robot_observer.set_current_position_as_reference() + robot_observer.set_current_orientation_as_reference() + + robot_position_old = robot_observer.get_rel_position() + robot_orientation_old = robot_observer.get_rel_orientation() + robot_odometry = OdometryData() + + while supervisor.step(timestep) != -1: + if supervisor_com_rx is not None: + rx_data = serial_com_read(supervisor_com_rx) + + if rx_data is None: + pass + + else: + command = rx_data.split(',') + + # Reset robot position and orientation? + if command[0] == "RST": + print("RST") + robot_observer.set_current_position_as_reference() + robot_observer.set_current_orientation_as_reference() + robot_odometry.reset() + + # Robot odometry data received? + elif command[0] == "ODO": + robot_odometry.x = int(command[1]) # [mm] + robot_odometry.y = int(command[2]) # [mm] + robot_odometry.yaw_angle = float( + command[3]) / 1000.0 # [rad] + + # Unknown command. + else: + print(f"Unknown command: {command[0]}") + + robot_position = robot_observer.get_rel_position() + robot_orientation = robot_observer.get_rel_orientation() + + any_change = has_position_changed( + robot_position, robot_position_old) + + if any_change is False: + any_change = has_orientation_changed( + robot_orientation, robot_orientation_old) + + if any_change is True: + + robot_yaw_angle = robot_orientation[2] + yaw_ru_angle = convert_webots_angle_to_ru_angle( + robot_yaw_angle) + + print(f"{int(robot_position[0])}, ", end="") + print(f"{int(robot_position[1])}, ", end="") + print(f"{int(rad_to_deg(yaw_ru_angle))} / ", end="") + print(f"{robot_odometry.x}, ", end="") + print(f"{robot_odometry.y}, ", end="") + print(f"{int(rad_to_deg(robot_odometry.yaw_angle))}") + + robot_position_old = robot_position + robot_orientation_old = robot_orientation + + return status + + +sys.exit(main_loop()) \ No newline at end of file From 68d91b29fada301d7344b007ea823c30fa6a18ae Mon Sep 17 00:00:00 2001 From: Akram Date: Mon, 29 Jul 2024 11:04:40 +0200 Subject: [PATCH 33/58] Adjustments to the supervisor with the latest update and renaming files - Deleted temp.py - Temporarily renamed Supervisor.py to temp_supervisor.py - Finally renamed temp_supervisor.py to supervisor.py --- webots/controllers/Supervisor/Supervisor.py | 228 ------------------ .../Supervisor/{temp.py => supervisor.py} | 0 2 files changed, 228 deletions(-) delete mode 100644 webots/controllers/Supervisor/Supervisor.py rename webots/controllers/Supervisor/{temp.py => supervisor.py} (100%) diff --git a/webots/controllers/Supervisor/Supervisor.py b/webots/controllers/Supervisor/Supervisor.py deleted file mode 100644 index be0bc9e5..00000000 --- a/webots/controllers/Supervisor/Supervisor.py +++ /dev/null @@ -1,228 +0,0 @@ -"""Supervisor controller. - Details: https://github.com/cyberbotics/webots/blob/master/docs/guide/supervisor-programming.md -""" - -import math -import sys -from controller import Supervisor -from robot_observer import RobotObserver - -# The PROTO DEF name must be given! -ROBOT_NAME = "ROBOT" - -# The supervisor receiver name. -SUPERVISOR_RX_NAME = "serialComRx" - -def serial_com_read(serial_com_rx_device): - """Read from serial communication device. - - Args: - serial_com_rx_device (obj): Serial COM device - - Returns: - Union[str, None]: Received data or None. - """ - rx_data = None - - if serial_com_rx_device.getQueueLength() > 0: - rx_data = serial_com_rx_device.getString() - serial_com_rx_device.nextPacket() - - return rx_data - -def rad_to_deg(angle_rad): - """Convert angle in rad to degree. - - Args: - angle_rad (float): Angle in rad - - Returns: - float: Angle in degree - """ - return angle_rad * 180 / math.pi - -def convert_angle_to_2pi(angle): - """Convert angle from range [-PI; PI] in rad to [0; 2PI]. - - Args: - angle (float): Angle in rad, range [-PI; PI]. - - Returns: - float: Angle in rad, range [0; 2PI] - """ - if angle < 0: - angle += 2 * math.pi - - return angle - -def convert_webots_angle_to_ru_angle(webots_angle): - """Convert Webots angle to RadonUlzer angle. - - Args: - webots_angle (float): Webots angle in rad, range [-PI; PI]. - - Returns: - float: Angle in rad, range [-2PI; 2PI] - """ - # Radon Ulzer - # Angle [-2PI; 2PI] - # North are 90° (PI/2) - # - # Webots angle [-PI; PI] - webots_angle += math.pi / 2 - - webots_angle_2pi = convert_angle_to_2pi(webots_angle) - - # TODO Handling the negative range. - - return webots_angle_2pi - -def has_position_changed(position, position_old): - """Returns whether the position changed. - - Args: - position (list[float]): Position 1 - position_old (list[float]): Position 2 - - Returns: - bool: If changed, it will return True otherwise False. - """ - has_changed = False - - for idx, value in enumerate(position): - if int(value) != int(position_old[idx]): - has_changed = True - break - - return has_changed - -def has_orientation_changed(orientation, orientation_old): - """Returns whether the orientation changed. - - Args: - orientation (list[float]): Orientation 1 - orientation_old (list[float]): Orientation 2 - - Returns: - bool: If changed, it will return True otherwise False. - """ - has_changed = False - - for idx, value in enumerate(orientation): - if int(rad_to_deg(value)) != int(rad_to_deg(orientation_old[idx])): - has_changed = True - break - - return has_changed - -class OdometryData(): # pylint: disable=too-few-public-methods - """Odometry data container. - """ - def __init__(self) -> None: - self.x = 0 - self.y = 0 - self.yaw_angle = 0 - - def reset(self): - """Reset odometry data. - """ - self.x = 0 - self.y = 0 - self.yaw_angle = 0 - -def main_loop(): - """Main loop: - - Perform simulation steps until Webots is stopping the controller- - - Returns: - number: Status - """ - status = 0 - - # Create the Supervisor instance. - supervisor = Supervisor() - - # Get the time step of the current world. - timestep = int(supervisor.getBasicTimeStep()) - - # Enable supervisor receiver to receive any message from the robot or other - # devices in the simulation. - supervisor_com_rx = supervisor.getDevice(SUPERVISOR_RX_NAME) - - if supervisor_com_rx is None: - print(f"No supervisor communication possible, because {SUPERVISOR_RX_NAME} not found.") - else: - supervisor_com_rx.enable(timestep) - - # Get robot node which to observe. - robot_node = supervisor.getFromDef(ROBOT_NAME) - - if robot_node is None: - - print(f"Robot DEF {ROBOT_NAME} not found.") - status = -1 - - else: - robot_observer = RobotObserver(robot_node) - - # Set current position and orientation in the map as reference. - robot_observer.set_current_position_as_reference() - robot_observer.set_current_orientation_as_reference() - - robot_position_old = robot_observer.get_rel_position() - robot_orientation_old = robot_observer.get_rel_orientation() - robot_odometry = OdometryData() - - while supervisor.step(timestep) != -1: - if supervisor_com_rx is not None: - rx_data = serial_com_read(supervisor_com_rx) - - if rx_data is None: - pass - - else: - command = rx_data.split(',') - - # Reset robot position and orientation? - if command[0] == "RST": - print("RST") - robot_observer.set_current_position_as_reference() - robot_observer.set_current_orientation_as_reference() - robot_odometry.reset() - - # Robot odometry data received? - elif command[0] == "ODO": - robot_odometry.x = int(command[1]) # [mm] - robot_odometry.y = int(command[2]) # [mm] - robot_odometry.yaw_angle = float(command[3]) / 1000.0 # [rad] - - # Unknown command. - else: - print(f"Unknown command: {command[0]}") - - robot_position = robot_observer.get_rel_position() - robot_orientation = robot_observer.get_rel_orientation() - - any_change = has_position_changed(robot_position, robot_position_old) - - if any_change is False: - any_change = has_orientation_changed(robot_orientation, robot_orientation_old) - - if any_change is True: - - robot_yaw_angle = robot_orientation[2] - yaw_ru_angle = convert_webots_angle_to_ru_angle(robot_yaw_angle) - - print(f"{int(robot_position[0])}, ", end="") - print(f"{int(robot_position[1])}, ", end="") - print(f"{int(rad_to_deg(yaw_ru_angle))} / ", end="") - print(f"{robot_odometry.x}, ", end="") - print(f"{robot_odometry.y}, ", end="") - print(f"{int(rad_to_deg(robot_odometry.yaw_angle))}") - - robot_position_old = robot_position - robot_orientation_old = robot_orientation - - return status - -sys.exit(main_loop()) \ No newline at end of file diff --git a/webots/controllers/Supervisor/temp.py b/webots/controllers/Supervisor/supervisor.py similarity index 100% rename from webots/controllers/Supervisor/temp.py rename to webots/controllers/Supervisor/supervisor.py From 4dcb5e7d83e8fb80d8239a2cd8f7d2f20c0ee2c0 Mon Sep 17 00:00:00 2001 From: Gabryel Reyes Date: Mon, 29 Jul 2024 15:17:25 +0200 Subject: [PATCH 34/58] Fixed line endings of the UML diagrams --- .../Application.plantuml | 204 ++++++++--------- .../DrivingState.plantuml | 208 +++++++++--------- .../ReinforcementLearning/ErrorState.plantuml | 102 ++++----- .../LineSensorsCalibrationState.plantuml | 180 +++++++-------- .../MotorSpeedCalibrationState.plantuml | 168 +++++++------- .../ReinforcementLearning/ReadyState.plantuml | 128 +++++------ .../TrainigState.plantuml | 206 ++++++++--------- .../SystemStates.plantuml | 84 +++---- 8 files changed, 640 insertions(+), 640 deletions(-) diff --git a/doc/architecture/uml/LogicalView/ReinforcementLearning/Application.plantuml b/doc/architecture/uml/LogicalView/ReinforcementLearning/Application.plantuml index 0dda04dd..9472ec70 100644 --- a/doc/architecture/uml/LogicalView/ReinforcementLearning/Application.plantuml +++ b/doc/architecture/uml/LogicalView/ReinforcementLearning/Application.plantuml @@ -1,103 +1,103 @@ -@startuml - -title Application - -package "Application" as appLayer { - - class App <
> { - + setup() : void - + loop() : void - } - - note left of App - The program entry point. - end note - - class StateMachine <> { - + setState(state : IState*) : void - + getState() : IState* - + process() : void - } - - note left of StateMachine - The state machine executes always one - state exclusive. It is responsible to - enter/leave/process a state. - end note - - interface IState { - + {abstract} entry() : void - + {abstract} process(sm : StateMachine&) : void - + {abstract} exit() : void - } - - note left of IState - Defines the abstract state interface, - which forces every inherited class - to realize it. - end note - - class StartupState <> - class MotorSpeedCalibrationState <> - class LineSensorsCalibrationState <> - class ErrorState <> - class DringState <> - class ReadyState <> - class TrainingState <> - - note bottom of StartupState - The system starts up and shows - the Applikation name on the display. - end note - - note bottom of MotorSpeedCalibrationState - The robot drives with full speed forward - and with full speed backwar to determine - the max speed in steps/s. The slowest - motor is considered! - end note - - note bottom of LineSensorsCalibrationState - The robot turns several times the - line sensors over the track for - calibration. - end note - - note bottom of ErrorState - Error information is shown on display. - Confirmation from operator is requested. - end note - - note bottom of DringState - The robot follows the line. - end note - - note bottom of ReadyState - The robot is stopped and waits for - operator input. - end note - - note bottom of TrainingState - the robot trains to follow the line. - end note -} - -note top of appLayer - Hint: See the application state behaviour - in the corresponding state diagram. -end note - -App *--> StateMachine - -StateMachine o--> "0..1" IState - -IState <|.. StartupState: <> -IState <|.... MotorSpeedCalibrationState: <> -IState <|.. LineSensorsCalibrationState: <> -IState <|.... ErrorState: <> -IState <|.. ReadyState: <> -IState <|.... TrainingState: <> -IState <|.... DringState: <> - - +@startuml + +title Application + +package "Application" as appLayer { + + class App <
> { + + setup() : void + + loop() : void + } + + note left of App + The program entry point. + end note + + class StateMachine <> { + + setState(state : IState*) : void + + getState() : IState* + + process() : void + } + + note left of StateMachine + The state machine executes always one + state exclusive. It is responsible to + enter/leave/process a state. + end note + + interface IState { + + {abstract} entry() : void + + {abstract} process(sm : StateMachine&) : void + + {abstract} exit() : void + } + + note left of IState + Defines the abstract state interface, + which forces every inherited class + to realize it. + end note + + class StartupState <> + class MotorSpeedCalibrationState <> + class LineSensorsCalibrationState <> + class ErrorState <> + class DringState <> + class ReadyState <> + class TrainingState <> + + note bottom of StartupState + The system starts up and shows + the Applikation name on the display. + end note + + note bottom of MotorSpeedCalibrationState + The robot drives with full speed forward + and with full speed backwar to determine + the max speed in steps/s. The slowest + motor is considered! + end note + + note bottom of LineSensorsCalibrationState + The robot turns several times the + line sensors over the track for + calibration. + end note + + note bottom of ErrorState + Error information is shown on display. + Confirmation from operator is requested. + end note + + note bottom of DringState + The robot follows the line. + end note + + note bottom of ReadyState + The robot is stopped and waits for + operator input. + end note + + note bottom of TrainingState + the robot trains to follow the line. + end note +} + +note top of appLayer + Hint: See the application state behaviour + in the corresponding state diagram. +end note + +App *--> StateMachine + +StateMachine o--> "0..1" IState + +IState <|.. StartupState: <> +IState <|.... MotorSpeedCalibrationState: <> +IState <|.. LineSensorsCalibrationState: <> +IState <|.... ErrorState: <> +IState <|.. ReadyState: <> +IState <|.... TrainingState: <> +IState <|.... DringState: <> + + @enduml \ No newline at end of file diff --git a/doc/architecture/uml/LogicalView/ReinforcementLearning/DrivingState.plantuml b/doc/architecture/uml/LogicalView/ReinforcementLearning/DrivingState.plantuml index 4ab50d28..abf49700 100644 --- a/doc/architecture/uml/LogicalView/ReinforcementLearning/DrivingState.plantuml +++ b/doc/architecture/uml/LogicalView/ReinforcementLearning/DrivingState.plantuml @@ -1,105 +1,105 @@ -@startuml - -title Driving State - -package "Application" as appLayer { - - class RacingState <> { - + {static} getInstance() : DrivingState - + entry() : void - + process(sm : StateMachine&) : void - + exit() : void - } - - class ReadyState <> - - RacingState .r.> ReadyState: <> - -} - -package "Service" as serviceLayer { - - class SimpleTimer <> { - + start(duration : uint32_t) : void - + restart() : void - + stop() : void - + isTimeout() : bool - } - class DifferentialDrive <> -} - -package "HAL" as hal { - - package "Interfaces" as halInterfaces { - interface IDisplay { - + {abstract} clear() : void - + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void - + {abstract} print(str : const String&) : size_t - + {abstract} print(str : const char[]) : size_t - + {abstract} print(value : uint8_t) : size_t - + {abstract} print(value : uint16_t) : size_t - + {abstract} print(value : uint32_t) : size_t - + {abstract} print(value : int8_t) : size_t - + {abstract} print(value : int16_t) : size_t - + {abstract} print(value : int32_t) : size_t - } - - interface ILineSensors { - + {abstract} init() : void - + {abstract} calibrate() : void - + {abstract} readLine() : int16_t - + {abstract} getSensorValues() : const uint16_t* - + {abstract} isCalibrationSuccessful() : bool - + {abstract} getCalibErrorInfo() const : uint8_t - + {abstract} getNumLineSensors() const : uint8_t - + {abstract} getSensorValueMax() const : uint16_t - } - - interface ILed { - + {abstract} enable(enableIt : bool) : void - } - - interface IButton { - + {abstract} isPressed() : bool - + {abstract} waitForRelease() : void - } - - } - - class Board << namespace >> { - + getDisplay() : IDisplay& - + getLineSensors() : ILineSensors& - + getLedYellow() : ILed& - + getButtonA() : IButton& - } - class WebotsSerialDrv { - + setRxChannel(channelId: int32_t) : void - + setTxChannel(channelId: int32_t ) : void - + print(str: const char[]) : void - + print(value: uint8_t ) : void - + print(value: uint16_t ) : void - + print(value: uint32_t ) : void - + print(value: int8_t ) : void - + print(value: int16_t ) : void - + print(value: int32_t ) : void - + println(str: const char[]) : void - + println(value: uint8_t ) : void - + println(value: uint16_t ) : void - + println(value: uint32_t ) : void - + println(value: int8_t ) : void - + println(value: int16_t ) : void - + println(value: int32_t ) : void - + write( buffer: const uint8_t*, length: size_t ) : size_t - + readBytes( buffer: uint8_t*, length: size_t ) : size_t - } -} - -RacingState *-> SimpleTimer -RacingState ..> DifferentialDrive: <> -RacingState ...> IDisplay: <> -RacingState ...> ILineSensors: <> -RacingState ...> ILed: <> -RacingState ...> Board: <> -RacingState ...>WebotsSerialDrv: <> -RacingState ...>IButton: <> +@startuml + +title Driving State + +package "Application" as appLayer { + + class RacingState <> { + + {static} getInstance() : DrivingState + + entry() : void + + process(sm : StateMachine&) : void + + exit() : void + } + + class ReadyState <> + + RacingState .r.> ReadyState: <> + +} + +package "Service" as serviceLayer { + + class SimpleTimer <> { + + start(duration : uint32_t) : void + + restart() : void + + stop() : void + + isTimeout() : bool + } + class DifferentialDrive <> +} + +package "HAL" as hal { + + package "Interfaces" as halInterfaces { + interface IDisplay { + + {abstract} clear() : void + + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void + + {abstract} print(str : const String&) : size_t + + {abstract} print(str : const char[]) : size_t + + {abstract} print(value : uint8_t) : size_t + + {abstract} print(value : uint16_t) : size_t + + {abstract} print(value : uint32_t) : size_t + + {abstract} print(value : int8_t) : size_t + + {abstract} print(value : int16_t) : size_t + + {abstract} print(value : int32_t) : size_t + } + + interface ILineSensors { + + {abstract} init() : void + + {abstract} calibrate() : void + + {abstract} readLine() : int16_t + + {abstract} getSensorValues() : const uint16_t* + + {abstract} isCalibrationSuccessful() : bool + + {abstract} getCalibErrorInfo() const : uint8_t + + {abstract} getNumLineSensors() const : uint8_t + + {abstract} getSensorValueMax() const : uint16_t + } + + interface ILed { + + {abstract} enable(enableIt : bool) : void + } + + interface IButton { + + {abstract} isPressed() : bool + + {abstract} waitForRelease() : void + } + + } + + class Board << namespace >> { + + getDisplay() : IDisplay& + + getLineSensors() : ILineSensors& + + getLedYellow() : ILed& + + getButtonA() : IButton& + } + class WebotsSerialDrv { + + setRxChannel(channelId: int32_t) : void + + setTxChannel(channelId: int32_t ) : void + + print(str: const char[]) : void + + print(value: uint8_t ) : void + + print(value: uint16_t ) : void + + print(value: uint32_t ) : void + + print(value: int8_t ) : void + + print(value: int16_t ) : void + + print(value: int32_t ) : void + + println(str: const char[]) : void + + println(value: uint8_t ) : void + + println(value: uint16_t ) : void + + println(value: uint32_t ) : void + + println(value: int8_t ) : void + + println(value: int16_t ) : void + + println(value: int32_t ) : void + + write( buffer: const uint8_t*, length: size_t ) : size_t + + readBytes( buffer: uint8_t*, length: size_t ) : size_t + } +} + +RacingState *-> SimpleTimer +RacingState ..> DifferentialDrive: <> +RacingState ...> IDisplay: <> +RacingState ...> ILineSensors: <> +RacingState ...> ILed: <> +RacingState ...> Board: <> +RacingState ...>WebotsSerialDrv: <> +RacingState ...>IButton: <> @enduml \ No newline at end of file diff --git a/doc/architecture/uml/LogicalView/ReinforcementLearning/ErrorState.plantuml b/doc/architecture/uml/LogicalView/ReinforcementLearning/ErrorState.plantuml index 49c0ce7c..810526a6 100644 --- a/doc/architecture/uml/LogicalView/ReinforcementLearning/ErrorState.plantuml +++ b/doc/architecture/uml/LogicalView/ReinforcementLearning/ErrorState.plantuml @@ -1,52 +1,52 @@ -@startuml - -title Error State - -package "Application" as appLayer { - - class ErrorState <> { - + {static} getInstance() : ErrorState - + entry() : void - + process(sm : StateMachine&) : void - + exit() : void - + setErrorMsg(msg : const String&) : void - } - - class StartupState <> - - ErrorState .r.> StartupState: <> -} - -package "HAL" as hal { - - package "Interfaces" as halInterfaces { - interface IDisplay { - + {abstract} clear() : void - + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void - + {abstract} print(str : const String&) : size_t - + {abstract} print(str : const char[]) : size_t - + {abstract} print(value : uint8_t) : size_t - + {abstract} print(value : uint16_t) : size_t - + {abstract} print(value : uint32_t) : size_t - + {abstract} print(value : int8_t) : size_t - + {abstract} print(value : int16_t) : size_t - + {abstract} print(value : int32_t) : size_t - } - - interface IButton { - + {abstract} isPressed() : bool - + {abstract} waitForRelease() : void - } - } - - class Board << namespace >> { - + getDisplay() : IDisplay& - + getButtonA() : IButton& - } -} - -ErrorState ..> Board: <> -ErrorState ..> IDisplay: <> -ErrorState ..> IButton: <> - +@startuml + +title Error State + +package "Application" as appLayer { + + class ErrorState <> { + + {static} getInstance() : ErrorState + + entry() : void + + process(sm : StateMachine&) : void + + exit() : void + + setErrorMsg(msg : const String&) : void + } + + class StartupState <> + + ErrorState .r.> StartupState: <> +} + +package "HAL" as hal { + + package "Interfaces" as halInterfaces { + interface IDisplay { + + {abstract} clear() : void + + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void + + {abstract} print(str : const String&) : size_t + + {abstract} print(str : const char[]) : size_t + + {abstract} print(value : uint8_t) : size_t + + {abstract} print(value : uint16_t) : size_t + + {abstract} print(value : uint32_t) : size_t + + {abstract} print(value : int8_t) : size_t + + {abstract} print(value : int16_t) : size_t + + {abstract} print(value : int32_t) : size_t + } + + interface IButton { + + {abstract} isPressed() : bool + + {abstract} waitForRelease() : void + } + } + + class Board << namespace >> { + + getDisplay() : IDisplay& + + getButtonA() : IButton& + } +} + +ErrorState ..> Board: <> +ErrorState ..> IDisplay: <> +ErrorState ..> IButton: <> + @enduml \ No newline at end of file diff --git a/doc/architecture/uml/LogicalView/ReinforcementLearning/LineSensorsCalibrationState.plantuml b/doc/architecture/uml/LogicalView/ReinforcementLearning/LineSensorsCalibrationState.plantuml index 8caa4082..77bddb98 100644 --- a/doc/architecture/uml/LogicalView/ReinforcementLearning/LineSensorsCalibrationState.plantuml +++ b/doc/architecture/uml/LogicalView/ReinforcementLearning/LineSensorsCalibrationState.plantuml @@ -1,91 +1,91 @@ -@startuml - -title Line Sensors Calibration State - -package "Application" as appLayer { - - class LineSensorsCalibrationState <> { - + {static} getInstance() : LineSensorsCalibrationState - + entry() : void - + process(sm : StateMachine&) : void - + exit() : void - } - - class ReadyState <> - class ErrorState <> - - LineSensorsCalibrationState ..> ReadyState: <> - LineSensorsCalibrationState ..> ErrorState: <> -} - -package "Service" as serviceLayer { - - class SimpleTimer <> - class RelativeEncoder <> - class DifferentialDrive <><> - class Speedometer <><> - - DifferentialDrive ..> Speedometer: <> -} - -package "HAL" as hal { - - package "Interfaces" as halInterfaces { - interface IDisplay { - + {abstract} clear() : void - + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void - + {abstract} print(str : const String&) : size_t - + {abstract} print(str : const char[]) : size_t - + {abstract} print(value : uint8_t) : size_t - + {abstract} print(value : uint16_t) : size_t - + {abstract} print(value : uint32_t) : size_t - + {abstract} print(value : int8_t) : size_t - + {abstract} print(value : int16_t) : size_t - + {abstract} print(value : int32_t) : size_t - } - - interface IMotors { - + {abstract} setSpeeds(leftSpeed : int16_t, rightSpeed : int16_t) : void - + {abstract} getMaxSpeed() : int16_t - } - - interface ILineSensors { - + {abstract} init() : void - + {abstract} calibrate() : void - + {abstract} readLine() : int16_t - + {abstract} getSensorValues() : const uint16_t* - + {abstract} getNumLineSensors() const : uint8_t - + {abstract} getSensorValueMax() const : uint16_t - } - - interface IEncoders { - + {abstract} getCountsLeft() : int16_t - + {abstract} getCountsRight() : int16_t - + {abstract} getCountsAndResetLeft() : int16_t - + {abstract} getCountsAndResetRight() : int16_t - + {abstract} getResolution() const : uint16_t - } - } - - class Board << namespace >> { - + getDisplay() : IDisplay& - + getMotors() : IMotors& - + getLineSensors() : ILineSensors& - } -} - -appLayer -[hidden]-- serviceLayer -serviceLayer -[hidden]-- hal - -LineSensorsCalibrationState ....> IDisplay: <> -LineSensorsCalibrationState ....> ILineSensors: <> -LineSensorsCalibrationState ....> Board: <> -LineSensorsCalibrationState *--> SimpleTimer -LineSensorsCalibrationState *--> RelativeEncoder -LineSensorsCalibrationState ...> DifferentialDrive: <> - -DifferentialDrive ...> IMotors: <> -Speedometer ..> IEncoders: <> -Speedometer ..> IMotors: <> - +@startuml + +title Line Sensors Calibration State + +package "Application" as appLayer { + + class LineSensorsCalibrationState <> { + + {static} getInstance() : LineSensorsCalibrationState + + entry() : void + + process(sm : StateMachine&) : void + + exit() : void + } + + class ReadyState <> + class ErrorState <> + + LineSensorsCalibrationState ..> ReadyState: <> + LineSensorsCalibrationState ..> ErrorState: <> +} + +package "Service" as serviceLayer { + + class SimpleTimer <> + class RelativeEncoder <> + class DifferentialDrive <><> + class Speedometer <><> + + DifferentialDrive ..> Speedometer: <> +} + +package "HAL" as hal { + + package "Interfaces" as halInterfaces { + interface IDisplay { + + {abstract} clear() : void + + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void + + {abstract} print(str : const String&) : size_t + + {abstract} print(str : const char[]) : size_t + + {abstract} print(value : uint8_t) : size_t + + {abstract} print(value : uint16_t) : size_t + + {abstract} print(value : uint32_t) : size_t + + {abstract} print(value : int8_t) : size_t + + {abstract} print(value : int16_t) : size_t + + {abstract} print(value : int32_t) : size_t + } + + interface IMotors { + + {abstract} setSpeeds(leftSpeed : int16_t, rightSpeed : int16_t) : void + + {abstract} getMaxSpeed() : int16_t + } + + interface ILineSensors { + + {abstract} init() : void + + {abstract} calibrate() : void + + {abstract} readLine() : int16_t + + {abstract} getSensorValues() : const uint16_t* + + {abstract} getNumLineSensors() const : uint8_t + + {abstract} getSensorValueMax() const : uint16_t + } + + interface IEncoders { + + {abstract} getCountsLeft() : int16_t + + {abstract} getCountsRight() : int16_t + + {abstract} getCountsAndResetLeft() : int16_t + + {abstract} getCountsAndResetRight() : int16_t + + {abstract} getResolution() const : uint16_t + } + } + + class Board << namespace >> { + + getDisplay() : IDisplay& + + getMotors() : IMotors& + + getLineSensors() : ILineSensors& + } +} + +appLayer -[hidden]-- serviceLayer +serviceLayer -[hidden]-- hal + +LineSensorsCalibrationState ....> IDisplay: <> +LineSensorsCalibrationState ....> ILineSensors: <> +LineSensorsCalibrationState ....> Board: <> +LineSensorsCalibrationState *--> SimpleTimer +LineSensorsCalibrationState *--> RelativeEncoder +LineSensorsCalibrationState ...> DifferentialDrive: <> + +DifferentialDrive ...> IMotors: <> +Speedometer ..> IEncoders: <> +Speedometer ..> IMotors: <> + @enduml \ No newline at end of file diff --git a/doc/architecture/uml/LogicalView/ReinforcementLearning/MotorSpeedCalibrationState.plantuml b/doc/architecture/uml/LogicalView/ReinforcementLearning/MotorSpeedCalibrationState.plantuml index dc6f3d8e..6ff8397a 100644 --- a/doc/architecture/uml/LogicalView/ReinforcementLearning/MotorSpeedCalibrationState.plantuml +++ b/doc/architecture/uml/LogicalView/ReinforcementLearning/MotorSpeedCalibrationState.plantuml @@ -1,85 +1,85 @@ -@startuml - -title MotorSpeedCalibration State - -package "Application" as appLayer { - - class MotorSpeedCalibrationState <> { - + {static} getInstance() : MotorSpeedCalibrationState - + entry() : void - + process(sm : StateMachine&) : void - + exit() : void - } - - class LineSensorsCalibrationState <> - class ErrorState <> - - MotorSpeedCalibrationState ..> LineSensorsCalibrationState: <> - MotorSpeedCalibrationState ..> ErrorState: <> -} - -package "Service" as serviceLayer { - - class SimpleTimer <> - class RelativeEncoder <> - class DifferentialDrive <> -} - -package "HAL" as hal { - - package "Interfaces" as halInterfaces { - interface IDisplay { - + {abstract} clear() : void - + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void - + {abstract} print(str : const String&) : size_t - + {abstract} print(str : const char[]) : size_t - + {abstract} print(value : uint8_t) : size_t - + {abstract} print(value : uint16_t) : size_t - + {abstract} print(value : uint32_t) : size_t - + {abstract} print(value : int8_t) : size_t - + {abstract} print(value : int16_t) : size_t - + {abstract} print(value : int32_t) : size_t - } - - interface IMotors { - + {abstract} setSpeeds(leftSpeed : int16_t, rightSpeed : int16_t) : void - + {abstract} getMaxSpeed() : int16_t - } - - interface ILineSensors { - + {abstract} init() : void - + {abstract} calibrate() : void - + {abstract} readLine() : int16_t - + {abstract} getSensorValues() : const uint16_t* - + {abstract} getNumLineSensors() const : uint8_t - + {abstract} getSensorValueMax() const : uint16_t - } - - interface ISettings { - + {abstract} init() : void - + {abstract} getMaxSpeed() : int16_t - + {abstract} setMaxSpeed(maxSpeed : int16_t) : void - } - } - - class Board << namespace >> { - + getDisplay() : IDisplay& - + getMotors() : IMotors& - + getLineSensors() : ILineSensors& - + getSettings() : ISettings& - } -} - -appLayer -[hidden]-- serviceLayer -serviceLayer -[hidden]-- hal - -MotorSpeedCalibrationState ....> IDisplay: <> -MotorSpeedCalibrationState ....> IMotors: <> -MotorSpeedCalibrationState ....> ILineSensors: <> -MotorSpeedCalibrationState ....> ISettings: <> -MotorSpeedCalibrationState ....> Board: <> -MotorSpeedCalibrationState *--> SimpleTimer -MotorSpeedCalibrationState *--> RelativeEncoder -MotorSpeedCalibrationState ...> DifferentialDrive: <> - +@startuml + +title MotorSpeedCalibration State + +package "Application" as appLayer { + + class MotorSpeedCalibrationState <> { + + {static} getInstance() : MotorSpeedCalibrationState + + entry() : void + + process(sm : StateMachine&) : void + + exit() : void + } + + class LineSensorsCalibrationState <> + class ErrorState <> + + MotorSpeedCalibrationState ..> LineSensorsCalibrationState: <> + MotorSpeedCalibrationState ..> ErrorState: <> +} + +package "Service" as serviceLayer { + + class SimpleTimer <> + class RelativeEncoder <> + class DifferentialDrive <> +} + +package "HAL" as hal { + + package "Interfaces" as halInterfaces { + interface IDisplay { + + {abstract} clear() : void + + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void + + {abstract} print(str : const String&) : size_t + + {abstract} print(str : const char[]) : size_t + + {abstract} print(value : uint8_t) : size_t + + {abstract} print(value : uint16_t) : size_t + + {abstract} print(value : uint32_t) : size_t + + {abstract} print(value : int8_t) : size_t + + {abstract} print(value : int16_t) : size_t + + {abstract} print(value : int32_t) : size_t + } + + interface IMotors { + + {abstract} setSpeeds(leftSpeed : int16_t, rightSpeed : int16_t) : void + + {abstract} getMaxSpeed() : int16_t + } + + interface ILineSensors { + + {abstract} init() : void + + {abstract} calibrate() : void + + {abstract} readLine() : int16_t + + {abstract} getSensorValues() : const uint16_t* + + {abstract} getNumLineSensors() const : uint8_t + + {abstract} getSensorValueMax() const : uint16_t + } + + interface ISettings { + + {abstract} init() : void + + {abstract} getMaxSpeed() : int16_t + + {abstract} setMaxSpeed(maxSpeed : int16_t) : void + } + } + + class Board << namespace >> { + + getDisplay() : IDisplay& + + getMotors() : IMotors& + + getLineSensors() : ILineSensors& + + getSettings() : ISettings& + } +} + +appLayer -[hidden]-- serviceLayer +serviceLayer -[hidden]-- hal + +MotorSpeedCalibrationState ....> IDisplay: <> +MotorSpeedCalibrationState ....> IMotors: <> +MotorSpeedCalibrationState ....> ILineSensors: <> +MotorSpeedCalibrationState ....> ISettings: <> +MotorSpeedCalibrationState ....> Board: <> +MotorSpeedCalibrationState *--> SimpleTimer +MotorSpeedCalibrationState *--> RelativeEncoder +MotorSpeedCalibrationState ...> DifferentialDrive: <> + @enduml \ No newline at end of file diff --git a/doc/architecture/uml/LogicalView/ReinforcementLearning/ReadyState.plantuml b/doc/architecture/uml/LogicalView/ReinforcementLearning/ReadyState.plantuml index 2690db4e..54776b5e 100644 --- a/doc/architecture/uml/LogicalView/ReinforcementLearning/ReadyState.plantuml +++ b/doc/architecture/uml/LogicalView/ReinforcementLearning/ReadyState.plantuml @@ -1,65 +1,65 @@ -@startuml - -title Ready State - -package "Application" as appLayer { - - class ReadyState <> { - + {static} getInstance() : ReadyState - + entry() : void - + process(sm : StateMachine&) : void - + exit() : void - + setLapTIme(lapTime : uint32_t) void - } - - class TrainingState <> - class RacingState <> - ReadyState .r.> RacingState: <> - ReadyState .l.> TrainingState: <> -} - -package "HAL" as hal { - - package "Interfaces" as halInterfaces { - interface IDisplay { - + {abstract} clear() : void - + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void - + {abstract} print(str : const String&) : size_t - + {abstract} print(str : const char[]) : size_t - + {abstract} print(value : uint8_t) : size_t - + {abstract} print(value : uint16_t) : size_t - + {abstract} print(value : uint32_t) : size_t - + {abstract} print(value : int8_t) : size_t - + {abstract} print(value : int16_t) : size_t - + {abstract} print(value : int32_t) : size_t - } - - interface IButton { - + {abstract} isPressed() : bool - + {abstract} waitForRelease() : void - } - - interface ILineSensors { - + {abstract} init() : void - + {abstract} calibrate() : void - + {abstract} readLine() : int16_t - + {abstract} getSensorValues() : const uint16_t* - + {abstract} getNumLineSensors() const : uint8_t - + {abstract} getSensorValueMax() const : uint16_t - } - } - - class Board << namespace >> { - + getDisplay() : IDisplay& - + getButtonA() : IButton& - + getButtonB() : IButton& - + getLineSensors() : ILineSensors& - } -} - -ReadyState ..> IDisplay: <> -ReadyState ..> IButton: <> -ReadyState ..> ILineSensors: <> -ReadyState ..> Board:: <> - +@startuml + +title Ready State + +package "Application" as appLayer { + + class ReadyState <> { + + {static} getInstance() : ReadyState + + entry() : void + + process(sm : StateMachine&) : void + + exit() : void + + setLapTIme(lapTime : uint32_t) void + } + + class TrainingState <> + class RacingState <> + ReadyState .r.> RacingState: <> + ReadyState .l.> TrainingState: <> +} + +package "HAL" as hal { + + package "Interfaces" as halInterfaces { + interface IDisplay { + + {abstract} clear() : void + + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void + + {abstract} print(str : const String&) : size_t + + {abstract} print(str : const char[]) : size_t + + {abstract} print(value : uint8_t) : size_t + + {abstract} print(value : uint16_t) : size_t + + {abstract} print(value : uint32_t) : size_t + + {abstract} print(value : int8_t) : size_t + + {abstract} print(value : int16_t) : size_t + + {abstract} print(value : int32_t) : size_t + } + + interface IButton { + + {abstract} isPressed() : bool + + {abstract} waitForRelease() : void + } + + interface ILineSensors { + + {abstract} init() : void + + {abstract} calibrate() : void + + {abstract} readLine() : int16_t + + {abstract} getSensorValues() : const uint16_t* + + {abstract} getNumLineSensors() const : uint8_t + + {abstract} getSensorValueMax() const : uint16_t + } + } + + class Board << namespace >> { + + getDisplay() : IDisplay& + + getButtonA() : IButton& + + getButtonB() : IButton& + + getLineSensors() : ILineSensors& + } +} + +ReadyState ..> IDisplay: <> +ReadyState ..> IButton: <> +ReadyState ..> ILineSensors: <> +ReadyState ..> Board:: <> + @enduml \ No newline at end of file diff --git a/doc/architecture/uml/LogicalView/ReinforcementLearning/TrainigState.plantuml b/doc/architecture/uml/LogicalView/ReinforcementLearning/TrainigState.plantuml index 9e74ff8e..06cef505 100644 --- a/doc/architecture/uml/LogicalView/ReinforcementLearning/TrainigState.plantuml +++ b/doc/architecture/uml/LogicalView/ReinforcementLearning/TrainigState.plantuml @@ -1,104 +1,104 @@ -@startuml - -title Training State - -package "Application" as appLayer { - - class TrainingState <> { - + {static} getInstance() : TrainingState - + entry() : void - + process(sm : StateMachine&) : void - + exit() : void - } - - class TrainingState <> - - TrainingState .r.> ReadyState: <> - -} - -package "Service" as serviceLayer { - - class SimpleTimer <> { - + start(duration : uint32_t) : void - + restart() : void - + stop() : void - + isTimeout() : bool - } - class DifferentialDrive <> -} - -package "HAL" as hal { - - package "Interfaces" as halInterfaces { - interface IDisplay { - + {abstract} clear() : void - + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void - + {abstract} print(str : const String&) : size_t - + {abstract} print(str : const char[]) : size_t - + {abstract} print(value : uint8_t) : size_t - + {abstract} print(value : uint16_t) : size_t - + {abstract} print(value : uint32_t) : size_t - + {abstract} print(value : int8_t) : size_t - + {abstract} print(value : int16_t) : size_t - + {abstract} print(value : int32_t) : size_t - } - - interface ILineSensors { - + {abstract} init() : void - + {abstract} calibrate() : void - + {abstract} readLine() : int16_t - + {abstract} getSensorValues() : const uint16_t* - + {abstract} isCalibrationSuccessful() : bool - + {abstract} getCalibErrorInfo() const : uint8_t - + {abstract} getNumLineSensors() const : uint8_t - + {abstract} getSensorValueMax() const : uint16_t - } - interface IButton { - + {abstract} isPressed() : bool - + {abstract} waitForRelease() : void - } - - interface ILed { - + {abstract} enable(enableIt : bool) : void - } - - } - - class Board << namespace >> { - + getDisplay() : IDisplay& - + getLineSensors() : ILineSensors& - + getLedYellow() : ILed& - + getButtonA() : IButton& - } - class WebotsSerialDrv { - + setRxChannel(channelId: int32_t) : void - + setTxChannel(channelId: int32_t ) : void - + print(str: const char[]) : void - + print(value: uint8_t ) : void - + print(value: uint16_t ) : void - + print(value: uint32_t ) : void - + print(value: int8_t ) : void - + print(value: int16_t ) : void - + print(value: int32_t ) : void - + println(str: const char[]) : void - + println(value: uint8_t ) : void - + println(value: uint16_t ) : void - + println(value: uint32_t ) : void - + println(value: int8_t ) : void - + println(value: int16_t ) : void - + println(value: int32_t ) : void - + write( buffer: const uint8_t*, length: size_t ) : size_t - + readBytes( buffer: uint8_t*, length: size_t ) : size_t - } -} - -TrainingState *-> SimpleTimer -TrainingState ..> DifferentialDrive: <> -TrainingState ...> IDisplay: <> -TrainingState ...> ILineSensors: <> -TrainingState ...> ILed: <> -TrainingState ...> Board: <> -TrainingState ...>WebotsSerialDrv: <> -TrainingState ...>IButton: <> +@startuml + +title Training State + +package "Application" as appLayer { + + class TrainingState <> { + + {static} getInstance() : TrainingState + + entry() : void + + process(sm : StateMachine&) : void + + exit() : void + } + + class TrainingState <> + + TrainingState .r.> ReadyState: <> + +} + +package "Service" as serviceLayer { + + class SimpleTimer <> { + + start(duration : uint32_t) : void + + restart() : void + + stop() : void + + isTimeout() : bool + } + class DifferentialDrive <> +} + +package "HAL" as hal { + + package "Interfaces" as halInterfaces { + interface IDisplay { + + {abstract} clear() : void + + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void + + {abstract} print(str : const String&) : size_t + + {abstract} print(str : const char[]) : size_t + + {abstract} print(value : uint8_t) : size_t + + {abstract} print(value : uint16_t) : size_t + + {abstract} print(value : uint32_t) : size_t + + {abstract} print(value : int8_t) : size_t + + {abstract} print(value : int16_t) : size_t + + {abstract} print(value : int32_t) : size_t + } + + interface ILineSensors { + + {abstract} init() : void + + {abstract} calibrate() : void + + {abstract} readLine() : int16_t + + {abstract} getSensorValues() : const uint16_t* + + {abstract} isCalibrationSuccessful() : bool + + {abstract} getCalibErrorInfo() const : uint8_t + + {abstract} getNumLineSensors() const : uint8_t + + {abstract} getSensorValueMax() const : uint16_t + } + interface IButton { + + {abstract} isPressed() : bool + + {abstract} waitForRelease() : void + } + + interface ILed { + + {abstract} enable(enableIt : bool) : void + } + + } + + class Board << namespace >> { + + getDisplay() : IDisplay& + + getLineSensors() : ILineSensors& + + getLedYellow() : ILed& + + getButtonA() : IButton& + } + class WebotsSerialDrv { + + setRxChannel(channelId: int32_t) : void + + setTxChannel(channelId: int32_t ) : void + + print(str: const char[]) : void + + print(value: uint8_t ) : void + + print(value: uint16_t ) : void + + print(value: uint32_t ) : void + + print(value: int8_t ) : void + + print(value: int16_t ) : void + + print(value: int32_t ) : void + + println(str: const char[]) : void + + println(value: uint8_t ) : void + + println(value: uint16_t ) : void + + println(value: uint32_t ) : void + + println(value: int8_t ) : void + + println(value: int16_t ) : void + + println(value: int32_t ) : void + + write( buffer: const uint8_t*, length: size_t ) : size_t + + readBytes( buffer: uint8_t*, length: size_t ) : size_t + } +} + +TrainingState *-> SimpleTimer +TrainingState ..> DifferentialDrive: <> +TrainingState ...> IDisplay: <> +TrainingState ...> ILineSensors: <> +TrainingState ...> ILed: <> +TrainingState ...> Board: <> +TrainingState ...>WebotsSerialDrv: <> +TrainingState ...>IButton: <> @enduml \ No newline at end of file diff --git a/doc/architecture/uml/ProcessView/ReinforcementLearning/SystemStates.plantuml b/doc/architecture/uml/ProcessView/ReinforcementLearning/SystemStates.plantuml index 716e9e3a..39b8f1a3 100644 --- a/doc/architecture/uml/ProcessView/ReinforcementLearning/SystemStates.plantuml +++ b/doc/architecture/uml/ProcessView/ReinforcementLearning/SystemStates.plantuml @@ -1,43 +1,43 @@ -@startuml - -title System States - -state StartupState: /entry Initialize HAL. -state StartupState: /entry Show operator info on LCD. -state StartupState: /do Wait for pushbutton A or B is triggered. - -state MotorSpeedCalibrationState: /entry Show operator info on LCD. -state MotorSpeedCalibrationState: /do Perform calibration. - -state LineSensorsCalibrationState: /entry Show operator info on LCD. -state LineSensorsCalibrationState: /do Perform calibration. - -state ErrorState: /entry Show error info on LCD. -state ErrorState: /do Wait for pushbutton A is triggered. - -state ReadyState: /entry Show operator info on LCD. -state ReadyState: /do Wait for button A or B to be triggered. - -state TrainingState: /entry Show operator info on LCD. -state TrainingState: /entry Start observation timer. -state TrainingState: /do Perform training. -state DrivingState: /exit Stop observation timer. - -state DrivingState: /entry Show operator info on LCD. -state DrivingState: /entry Start observation timer. -state DrivingState: /do Perform driving. -state DrivingState: /exit Stop observation timer. - -[*] --> StartupState: Power up -StartupState --> MotorSpeedCalibrationState: [Pushbutton A triggered] -StartupState --> LineSensorsCalibrationState: [Pushbutton B triggered] and\n[Max. motor speed calib. is available in settings] -MotorSpeedCalibrationState --> LineSensorsCalibrationState: [Calibration finished] -LineSensorsCalibrationState --> ReadyState: [Calibration finished] -LineSensorsCalibrationState --> ErrorState: [Calibration failed] -ReadyState ---> DrivingState: [Pushbutton A triggered] -ReadyState --> TrainingState: [Pushbutton B triggered] -TrainingState --> ReadyState: [Pushbutton A triggered] or\n [End line detected] or\n[Observation timer timeout] -DrivingState ---> ReadyState: [End line detected] or\n[Pushbutton A triggered] or\n[Observation timer timeout] -ErrorState --> StartupState: [Pushbutton A triggered] - +@startuml + +title System States + +state StartupState: /entry Initialize HAL. +state StartupState: /entry Show operator info on LCD. +state StartupState: /do Wait for pushbutton A or B is triggered. + +state MotorSpeedCalibrationState: /entry Show operator info on LCD. +state MotorSpeedCalibrationState: /do Perform calibration. + +state LineSensorsCalibrationState: /entry Show operator info on LCD. +state LineSensorsCalibrationState: /do Perform calibration. + +state ErrorState: /entry Show error info on LCD. +state ErrorState: /do Wait for pushbutton A is triggered. + +state ReadyState: /entry Show operator info on LCD. +state ReadyState: /do Wait for button A or B to be triggered. + +state TrainingState: /entry Show operator info on LCD. +state TrainingState: /entry Start observation timer. +state TrainingState: /do Perform training. +state DrivingState: /exit Stop observation timer. + +state DrivingState: /entry Show operator info on LCD. +state DrivingState: /entry Start observation timer. +state DrivingState: /do Perform driving. +state DrivingState: /exit Stop observation timer. + +[*] --> StartupState: Power up +StartupState --> MotorSpeedCalibrationState: [Pushbutton A triggered] +StartupState --> LineSensorsCalibrationState: [Pushbutton B triggered] and\n[Max. motor speed calib. is available in settings] +MotorSpeedCalibrationState --> LineSensorsCalibrationState: [Calibration finished] +LineSensorsCalibrationState --> ReadyState: [Calibration finished] +LineSensorsCalibrationState --> ErrorState: [Calibration failed] +ReadyState ---> DrivingState: [Pushbutton A triggered] +ReadyState --> TrainingState: [Pushbutton B triggered] +TrainingState --> ReadyState: [Pushbutton A triggered] or\n [End line detected] or\n[Observation timer timeout] +DrivingState ---> ReadyState: [End line detected] or\n[Pushbutton A triggered] or\n[Observation timer timeout] +ErrorState --> StartupState: [Pushbutton A triggered] + @enduml \ No newline at end of file From a1de1fe71313b1cb1176298ec9d159d936f858b4 Mon Sep 17 00:00:00 2001 From: Gabryel Reyes Date: Mon, 29 Jul 2024 15:19:19 +0200 Subject: [PATCH 35/58] Added missing final newline --- webots/controllers/Supervisor/supervisor.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/webots/controllers/Supervisor/supervisor.py b/webots/controllers/Supervisor/supervisor.py index 10b7d06c..7a15e485 100644 --- a/webots/controllers/Supervisor/supervisor.py +++ b/webots/controllers/Supervisor/supervisor.py @@ -203,4 +203,4 @@ def main_loop(): return status -sys.exit(main_loop()) \ No newline at end of file +sys.exit(main_loop()) From f69babca4803596d8e2cbdcadb634e88c47ef5f4 Mon Sep 17 00:00:00 2001 From: Akram Date: Mon, 29 Jul 2024 15:40:57 +0200 Subject: [PATCH 36/58] Keep list alphabetical --- platformio.ini | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/platformio.ini b/platformio.ini index dc0c0a4c..5907cf04 100644 --- a/platformio.ini +++ b/platformio.ini @@ -196,8 +196,8 @@ lib_ignore = APPCalib APPConvoyFollower APPLineFollower - APPRemoteControl APPReinforcementLearning + APPRemoteControl APPSensorFusion APPTest @@ -246,8 +246,8 @@ lib_ignore = APPCalib APPConvoyLeader APPLineFollower - APPRemoteControl APPReinforcementLearning + APPRemoteControl APPSensorFusion APPTest @@ -294,8 +294,8 @@ lib_deps = lib_ignore = APPCalib APPConvoyFollower - APPReinforcementLearning APPConvoyLeader + APPReinforcementLearning APPRemoteControl APPSensorFusion APPTest @@ -313,9 +313,9 @@ lib_deps = lib_ignore = APPCalib APPConvoyFollower - APPReinforcementLearning APPConvoyLeader APPLineFollower + APPReinforcementLearning APPRemoteControl APPSensorFusion APPTest @@ -398,9 +398,9 @@ lib_deps = lib_ignore = APPCalib APPConvoyFollower - APPReinforcementLearning APPConvoyLeader APPLineFollower + APPReinforcementLearning APPSensorFusion APPTest From 8b05ea938fa9602074cc4ef0a215f5d4c32ec013 Mon Sep 17 00:00:00 2001 From: Gabryel Reyes Date: Mon, 29 Jul 2024 16:15:07 +0200 Subject: [PATCH 37/58] Create Doxygen documentation via CI --- .github/workflows/main.yml | 2 +- doc/doxygen/ReinforcementLearningSimDoxyfile | 2862 ++++++++++++++++++ 2 files changed, 2863 insertions(+), 1 deletion(-) create mode 100644 doc/doxygen/ReinforcementLearningSimDoxyfile diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 86ef6fa4..322fa5a0 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -148,7 +148,7 @@ jobs: needs: intro strategy: matrix: - environment: ["CalibTarget", "ConvoyFollowerTarget", "ConvoyLeaderTarget", "LineFollowerTarget", "LineFollowerSimpleTarget", "RemoteControlTarget", "SensorFusionTarget", "CalibSim", "ConvoyFollowerSim", "ConvoyLeaderSim", "LineFollowerSim", "LineFollowerSimpleSim", "RemoteControlSim", "SensorFusionSim"] + environment: ["CalibTarget", "ConvoyFollowerTarget", "ConvoyLeaderTarget", "LineFollowerTarget", "LineFollowerSimpleTarget", "RemoteControlTarget", "SensorFusionTarget", "CalibSim", "ConvoyFollowerSim", "ConvoyLeaderSim", "LineFollowerSim", "LineFollowerSimpleSim", "ReinforcementLearningSim", "RemoteControlSim", "SensorFusionSim"] steps: - name: Checkout repository diff --git a/doc/doxygen/ReinforcementLearningSimDoxyfile b/doc/doxygen/ReinforcementLearningSimDoxyfile new file mode 100644 index 00000000..08aac94a --- /dev/null +++ b/doc/doxygen/ReinforcementLearningSimDoxyfile @@ -0,0 +1,2862 @@ +# Doxyfile 1.9.8 + +# This file describes the settings to be used by the documentation system +# doxygen (www.doxygen.org) for a project. +# +# All text after a double hash (##) is considered a comment and is placed in +# front of the TAG it is preceding. +# +# All text after a single hash (#) is considered a comment and will be ignored. +# The format is: +# TAG = value [value, ...] +# For lists, items can also be appended using: +# TAG += value [value, ...] +# Values that contain spaces should be placed between quotes (\" \"). +# +# Note: +# +# Use doxygen to compare the used configuration file with the template +# configuration file: +# doxygen -x [configFile] +# Use doxygen to compare the used configuration file with the template +# configuration file without replacing the environment variables or CMake type +# replacement variables: +# doxygen -x_noenv [configFile] + +#--------------------------------------------------------------------------- +# Project related configuration options +#--------------------------------------------------------------------------- + +# This tag specifies the encoding used for all characters in the configuration +# file that follow. The default is UTF-8 which is also the encoding used for all +# text before the first occurrence of this tag. Doxygen uses libiconv (or the +# iconv built into libc) for the transcoding. See +# https://www.gnu.org/software/libiconv/ for the list of possible encodings. +# The default value is: UTF-8. + +DOXYFILE_ENCODING = UTF-8 + +# The PROJECT_NAME tag is a single word (or a sequence of words surrounded by +# double-quotes, unless you are using Doxywizard) that should identify the +# project for which the documentation is generated. This name is used in the +# title of most generated pages and in a few other places. +# The default value is: My Project. + +PROJECT_NAME = "Radon Ulzer - ReinforcementLearning on simulation" + +# The PROJECT_NUMBER tag can be used to enter a project or revision number. This +# could be handy for archiving the generated documentation or if some version +# control system is used. + +PROJECT_NUMBER = trunk + +# Using the PROJECT_BRIEF tag one can provide an optional one line description +# for a project that appears at the top of each page and should give viewer a +# quick idea about the purpose of the project. Keep the description short. + +PROJECT_BRIEF = + +# With the PROJECT_LOGO tag one can specify a logo or an icon that is included +# in the documentation. The maximum height of the logo should not exceed 55 +# pixels and the maximum width should not exceed 200 pixels. Doxygen will copy +# the logo to the output directory. + +PROJECT_LOGO = + +# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path +# into which the generated documentation will be written. If a relative path is +# entered, it will be relative to the location where doxygen was started. If +# left blank the current directory will be used. + +OUTPUT_DIRECTORY = ./ReinforcementLearningSim + +# If the CREATE_SUBDIRS tag is set to YES then doxygen will create up to 4096 +# sub-directories (in 2 levels) under the output directory of each output format +# and will distribute the generated files over these directories. Enabling this +# option can be useful when feeding doxygen a huge amount of source files, where +# putting all generated files in the same directory would otherwise causes +# performance problems for the file system. Adapt CREATE_SUBDIRS_LEVEL to +# control the number of sub-directories. +# The default value is: NO. + +CREATE_SUBDIRS = NO + +# Controls the number of sub-directories that will be created when +# CREATE_SUBDIRS tag is set to YES. Level 0 represents 16 directories, and every +# level increment doubles the number of directories, resulting in 4096 +# directories at level 8 which is the default and also the maximum value. The +# sub-directories are organized in 2 levels, the first level always has a fixed +# number of 16 directories. +# Minimum value: 0, maximum value: 8, default value: 8. +# This tag requires that the tag CREATE_SUBDIRS is set to YES. + +CREATE_SUBDIRS_LEVEL = 8 + +# If the ALLOW_UNICODE_NAMES tag is set to YES, doxygen will allow non-ASCII +# characters to appear in the names of generated files. If set to NO, non-ASCII +# characters will be escaped, for example _xE3_x81_x84 will be used for Unicode +# U+3044. +# The default value is: NO. + +ALLOW_UNICODE_NAMES = NO + +# The OUTPUT_LANGUAGE tag is used to specify the language in which all +# documentation generated by doxygen is written. Doxygen will use this +# information to generate all constant output in the proper language. +# Possible values are: Afrikaans, Arabic, Armenian, Brazilian, Bulgarian, +# Catalan, Chinese, Chinese-Traditional, Croatian, Czech, Danish, Dutch, English +# (United States), Esperanto, Farsi (Persian), Finnish, French, German, Greek, +# Hindi, Hungarian, Indonesian, Italian, Japanese, Japanese-en (Japanese with +# English messages), Korean, Korean-en (Korean with English messages), Latvian, +# Lithuanian, Macedonian, Norwegian, Persian (Farsi), Polish, Portuguese, +# Romanian, Russian, Serbian, Serbian-Cyrillic, Slovak, Slovene, Spanish, +# Swedish, Turkish, Ukrainian and Vietnamese. +# The default value is: English. + +OUTPUT_LANGUAGE = English + +# If the BRIEF_MEMBER_DESC tag is set to YES, doxygen will include brief member +# descriptions after the members that are listed in the file and class +# documentation (similar to Javadoc). Set to NO to disable this. +# The default value is: YES. + +BRIEF_MEMBER_DESC = YES + +# If the REPEAT_BRIEF tag is set to YES, doxygen will prepend the brief +# description of a member or function before the detailed description +# +# Note: If both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the +# brief descriptions will be completely suppressed. +# The default value is: YES. + +REPEAT_BRIEF = YES + +# This tag implements a quasi-intelligent brief description abbreviator that is +# used to form the text in various listings. Each string in this list, if found +# as the leading text of the brief description, will be stripped from the text +# and the result, after processing the whole list, is used as the annotated +# text. Otherwise, the brief description is used as-is. If left blank, the +# following values are used ($name is automatically replaced with the name of +# the entity):The $name class, The $name widget, The $name file, is, provides, +# specifies, contains, represents, a, an and the. + +ABBREVIATE_BRIEF = "The $name class" \ + "The $name widget" \ + "The $name file" \ + is \ + provides \ + specifies \ + contains \ + represents \ + a \ + an \ + the + +# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then +# doxygen will generate a detailed section even if there is only a brief +# description. +# The default value is: NO. + +ALWAYS_DETAILED_SEC = NO + +# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all +# inherited members of a class in the documentation of that class as if those +# members were ordinary class members. Constructors, destructors and assignment +# operators of the base classes will not be shown. +# The default value is: NO. + +INLINE_INHERITED_MEMB = NO + +# If the FULL_PATH_NAMES tag is set to YES, doxygen will prepend the full path +# before files name in the file list and in the header files. If set to NO the +# shortest path that makes the file name unique will be used +# The default value is: YES. + +FULL_PATH_NAMES = YES + +# The STRIP_FROM_PATH tag can be used to strip a user-defined part of the path. +# Stripping is only done if one of the specified strings matches the left-hand +# part of the path. The tag can be used to show relative paths in the file list. +# If left blank the directory from which doxygen is run is used as the path to +# strip. +# +# Note that you can specify absolute paths here, but also relative paths, which +# will be relative from the directory where doxygen is started. +# This tag requires that the tag FULL_PATH_NAMES is set to YES. + +STRIP_FROM_PATH = .. + +# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of the +# path mentioned in the documentation of a class, which tells the reader which +# header file to include in order to use a class. If left blank only the name of +# the header file containing the class definition is used. Otherwise one should +# specify the list of include paths that are normally passed to the compiler +# using the -I flag. + +STRIP_FROM_INC_PATH = + +# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter (but +# less readable) file names. This can be useful is your file systems doesn't +# support long names like on DOS, Mac, or CD-ROM. +# The default value is: NO. + +SHORT_NAMES = NO + +# If the JAVADOC_AUTOBRIEF tag is set to YES then doxygen will interpret the +# first line (until the first dot) of a Javadoc-style comment as the brief +# description. If set to NO, the Javadoc-style will behave just like regular Qt- +# style comments (thus requiring an explicit @brief command for a brief +# description.) +# The default value is: NO. + +JAVADOC_AUTOBRIEF = NO + +# If the JAVADOC_BANNER tag is set to YES then doxygen will interpret a line +# such as +# /*************** +# as being the beginning of a Javadoc-style comment "banner". If set to NO, the +# Javadoc-style will behave just like regular comments and it will not be +# interpreted by doxygen. +# The default value is: NO. + +JAVADOC_BANNER = NO + +# If the QT_AUTOBRIEF tag is set to YES then doxygen will interpret the first +# line (until the first dot) of a Qt-style comment as the brief description. If +# set to NO, the Qt-style will behave just like regular Qt-style comments (thus +# requiring an explicit \brief command for a brief description.) +# The default value is: NO. + +QT_AUTOBRIEF = NO + +# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make doxygen treat a +# multi-line C++ special comment block (i.e. a block of //! or /// comments) as +# a brief description. This used to be the default behavior. The new default is +# to treat a multi-line C++ comment block as a detailed description. Set this +# tag to YES if you prefer the old behavior instead. +# +# Note that setting this tag to YES also means that rational rose comments are +# not recognized any more. +# The default value is: NO. + +MULTILINE_CPP_IS_BRIEF = NO + +# By default Python docstrings are displayed as preformatted text and doxygen's +# special commands cannot be used. By setting PYTHON_DOCSTRING to NO the +# doxygen's special commands can be used and the contents of the docstring +# documentation blocks is shown as doxygen documentation. +# The default value is: YES. + +PYTHON_DOCSTRING = YES + +# If the INHERIT_DOCS tag is set to YES then an undocumented member inherits the +# documentation from any documented member that it re-implements. +# The default value is: YES. + +INHERIT_DOCS = YES + +# If the SEPARATE_MEMBER_PAGES tag is set to YES then doxygen will produce a new +# page for each member. If set to NO, the documentation of a member will be part +# of the file/class/namespace that contains it. +# The default value is: NO. + +SEPARATE_MEMBER_PAGES = NO + +# The TAB_SIZE tag can be used to set the number of spaces in a tab. Doxygen +# uses this value to replace tabs by spaces in code fragments. +# Minimum value: 1, maximum value: 16, default value: 4. + +TAB_SIZE = 4 + +# This tag can be used to specify a number of aliases that act as commands in +# the documentation. An alias has the form: +# name=value +# For example adding +# "sideeffect=@par Side Effects:^^" +# will allow you to put the command \sideeffect (or @sideeffect) in the +# documentation, which will result in a user-defined paragraph with heading +# "Side Effects:". Note that you cannot put \n's in the value part of an alias +# to insert newlines (in the resulting output). You can put ^^ in the value part +# of an alias to insert a newline as if a physical newline was in the original +# file. When you need a literal { or } or , in the value part of an alias you +# have to escape them by means of a backslash (\), this can lead to conflicts +# with the commands \{ and \} for these it is advised to use the version @{ and +# @} or use a double escape (\\{ and \\}) + +ALIASES = + +# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C sources +# only. Doxygen will then generate output that is more tailored for C. For +# instance, some of the names that are used will be different. The list of all +# members will be omitted, etc. +# The default value is: NO. + +OPTIMIZE_OUTPUT_FOR_C = NO + +# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java or +# Python sources only. Doxygen will then generate output that is more tailored +# for that language. For instance, namespaces will be presented as packages, +# qualified scopes will look different, etc. +# The default value is: NO. + +OPTIMIZE_OUTPUT_JAVA = NO + +# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran +# sources. Doxygen will then generate output that is tailored for Fortran. +# The default value is: NO. + +OPTIMIZE_FOR_FORTRAN = NO + +# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL +# sources. Doxygen will then generate output that is tailored for VHDL. +# The default value is: NO. + +OPTIMIZE_OUTPUT_VHDL = NO + +# Set the OPTIMIZE_OUTPUT_SLICE tag to YES if your project consists of Slice +# sources only. Doxygen will then generate output that is more tailored for that +# language. For instance, namespaces will be presented as modules, types will be +# separated into more groups, etc. +# The default value is: NO. + +OPTIMIZE_OUTPUT_SLICE = NO + +# Doxygen selects the parser to use depending on the extension of the files it +# parses. With this tag you can assign which parser to use for a given +# extension. Doxygen has a built-in mapping, but you can override or extend it +# using this tag. The format is ext=language, where ext is a file extension, and +# language is one of the parsers supported by doxygen: IDL, Java, JavaScript, +# Csharp (C#), C, C++, Lex, D, PHP, md (Markdown), Objective-C, Python, Slice, +# VHDL, Fortran (fixed format Fortran: FortranFixed, free formatted Fortran: +# FortranFree, unknown formatted Fortran: Fortran. In the later case the parser +# tries to guess whether the code is fixed or free formatted code, this is the +# default for Fortran type files). For instance to make doxygen treat .inc files +# as Fortran files (default is PHP), and .f files as C (default is Fortran), +# use: inc=Fortran f=C. +# +# Note: For files without extension you can use no_extension as a placeholder. +# +# Note that for custom extensions you also need to set FILE_PATTERNS otherwise +# the files are not read by doxygen. When specifying no_extension you should add +# * to the FILE_PATTERNS. +# +# Note see also the list of default file extension mappings. + +EXTENSION_MAPPING = + +# If the MARKDOWN_SUPPORT tag is enabled then doxygen pre-processes all comments +# according to the Markdown format, which allows for more readable +# documentation. See https://daringfireball.net/projects/markdown/ for details. +# The output of markdown processing is further processed by doxygen, so you can +# mix doxygen, HTML, and XML commands with Markdown formatting. Disable only in +# case of backward compatibilities issues. +# The default value is: YES. + +MARKDOWN_SUPPORT = YES + +# When the TOC_INCLUDE_HEADINGS tag is set to a non-zero value, all headings up +# to that level are automatically included in the table of contents, even if +# they do not have an id attribute. +# Note: This feature currently applies only to Markdown headings. +# Minimum value: 0, maximum value: 99, default value: 5. +# This tag requires that the tag MARKDOWN_SUPPORT is set to YES. + +TOC_INCLUDE_HEADINGS = 0 + +# The MARKDOWN_ID_STYLE tag can be used to specify the algorithm used to +# generate identifiers for the Markdown headings. Note: Every identifier is +# unique. +# Possible values are: DOXYGEN use a fixed 'autotoc_md' string followed by a +# sequence number starting at 0 and GITHUB use the lower case version of title +# with any whitespace replaced by '-' and punctuation characters removed. +# The default value is: DOXYGEN. +# This tag requires that the tag MARKDOWN_SUPPORT is set to YES. + +MARKDOWN_ID_STYLE = DOXYGEN + +# When enabled doxygen tries to link words that correspond to documented +# classes, or namespaces to their corresponding documentation. Such a link can +# be prevented in individual cases by putting a % sign in front of the word or +# globally by setting AUTOLINK_SUPPORT to NO. +# The default value is: YES. + +AUTOLINK_SUPPORT = YES + +# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want +# to include (a tag file for) the STL sources as input, then you should set this +# tag to YES in order to let doxygen match functions declarations and +# definitions whose arguments contain STL classes (e.g. func(std::string); +# versus func(std::string) {}). This also make the inheritance and collaboration +# diagrams that involve STL classes more complete and accurate. +# The default value is: NO. + +BUILTIN_STL_SUPPORT = NO + +# If you use Microsoft's C++/CLI language, you should set this option to YES to +# enable parsing support. +# The default value is: NO. + +CPP_CLI_SUPPORT = NO + +# Set the SIP_SUPPORT tag to YES if your project consists of sip (see: +# https://www.riverbankcomputing.com/software/sip/intro) sources only. Doxygen +# will parse them like normal C++ but will assume all classes use public instead +# of private inheritance when no explicit protection keyword is present. +# The default value is: NO. + +SIP_SUPPORT = NO + +# For Microsoft's IDL there are propget and propput attributes to indicate +# getter and setter methods for a property. Setting this option to YES will make +# doxygen to replace the get and set methods by a property in the documentation. +# This will only work if the methods are indeed getting or setting a simple +# type. If this is not the case, or you want to show the methods anyway, you +# should set this option to NO. +# The default value is: YES. + +IDL_PROPERTY_SUPPORT = YES + +# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC +# tag is set to YES then doxygen will reuse the documentation of the first +# member in the group (if any) for the other members of the group. By default +# all members of a group must be documented explicitly. +# The default value is: NO. + +DISTRIBUTE_GROUP_DOC = NO + +# If one adds a struct or class to a group and this option is enabled, then also +# any nested class or struct is added to the same group. By default this option +# is disabled and one has to add nested compounds explicitly via \ingroup. +# The default value is: NO. + +GROUP_NESTED_COMPOUNDS = NO + +# Set the SUBGROUPING tag to YES to allow class member groups of the same type +# (for instance a group of public functions) to be put as a subgroup of that +# type (e.g. under the Public Functions section). Set it to NO to prevent +# subgrouping. Alternatively, this can be done per class using the +# \nosubgrouping command. +# The default value is: YES. + +SUBGROUPING = YES + +# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and unions +# are shown inside the group in which they are included (e.g. using \ingroup) +# instead of on a separate page (for HTML and Man pages) or section (for LaTeX +# and RTF). +# +# Note that this feature does not work in combination with +# SEPARATE_MEMBER_PAGES. +# The default value is: NO. + +INLINE_GROUPED_CLASSES = NO + +# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and unions +# with only public data fields or simple typedef fields will be shown inline in +# the documentation of the scope in which they are defined (i.e. file, +# namespace, or group documentation), provided this scope is documented. If set +# to NO, structs, classes, and unions are shown on a separate page (for HTML and +# Man pages) or section (for LaTeX and RTF). +# The default value is: NO. + +INLINE_SIMPLE_STRUCTS = NO + +# When TYPEDEF_HIDES_STRUCT tag is enabled, a typedef of a struct, union, or +# enum is documented as struct, union, or enum with the name of the typedef. So +# typedef struct TypeS {} TypeT, will appear in the documentation as a struct +# with name TypeT. When disabled the typedef will appear as a member of a file, +# namespace, or class. And the struct will be named TypeS. This can typically be +# useful for C code in case the coding convention dictates that all compound +# types are typedef'ed and only the typedef is referenced, never the tag name. +# The default value is: NO. + +TYPEDEF_HIDES_STRUCT = NO + +# The size of the symbol lookup cache can be set using LOOKUP_CACHE_SIZE. This +# cache is used to resolve symbols given their name and scope. Since this can be +# an expensive process and often the same symbol appears multiple times in the +# code, doxygen keeps a cache of pre-resolved symbols. If the cache is too small +# doxygen will become slower. If the cache is too large, memory is wasted. The +# cache size is given by this formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range +# is 0..9, the default is 0, corresponding to a cache size of 2^16=65536 +# symbols. At the end of a run doxygen will report the cache usage and suggest +# the optimal cache size from a speed point of view. +# Minimum value: 0, maximum value: 9, default value: 0. + +LOOKUP_CACHE_SIZE = 0 + +# The NUM_PROC_THREADS specifies the number of threads doxygen is allowed to use +# during processing. When set to 0 doxygen will based this on the number of +# cores available in the system. You can set it explicitly to a value larger +# than 0 to get more control over the balance between CPU load and processing +# speed. At this moment only the input processing can be done using multiple +# threads. Since this is still an experimental feature the default is set to 1, +# which effectively disables parallel processing. Please report any issues you +# encounter. Generating dot graphs in parallel is controlled by the +# DOT_NUM_THREADS setting. +# Minimum value: 0, maximum value: 32, default value: 1. + +NUM_PROC_THREADS = 1 + +# If the TIMESTAMP tag is set different from NO then each generated page will +# contain the date or date and time when the page was generated. Setting this to +# NO can help when comparing the output of multiple runs. +# Possible values are: YES, NO, DATETIME and DATE. +# The default value is: NO. + +TIMESTAMP = NO + +#--------------------------------------------------------------------------- +# Build related configuration options +#--------------------------------------------------------------------------- + +# If the EXTRACT_ALL tag is set to YES, doxygen will assume all entities in +# documentation are documented, even if no documentation was available. Private +# class members and static file members will be hidden unless the +# EXTRACT_PRIVATE respectively EXTRACT_STATIC tags are set to YES. +# Note: This will also disable the warnings about undocumented members that are +# normally produced when WARNINGS is set to YES. +# The default value is: NO. + +EXTRACT_ALL = NO + +# If the EXTRACT_PRIVATE tag is set to YES, all private members of a class will +# be included in the documentation. +# The default value is: NO. + +EXTRACT_PRIVATE = YES + +# If the EXTRACT_PRIV_VIRTUAL tag is set to YES, documented private virtual +# methods of a class will be included in the documentation. +# The default value is: NO. + +EXTRACT_PRIV_VIRTUAL = YES + +# If the EXTRACT_PACKAGE tag is set to YES, all members with package or internal +# scope will be included in the documentation. +# The default value is: NO. + +EXTRACT_PACKAGE = NO + +# If the EXTRACT_STATIC tag is set to YES, all static members of a file will be +# included in the documentation. +# The default value is: NO. + +EXTRACT_STATIC = YES + +# If the EXTRACT_LOCAL_CLASSES tag is set to YES, classes (and structs) defined +# locally in source files will be included in the documentation. If set to NO, +# only classes defined in header files are included. Does not have any effect +# for Java sources. +# The default value is: YES. + +EXTRACT_LOCAL_CLASSES = YES + +# This flag is only useful for Objective-C code. If set to YES, local methods, +# which are defined in the implementation section but not in the interface are +# included in the documentation. If set to NO, only methods in the interface are +# included. +# The default value is: NO. + +EXTRACT_LOCAL_METHODS = NO + +# If this flag is set to YES, the members of anonymous namespaces will be +# extracted and appear in the documentation as a namespace called +# 'anonymous_namespace{file}', where file will be replaced with the base name of +# the file that contains the anonymous namespace. By default anonymous namespace +# are hidden. +# The default value is: NO. + +EXTRACT_ANON_NSPACES = NO + +# If this flag is set to YES, the name of an unnamed parameter in a declaration +# will be determined by the corresponding definition. By default unnamed +# parameters remain unnamed in the output. +# The default value is: YES. + +RESOLVE_UNNAMED_PARAMS = YES + +# If the HIDE_UNDOC_MEMBERS tag is set to YES, doxygen will hide all +# undocumented members inside documented classes or files. If set to NO these +# members will be included in the various overviews, but no documentation +# section is generated. This option has no effect if EXTRACT_ALL is enabled. +# The default value is: NO. + +HIDE_UNDOC_MEMBERS = NO + +# If the HIDE_UNDOC_CLASSES tag is set to YES, doxygen will hide all +# undocumented classes that are normally visible in the class hierarchy. If set +# to NO, these classes will be included in the various overviews. This option +# will also hide undocumented C++ concepts if enabled. This option has no effect +# if EXTRACT_ALL is enabled. +# The default value is: NO. + +HIDE_UNDOC_CLASSES = NO + +# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, doxygen will hide all friend +# declarations. If set to NO, these declarations will be included in the +# documentation. +# The default value is: NO. + +HIDE_FRIEND_COMPOUNDS = NO + +# If the HIDE_IN_BODY_DOCS tag is set to YES, doxygen will hide any +# documentation blocks found inside the body of a function. If set to NO, these +# blocks will be appended to the function's detailed documentation block. +# The default value is: NO. + +HIDE_IN_BODY_DOCS = NO + +# The INTERNAL_DOCS tag determines if documentation that is typed after a +# \internal command is included. If the tag is set to NO then the documentation +# will be excluded. Set it to YES to include the internal documentation. +# The default value is: NO. + +INTERNAL_DOCS = NO + +# With the correct setting of option CASE_SENSE_NAMES doxygen will better be +# able to match the capabilities of the underlying filesystem. In case the +# filesystem is case sensitive (i.e. it supports files in the same directory +# whose names only differ in casing), the option must be set to YES to properly +# deal with such files in case they appear in the input. For filesystems that +# are not case sensitive the option should be set to NO to properly deal with +# output files written for symbols that only differ in casing, such as for two +# classes, one named CLASS and the other named Class, and to also support +# references to files without having to specify the exact matching casing. On +# Windows (including Cygwin) and MacOS, users should typically set this option +# to NO, whereas on Linux or other Unix flavors it should typically be set to +# YES. +# Possible values are: SYSTEM, NO and YES. +# The default value is: SYSTEM. + +CASE_SENSE_NAMES = NO + +# If the HIDE_SCOPE_NAMES tag is set to NO then doxygen will show members with +# their full class and namespace scopes in the documentation. If set to YES, the +# scope will be hidden. +# The default value is: NO. + +HIDE_SCOPE_NAMES = NO + +# If the HIDE_COMPOUND_REFERENCE tag is set to NO (default) then doxygen will +# append additional text to a page's title, such as Class Reference. If set to +# YES the compound reference will be hidden. +# The default value is: NO. + +HIDE_COMPOUND_REFERENCE= NO + +# If the SHOW_HEADERFILE tag is set to YES then the documentation for a class +# will show which file needs to be included to use the class. +# The default value is: YES. + +SHOW_HEADERFILE = YES + +# If the SHOW_INCLUDE_FILES tag is set to YES then doxygen will put a list of +# the files that are included by a file in the documentation of that file. +# The default value is: YES. + +SHOW_INCLUDE_FILES = YES + +# If the SHOW_GROUPED_MEMB_INC tag is set to YES then Doxygen will add for each +# grouped member an include statement to the documentation, telling the reader +# which file to include in order to use the member. +# The default value is: NO. + +SHOW_GROUPED_MEMB_INC = NO + +# If the FORCE_LOCAL_INCLUDES tag is set to YES then doxygen will list include +# files with double quotes in the documentation rather than with sharp brackets. +# The default value is: NO. + +FORCE_LOCAL_INCLUDES = NO + +# If the INLINE_INFO tag is set to YES then a tag [inline] is inserted in the +# documentation for inline members. +# The default value is: YES. + +INLINE_INFO = YES + +# If the SORT_MEMBER_DOCS tag is set to YES then doxygen will sort the +# (detailed) documentation of file and class members alphabetically by member +# name. If set to NO, the members will appear in declaration order. +# The default value is: YES. + +SORT_MEMBER_DOCS = YES + +# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the brief +# descriptions of file, namespace and class members alphabetically by member +# name. If set to NO, the members will appear in declaration order. Note that +# this will also influence the order of the classes in the class list. +# The default value is: NO. + +SORT_BRIEF_DOCS = NO + +# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen will sort the +# (brief and detailed) documentation of class members so that constructors and +# destructors are listed first. If set to NO the constructors will appear in the +# respective orders defined by SORT_BRIEF_DOCS and SORT_MEMBER_DOCS. +# Note: If SORT_BRIEF_DOCS is set to NO this option is ignored for sorting brief +# member documentation. +# Note: If SORT_MEMBER_DOCS is set to NO this option is ignored for sorting +# detailed member documentation. +# The default value is: NO. + +SORT_MEMBERS_CTORS_1ST = NO + +# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the hierarchy +# of group names into alphabetical order. If set to NO the group names will +# appear in their defined order. +# The default value is: NO. + +SORT_GROUP_NAMES = NO + +# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be sorted by +# fully-qualified names, including namespaces. If set to NO, the class list will +# be sorted only by class name, not including the namespace part. +# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES. +# Note: This option applies only to the class list, not to the alphabetical +# list. +# The default value is: NO. + +SORT_BY_SCOPE_NAME = NO + +# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to do proper +# type resolution of all parameters of a function it will reject a match between +# the prototype and the implementation of a member function even if there is +# only one candidate or it is obvious which candidate to choose by doing a +# simple string match. By disabling STRICT_PROTO_MATCHING doxygen will still +# accept a match between prototype and implementation in such cases. +# The default value is: NO. + +STRICT_PROTO_MATCHING = NO + +# The GENERATE_TODOLIST tag can be used to enable (YES) or disable (NO) the todo +# list. This list is created by putting \todo commands in the documentation. +# The default value is: YES. + +GENERATE_TODOLIST = YES + +# The GENERATE_TESTLIST tag can be used to enable (YES) or disable (NO) the test +# list. This list is created by putting \test commands in the documentation. +# The default value is: YES. + +GENERATE_TESTLIST = YES + +# The GENERATE_BUGLIST tag can be used to enable (YES) or disable (NO) the bug +# list. This list is created by putting \bug commands in the documentation. +# The default value is: YES. + +GENERATE_BUGLIST = YES + +# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or disable (NO) +# the deprecated list. This list is created by putting \deprecated commands in +# the documentation. +# The default value is: YES. + +GENERATE_DEPRECATEDLIST= YES + +# The ENABLED_SECTIONS tag can be used to enable conditional documentation +# sections, marked by \if ... \endif and \cond +# ... \endcond blocks. + +ENABLED_SECTIONS = + +# The MAX_INITIALIZER_LINES tag determines the maximum number of lines that the +# initial value of a variable or macro / define can have for it to appear in the +# documentation. If the initializer consists of more lines than specified here +# it will be hidden. Use a value of 0 to hide initializers completely. The +# appearance of the value of individual variables and macros / defines can be +# controlled using \showinitializer or \hideinitializer command in the +# documentation regardless of this setting. +# Minimum value: 0, maximum value: 10000, default value: 30. + +MAX_INITIALIZER_LINES = 30 + +# Set the SHOW_USED_FILES tag to NO to disable the list of files generated at +# the bottom of the documentation of classes and structs. If set to YES, the +# list will mention the files that were used to generate the documentation. +# The default value is: YES. + +SHOW_USED_FILES = YES + +# Set the SHOW_FILES tag to NO to disable the generation of the Files page. This +# will remove the Files entry from the Quick Index and from the Folder Tree View +# (if specified). +# The default value is: YES. + +SHOW_FILES = YES + +# Set the SHOW_NAMESPACES tag to NO to disable the generation of the Namespaces +# page. This will remove the Namespaces entry from the Quick Index and from the +# Folder Tree View (if specified). +# The default value is: YES. + +SHOW_NAMESPACES = YES + +# The FILE_VERSION_FILTER tag can be used to specify a program or script that +# doxygen should invoke to get the current version for each file (typically from +# the version control system). Doxygen will invoke the program by executing (via +# popen()) the command command input-file, where command is the value of the +# FILE_VERSION_FILTER tag, and input-file is the name of an input file provided +# by doxygen. Whatever the program writes to standard output is used as the file +# version. For an example see the documentation. + +FILE_VERSION_FILTER = + +# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed +# by doxygen. The layout file controls the global structure of the generated +# output files in an output format independent way. To create the layout file +# that represents doxygen's defaults, run doxygen with the -l option. You can +# optionally specify a file name after the option, if omitted DoxygenLayout.xml +# will be used as the name of the layout file. See also section "Changing the +# layout of pages" for information. +# +# Note that if you run doxygen from a directory containing a file called +# DoxygenLayout.xml, doxygen will parse it automatically even if the LAYOUT_FILE +# tag is left empty. + +LAYOUT_FILE = + +# The CITE_BIB_FILES tag can be used to specify one or more bib files containing +# the reference definitions. This must be a list of .bib files. The .bib +# extension is automatically appended if omitted. This requires the bibtex tool +# to be installed. See also https://en.wikipedia.org/wiki/BibTeX for more info. +# For LaTeX the style of the bibliography can be controlled using +# LATEX_BIB_STYLE. To use this feature you need bibtex and perl available in the +# search path. See also \cite for info how to create references. + +CITE_BIB_FILES = + +#--------------------------------------------------------------------------- +# Configuration options related to warning and progress messages +#--------------------------------------------------------------------------- + +# The QUIET tag can be used to turn on/off the messages that are generated to +# standard output by doxygen. If QUIET is set to YES this implies that the +# messages are off. +# The default value is: NO. + +QUIET = NO + +# The WARNINGS tag can be used to turn on/off the warning messages that are +# generated to standard error (stderr) by doxygen. If WARNINGS is set to YES +# this implies that the warnings are on. +# +# Tip: Turn warnings on while writing the documentation. +# The default value is: YES. + +WARNINGS = YES + +# If the WARN_IF_UNDOCUMENTED tag is set to YES then doxygen will generate +# warnings for undocumented members. If EXTRACT_ALL is set to YES then this flag +# will automatically be disabled. +# The default value is: YES. + +WARN_IF_UNDOCUMENTED = YES + +# If the WARN_IF_DOC_ERROR tag is set to YES, doxygen will generate warnings for +# potential errors in the documentation, such as documenting some parameters in +# a documented function twice, or documenting parameters that don't exist or +# using markup commands wrongly. +# The default value is: YES. + +WARN_IF_DOC_ERROR = YES + +# If WARN_IF_INCOMPLETE_DOC is set to YES, doxygen will warn about incomplete +# function parameter documentation. If set to NO, doxygen will accept that some +# parameters have no documentation without warning. +# The default value is: YES. + +WARN_IF_INCOMPLETE_DOC = YES + +# This WARN_NO_PARAMDOC option can be enabled to get warnings for functions that +# are documented, but have no documentation for their parameters or return +# value. If set to NO, doxygen will only warn about wrong parameter +# documentation, but not about the absence of documentation. If EXTRACT_ALL is +# set to YES then this flag will automatically be disabled. See also +# WARN_IF_INCOMPLETE_DOC +# The default value is: NO. + +WARN_NO_PARAMDOC = YES + +# If WARN_IF_UNDOC_ENUM_VAL option is set to YES, doxygen will warn about +# undocumented enumeration values. If set to NO, doxygen will accept +# undocumented enumeration values. If EXTRACT_ALL is set to YES then this flag +# will automatically be disabled. +# The default value is: NO. + +WARN_IF_UNDOC_ENUM_VAL = YES + +# If the WARN_AS_ERROR tag is set to YES then doxygen will immediately stop when +# a warning is encountered. If the WARN_AS_ERROR tag is set to FAIL_ON_WARNINGS +# then doxygen will continue running as if WARN_AS_ERROR tag is set to NO, but +# at the end of the doxygen process doxygen will return with a non-zero status. +# If the WARN_AS_ERROR tag is set to FAIL_ON_WARNINGS_PRINT then doxygen behaves +# like FAIL_ON_WARNINGS but in case no WARN_LOGFILE is defined doxygen will not +# write the warning messages in between other messages but write them at the end +# of a run, in case a WARN_LOGFILE is defined the warning messages will be +# besides being in the defined file also be shown at the end of a run, unless +# the WARN_LOGFILE is defined as - i.e. standard output (stdout) in that case +# the behavior will remain as with the setting FAIL_ON_WARNINGS. +# Possible values are: NO, YES, FAIL_ON_WARNINGS and FAIL_ON_WARNINGS_PRINT. +# The default value is: NO. + +WARN_AS_ERROR = FAIL_ON_WARNINGS_PRINT + +# The WARN_FORMAT tag determines the format of the warning messages that doxygen +# can produce. The string should contain the $file, $line, and $text tags, which +# will be replaced by the file and line number from which the warning originated +# and the warning text. Optionally the format may contain $version, which will +# be replaced by the version of the file (if it could be obtained via +# FILE_VERSION_FILTER) +# See also: WARN_LINE_FORMAT +# The default value is: $file:$line: $text. + +WARN_FORMAT = "$file:$line: $text" + +# In the $text part of the WARN_FORMAT command it is possible that a reference +# to a more specific place is given. To make it easier to jump to this place +# (outside of doxygen) the user can define a custom "cut" / "paste" string. +# Example: +# WARN_LINE_FORMAT = "'vi $file +$line'" +# See also: WARN_FORMAT +# The default value is: at line $line of file $file. + +WARN_LINE_FORMAT = "at line $line of file $file" + +# The WARN_LOGFILE tag can be used to specify a file to which warning and error +# messages should be written. If left blank the output is written to standard +# error (stderr). In case the file specified cannot be opened for writing the +# warning and error messages are written to standard error. When as file - is +# specified the warning and error messages are written to standard output +# (stdout). + +WARN_LOGFILE = doxygen_warnings.txt + +#--------------------------------------------------------------------------- +# Configuration options related to the input files +#--------------------------------------------------------------------------- + +# The INPUT tag is used to specify the files and/or directories that contain +# documented source files. You may enter file names like myfile.cpp or +# directories like /usr/src/myproject. Separate the files or directories with +# spaces. See also FILE_PATTERNS and EXTENSION_MAPPING +# Note: If this tag is empty the current directory is searched. + +INPUT = mainpage.dox \ + ../../src \ + ../../lib/Service \ + ../../lib/HALReinforcementLearningSim \ + ../../lib/APPReinforcementLearning + +# This tag can be used to specify the character encoding of the source files +# that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses +# libiconv (or the iconv built into libc) for the transcoding. See the libiconv +# documentation (see: +# https://www.gnu.org/software/libiconv/) for the list of possible encodings. +# See also: INPUT_FILE_ENCODING +# The default value is: UTF-8. + +INPUT_ENCODING = UTF-8 + +# This tag can be used to specify the character encoding of the source files +# that doxygen parses The INPUT_FILE_ENCODING tag can be used to specify +# character encoding on a per file pattern basis. Doxygen will compare the file +# name with each pattern and apply the encoding instead of the default +# INPUT_ENCODING) if there is a match. The character encodings are a list of the +# form: pattern=encoding (like *.php=ISO-8859-1). See cfg_input_encoding +# "INPUT_ENCODING" for further information on supported encodings. + +INPUT_FILE_ENCODING = + +# If the value of the INPUT tag contains directories, you can use the +# FILE_PATTERNS tag to specify one or more wildcard patterns (like *.cpp and +# *.h) to filter out the source-files in the directories. +# +# Note that for custom extensions or not directly supported extensions you also +# need to set EXTENSION_MAPPING for the extension otherwise the files are not +# read by doxygen. +# +# Note the list of default checked file patterns might differ from the list of +# default file extension mappings. +# +# If left blank the following patterns are tested:*.c, *.cc, *.cxx, *.cxxm, +# *.cpp, *.cppm, *.c++, *.c++m, *.java, *.ii, *.ixx, *.ipp, *.i++, *.inl, *.idl, +# *.ddl, *.odl, *.h, *.hh, *.hxx, *.hpp, *.h++, *.ixx, *.l, *.cs, *.d, *.php, +# *.php4, *.php5, *.phtml, *.inc, *.m, *.markdown, *.md, *.mm, *.dox (to be +# provided as doxygen C comment), *.py, *.pyw, *.f90, *.f95, *.f03, *.f08, +# *.f18, *.f, *.for, *.vhd, *.vhdl, *.ucf, *.qsf and *.ice. + +FILE_PATTERNS = *.c \ + *.cc \ + *.cxx \ + *.cpp \ + *.c++ \ + *.java \ + *.ii \ + *.ixx \ + *.ipp \ + *.i++ \ + *.inl \ + *.idl \ + *.ddl \ + *.odl \ + *.h \ + *.hh \ + *.hxx \ + *.hpp \ + *.h++ \ + *.cs \ + *.d \ + *.php \ + *.php4 \ + *.php5 \ + *.phtml \ + *.inc \ + *.m \ + *.markdown \ + *.md \ + *.mm \ + *.dox \ + *.py \ + *.pyw \ + *.f90 \ + *.f95 \ + *.f03 \ + *.f08 \ + *.f \ + *.for \ + *.tcl \ + *.vhd \ + *.vhdl \ + *.ucf \ + *.qsf + +# The RECURSIVE tag can be used to specify whether or not subdirectories should +# be searched for input files as well. +# The default value is: NO. + +RECURSIVE = YES + +# The EXCLUDE tag can be used to specify files and/or directories that should be +# excluded from the INPUT source files. This way you can easily exclude a +# subdirectory from a directory tree whose root is specified with the INPUT tag. +# +# Note that relative paths are relative to the directory from which doxygen is +# run. + +EXCLUDE = + +# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or +# directories that are symbolic links (a Unix file system feature) are excluded +# from the input. +# The default value is: NO. + +EXCLUDE_SYMLINKS = NO + +# If the value of the INPUT tag contains directories, you can use the +# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude +# certain files from those directories. +# +# Note that the wildcards are matched against the file with absolute path, so to +# exclude all test directories for example use the pattern */test/* + +EXCLUDE_PATTERNS = + +# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names +# (namespaces, classes, functions, etc.) that should be excluded from the +# output. The symbol name can be a fully qualified name, a word, or if the +# wildcard * is used, a substring. Examples: ANamespace, AClass, +# ANamespace::AClass, ANamespace::*Test + +EXCLUDE_SYMBOLS = + +# The EXAMPLE_PATH tag can be used to specify one or more files or directories +# that contain example code fragments that are included (see the \include +# command). + +EXAMPLE_PATH = + +# If the value of the EXAMPLE_PATH tag contains directories, you can use the +# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp and +# *.h) to filter out the source-files in the directories. If left blank all +# files are included. + +EXAMPLE_PATTERNS = * + +# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be +# searched for input files to be used with the \include or \dontinclude commands +# irrespective of the value of the RECURSIVE tag. +# The default value is: NO. + +EXAMPLE_RECURSIVE = NO + +# The IMAGE_PATH tag can be used to specify one or more files or directories +# that contain images that are to be included in the documentation (see the +# \image command). + +IMAGE_PATH = + +# The INPUT_FILTER tag can be used to specify a program that doxygen should +# invoke to filter for each input file. Doxygen will invoke the filter program +# by executing (via popen()) the command: +# +# +# +# where is the value of the INPUT_FILTER tag, and is the +# name of an input file. Doxygen will then use the output that the filter +# program writes to standard output. If FILTER_PATTERNS is specified, this tag +# will be ignored. +# +# Note that the filter must not add or remove lines; it is applied before the +# code is scanned, but not when the output code is generated. If lines are added +# or removed, the anchors will not be placed correctly. +# +# Note that doxygen will use the data processed and written to standard output +# for further processing, therefore nothing else, like debug statements or used +# commands (so in case of a Windows batch file always use @echo OFF), should be +# written to standard output. +# +# Note that for custom extensions or not directly supported extensions you also +# need to set EXTENSION_MAPPING for the extension otherwise the files are not +# properly processed by doxygen. + +INPUT_FILTER = + +# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern +# basis. Doxygen will compare the file name with each pattern and apply the +# filter if there is a match. The filters are a list of the form: pattern=filter +# (like *.cpp=my_cpp_filter). See INPUT_FILTER for further information on how +# filters are used. If the FILTER_PATTERNS tag is empty or if none of the +# patterns match the file name, INPUT_FILTER is applied. +# +# Note that for custom extensions or not directly supported extensions you also +# need to set EXTENSION_MAPPING for the extension otherwise the files are not +# properly processed by doxygen. + +FILTER_PATTERNS = + +# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using +# INPUT_FILTER) will also be used to filter the input files that are used for +# producing the source files to browse (i.e. when SOURCE_BROWSER is set to YES). +# The default value is: NO. + +FILTER_SOURCE_FILES = NO + +# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file +# pattern. A pattern will override the setting for FILTER_PATTERN (if any) and +# it is also possible to disable source filtering for a specific pattern using +# *.ext= (so without naming a filter). +# This tag requires that the tag FILTER_SOURCE_FILES is set to YES. + +FILTER_SOURCE_PATTERNS = + +# If the USE_MDFILE_AS_MAINPAGE tag refers to the name of a markdown file that +# is part of the input, its contents will be placed on the main page +# (index.html). This can be useful if you have a project on for instance GitHub +# and want to reuse the introduction page also for the doxygen output. + +USE_MDFILE_AS_MAINPAGE = + +# The Fortran standard specifies that for fixed formatted Fortran code all +# characters from position 72 are to be considered as comment. A common +# extension is to allow longer lines before the automatic comment starts. The +# setting FORTRAN_COMMENT_AFTER will also make it possible that longer lines can +# be processed before the automatic comment starts. +# Minimum value: 7, maximum value: 10000, default value: 72. + +FORTRAN_COMMENT_AFTER = 72 + +#--------------------------------------------------------------------------- +# Configuration options related to source browsing +#--------------------------------------------------------------------------- + +# If the SOURCE_BROWSER tag is set to YES then a list of source files will be +# generated. Documented entities will be cross-referenced with these sources. +# +# Note: To get rid of all source code in the generated output, make sure that +# also VERBATIM_HEADERS is set to NO. +# The default value is: NO. + +SOURCE_BROWSER = NO + +# Setting the INLINE_SOURCES tag to YES will include the body of functions, +# classes and enums directly into the documentation. +# The default value is: NO. + +INLINE_SOURCES = NO + +# Setting the STRIP_CODE_COMMENTS tag to YES will instruct doxygen to hide any +# special comment blocks from generated source code fragments. Normal C, C++ and +# Fortran comments will always remain visible. +# The default value is: YES. + +STRIP_CODE_COMMENTS = YES + +# If the REFERENCED_BY_RELATION tag is set to YES then for each documented +# entity all documented functions referencing it will be listed. +# The default value is: NO. + +REFERENCED_BY_RELATION = NO + +# If the REFERENCES_RELATION tag is set to YES then for each documented function +# all documented entities called/used by that function will be listed. +# The default value is: NO. + +REFERENCES_RELATION = NO + +# If the REFERENCES_LINK_SOURCE tag is set to YES and SOURCE_BROWSER tag is set +# to YES then the hyperlinks from functions in REFERENCES_RELATION and +# REFERENCED_BY_RELATION lists will link to the source code. Otherwise they will +# link to the documentation. +# The default value is: YES. + +REFERENCES_LINK_SOURCE = YES + +# If SOURCE_TOOLTIPS is enabled (the default) then hovering a hyperlink in the +# source code will show a tooltip with additional information such as prototype, +# brief description and links to the definition and documentation. Since this +# will make the HTML file larger and loading of large files a bit slower, you +# can opt to disable this feature. +# The default value is: YES. +# This tag requires that the tag SOURCE_BROWSER is set to YES. + +SOURCE_TOOLTIPS = YES + +# If the USE_HTAGS tag is set to YES then the references to source code will +# point to the HTML generated by the htags(1) tool instead of doxygen built-in +# source browser. The htags tool is part of GNU's global source tagging system +# (see https://www.gnu.org/software/global/global.html). You will need version +# 4.8.6 or higher. +# +# To use it do the following: +# - Install the latest version of global +# - Enable SOURCE_BROWSER and USE_HTAGS in the configuration file +# - Make sure the INPUT points to the root of the source tree +# - Run doxygen as normal +# +# Doxygen will invoke htags (and that will in turn invoke gtags), so these +# tools must be available from the command line (i.e. in the search path). +# +# The result: instead of the source browser generated by doxygen, the links to +# source code will now point to the output of htags. +# The default value is: NO. +# This tag requires that the tag SOURCE_BROWSER is set to YES. + +USE_HTAGS = NO + +# If the VERBATIM_HEADERS tag is set the YES then doxygen will generate a +# verbatim copy of the header file for each class for which an include is +# specified. Set to NO to disable this. +# See also: Section \class. +# The default value is: YES. + +VERBATIM_HEADERS = YES + +# If the CLANG_ASSISTED_PARSING tag is set to YES then doxygen will use the +# clang parser (see: +# http://clang.llvm.org/) for more accurate parsing at the cost of reduced +# performance. This can be particularly helpful with template rich C++ code for +# which doxygen's built-in parser lacks the necessary type information. +# Note: The availability of this option depends on whether or not doxygen was +# generated with the -Duse_libclang=ON option for CMake. +# The default value is: NO. + +CLANG_ASSISTED_PARSING = NO + +# If the CLANG_ASSISTED_PARSING tag is set to YES and the CLANG_ADD_INC_PATHS +# tag is set to YES then doxygen will add the directory of each input to the +# include path. +# The default value is: YES. +# This tag requires that the tag CLANG_ASSISTED_PARSING is set to YES. + +CLANG_ADD_INC_PATHS = YES + +# If clang assisted parsing is enabled you can provide the compiler with command +# line options that you would normally use when invoking the compiler. Note that +# the include paths will already be set by doxygen for the files and directories +# specified with INPUT and INCLUDE_PATH. +# This tag requires that the tag CLANG_ASSISTED_PARSING is set to YES. + +CLANG_OPTIONS = + +# If clang assisted parsing is enabled you can provide the clang parser with the +# path to the directory containing a file called compile_commands.json. This +# file is the compilation database (see: +# http://clang.llvm.org/docs/HowToSetupToolingForLLVM.html) containing the +# options used when the source files were built. This is equivalent to +# specifying the -p option to a clang tool, such as clang-check. These options +# will then be passed to the parser. Any options specified with CLANG_OPTIONS +# will be added as well. +# Note: The availability of this option depends on whether or not doxygen was +# generated with the -Duse_libclang=ON option for CMake. + +CLANG_DATABASE_PATH = + +#--------------------------------------------------------------------------- +# Configuration options related to the alphabetical class index +#--------------------------------------------------------------------------- + +# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index of all +# compounds will be generated. Enable this if the project contains a lot of +# classes, structs, unions or interfaces. +# The default value is: YES. + +ALPHABETICAL_INDEX = YES + +# The IGNORE_PREFIX tag can be used to specify a prefix (or a list of prefixes) +# that should be ignored while generating the index headers. The IGNORE_PREFIX +# tag works for classes, function and member names. The entity will be placed in +# the alphabetical list under the first letter of the entity name that remains +# after removing the prefix. +# This tag requires that the tag ALPHABETICAL_INDEX is set to YES. + +IGNORE_PREFIX = + +#--------------------------------------------------------------------------- +# Configuration options related to the HTML output +#--------------------------------------------------------------------------- + +# If the GENERATE_HTML tag is set to YES, doxygen will generate HTML output +# The default value is: YES. + +GENERATE_HTML = YES + +# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. If a +# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of +# it. +# The default directory is: html. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_OUTPUT = html + +# The HTML_FILE_EXTENSION tag can be used to specify the file extension for each +# generated HTML page (for example: .htm, .php, .asp). +# The default value is: .html. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_FILE_EXTENSION = .html + +# The HTML_HEADER tag can be used to specify a user-defined HTML header file for +# each generated HTML page. If the tag is left blank doxygen will generate a +# standard header. +# +# To get valid HTML the header file that includes any scripts and style sheets +# that doxygen needs, which is dependent on the configuration options used (e.g. +# the setting GENERATE_TREEVIEW). It is highly recommended to start with a +# default header using +# doxygen -w html new_header.html new_footer.html new_stylesheet.css +# YourConfigFile +# and then modify the file new_header.html. See also section "Doxygen usage" +# for information on how to generate the default header that doxygen normally +# uses. +# Note: The header is subject to change so you typically have to regenerate the +# default header when upgrading to a newer version of doxygen. For a description +# of the possible markers and block names see the documentation. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_HEADER = + +# The HTML_FOOTER tag can be used to specify a user-defined HTML footer for each +# generated HTML page. If the tag is left blank doxygen will generate a standard +# footer. See HTML_HEADER for more information on how to generate a default +# footer and what special commands can be used inside the footer. See also +# section "Doxygen usage" for information on how to generate the default footer +# that doxygen normally uses. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_FOOTER = + +# The HTML_STYLESHEET tag can be used to specify a user-defined cascading style +# sheet that is used by each HTML page. It can be used to fine-tune the look of +# the HTML output. If left blank doxygen will generate a default style sheet. +# See also section "Doxygen usage" for information on how to generate the style +# sheet that doxygen normally uses. +# Note: It is recommended to use HTML_EXTRA_STYLESHEET instead of this tag, as +# it is more robust and this tag (HTML_STYLESHEET) will in the future become +# obsolete. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_STYLESHEET = + +# The HTML_EXTRA_STYLESHEET tag can be used to specify additional user-defined +# cascading style sheets that are included after the standard style sheets +# created by doxygen. Using this option one can overrule certain style aspects. +# This is preferred over using HTML_STYLESHEET since it does not replace the +# standard style sheet and is therefore more robust against future updates. +# Doxygen will copy the style sheet files to the output directory. +# Note: The order of the extra style sheet files is of importance (e.g. the last +# style sheet in the list overrules the setting of the previous ones in the +# list). +# Note: Since the styling of scrollbars can currently not be overruled in +# Webkit/Chromium, the styling will be left out of the default doxygen.css if +# one or more extra stylesheets have been specified. So if scrollbar +# customization is desired it has to be added explicitly. For an example see the +# documentation. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_EXTRA_STYLESHEET = + +# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or +# other source files which should be copied to the HTML output directory. Note +# that these files will be copied to the base HTML output directory. Use the +# $relpath^ marker in the HTML_HEADER and/or HTML_FOOTER files to load these +# files. In the HTML_STYLESHEET file, use the file name only. Also note that the +# files will be copied as-is; there are no commands or markers available. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_EXTRA_FILES = + +# The HTML_COLORSTYLE tag can be used to specify if the generated HTML output +# should be rendered with a dark or light theme. +# Possible values are: LIGHT always generate light mode output, DARK always +# generate dark mode output, AUTO_LIGHT automatically set the mode according to +# the user preference, use light mode if no preference is set (the default), +# AUTO_DARK automatically set the mode according to the user preference, use +# dark mode if no preference is set and TOGGLE allow to user to switch between +# light and dark mode via a button. +# The default value is: AUTO_LIGHT. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_COLORSTYLE = AUTO_LIGHT + +# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. Doxygen +# will adjust the colors in the style sheet and background images according to +# this color. Hue is specified as an angle on a color-wheel, see +# https://en.wikipedia.org/wiki/Hue for more information. For instance the value +# 0 represents red, 60 is yellow, 120 is green, 180 is cyan, 240 is blue, 300 +# purple, and 360 is red again. +# Minimum value: 0, maximum value: 359, default value: 220. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_COLORSTYLE_HUE = 220 + +# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of the colors +# in the HTML output. For a value of 0 the output will use gray-scales only. A +# value of 255 will produce the most vivid colors. +# Minimum value: 0, maximum value: 255, default value: 100. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_COLORSTYLE_SAT = 100 + +# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to the +# luminance component of the colors in the HTML output. Values below 100 +# gradually make the output lighter, whereas values above 100 make the output +# darker. The value divided by 100 is the actual gamma applied, so 80 represents +# a gamma of 0.8, The value 220 represents a gamma of 2.2, and 100 does not +# change the gamma. +# Minimum value: 40, maximum value: 240, default value: 80. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_COLORSTYLE_GAMMA = 80 + +# If the HTML_DYNAMIC_MENUS tag is set to YES then the generated HTML +# documentation will contain a main index with vertical navigation menus that +# are dynamically created via JavaScript. If disabled, the navigation index will +# consists of multiple levels of tabs that are statically embedded in every HTML +# page. Disable this option to support browsers that do not have JavaScript, +# like the Qt help browser. +# The default value is: YES. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_DYNAMIC_MENUS = YES + +# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML +# documentation will contain sections that can be hidden and shown after the +# page has loaded. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_DYNAMIC_SECTIONS = NO + +# If the HTML_CODE_FOLDING tag is set to YES then classes and functions can be +# dynamically folded and expanded in the generated HTML source code. +# The default value is: YES. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_CODE_FOLDING = YES + +# With HTML_INDEX_NUM_ENTRIES one can control the preferred number of entries +# shown in the various tree structured indices initially; the user can expand +# and collapse entries dynamically later on. Doxygen will expand the tree to +# such a level that at most the specified number of entries are visible (unless +# a fully collapsed tree already exceeds this amount). So setting the number of +# entries 1 will produce a full collapsed tree by default. 0 is a special value +# representing an infinite number of entries and will result in a full expanded +# tree by default. +# Minimum value: 0, maximum value: 9999, default value: 100. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_INDEX_NUM_ENTRIES = 100 + +# If the GENERATE_DOCSET tag is set to YES, additional index files will be +# generated that can be used as input for Apple's Xcode 3 integrated development +# environment (see: +# https://developer.apple.com/xcode/), introduced with OSX 10.5 (Leopard). To +# create a documentation set, doxygen will generate a Makefile in the HTML +# output directory. Running make will produce the docset in that directory and +# running make install will install the docset in +# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find it at +# startup. See https://developer.apple.com/library/archive/featuredarticles/Doxy +# genXcode/_index.html for more information. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_DOCSET = NO + +# This tag determines the name of the docset feed. A documentation feed provides +# an umbrella under which multiple documentation sets from a single provider +# (such as a company or product suite) can be grouped. +# The default value is: Doxygen generated docs. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_FEEDNAME = "Doxygen generated docs" + +# This tag determines the URL of the docset feed. A documentation feed provides +# an umbrella under which multiple documentation sets from a single provider +# (such as a company or product suite) can be grouped. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_FEEDURL = + +# This tag specifies a string that should uniquely identify the documentation +# set bundle. This should be a reverse domain-name style string, e.g. +# com.mycompany.MyDocSet. Doxygen will append .docset to the name. +# The default value is: org.doxygen.Project. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_BUNDLE_ID = org.doxygen.Project + +# The DOCSET_PUBLISHER_ID tag specifies a string that should uniquely identify +# the documentation publisher. This should be a reverse domain-name style +# string, e.g. com.mycompany.MyDocSet.documentation. +# The default value is: org.doxygen.Publisher. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_PUBLISHER_ID = org.doxygen.Publisher + +# The DOCSET_PUBLISHER_NAME tag identifies the documentation publisher. +# The default value is: Publisher. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_PUBLISHER_NAME = Publisher + +# If the GENERATE_HTMLHELP tag is set to YES then doxygen generates three +# additional HTML index files: index.hhp, index.hhc, and index.hhk. The +# index.hhp is a project file that can be read by Microsoft's HTML Help Workshop +# on Windows. In the beginning of 2021 Microsoft took the original page, with +# a.o. the download links, offline the HTML help workshop was already many years +# in maintenance mode). You can download the HTML help workshop from the web +# archives at Installation executable (see: +# http://web.archive.org/web/20160201063255/http://download.microsoft.com/downlo +# ad/0/A/9/0A939EF6-E31C-430F-A3DF-DFAE7960D564/htmlhelp.exe). +# +# The HTML Help Workshop contains a compiler that can convert all HTML output +# generated by doxygen into a single compiled HTML file (.chm). Compiled HTML +# files are now used as the Windows 98 help format, and will replace the old +# Windows help format (.hlp) on all Windows platforms in the future. Compressed +# HTML files also contain an index, a table of contents, and you can search for +# words in the documentation. The HTML workshop also contains a viewer for +# compressed HTML files. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_HTMLHELP = NO + +# The CHM_FILE tag can be used to specify the file name of the resulting .chm +# file. You can add a path in front of the file if the result should not be +# written to the html output directory. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +CHM_FILE = + +# The HHC_LOCATION tag can be used to specify the location (absolute path +# including file name) of the HTML help compiler (hhc.exe). If non-empty, +# doxygen will try to run the HTML help compiler on the generated index.hhp. +# The file has to be specified with full path. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +HHC_LOCATION = + +# The GENERATE_CHI flag controls if a separate .chi index file is generated +# (YES) or that it should be included in the main .chm file (NO). +# The default value is: NO. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +GENERATE_CHI = NO + +# The CHM_INDEX_ENCODING is used to encode HtmlHelp index (hhk), content (hhc) +# and project file content. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +CHM_INDEX_ENCODING = + +# The BINARY_TOC flag controls whether a binary table of contents is generated +# (YES) or a normal table of contents (NO) in the .chm file. Furthermore it +# enables the Previous and Next buttons. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +BINARY_TOC = NO + +# The TOC_EXPAND flag can be set to YES to add extra items for group members to +# the table of contents of the HTML help documentation and to the tree view. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +TOC_EXPAND = NO + +# The SITEMAP_URL tag is used to specify the full URL of the place where the +# generated documentation will be placed on the server by the user during the +# deployment of the documentation. The generated sitemap is called sitemap.xml +# and placed on the directory specified by HTML_OUTPUT. In case no SITEMAP_URL +# is specified no sitemap is generated. For information about the sitemap +# protocol see https://www.sitemaps.org +# This tag requires that the tag GENERATE_HTML is set to YES. + +SITEMAP_URL = + +# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and +# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated that +# can be used as input for Qt's qhelpgenerator to generate a Qt Compressed Help +# (.qch) of the generated HTML documentation. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_QHP = NO + +# If the QHG_LOCATION tag is specified, the QCH_FILE tag can be used to specify +# the file name of the resulting .qch file. The path specified is relative to +# the HTML output folder. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QCH_FILE = + +# The QHP_NAMESPACE tag specifies the namespace to use when generating Qt Help +# Project output. For more information please see Qt Help Project / Namespace +# (see: +# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#namespace). +# The default value is: org.doxygen.Project. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_NAMESPACE = org.doxygen.Project + +# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating Qt +# Help Project output. For more information please see Qt Help Project / Virtual +# Folders (see: +# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#virtual-folders). +# The default value is: doc. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_VIRTUAL_FOLDER = doc + +# If the QHP_CUST_FILTER_NAME tag is set, it specifies the name of a custom +# filter to add. For more information please see Qt Help Project / Custom +# Filters (see: +# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#custom-filters). +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_CUST_FILTER_NAME = + +# The QHP_CUST_FILTER_ATTRS tag specifies the list of the attributes of the +# custom filter to add. For more information please see Qt Help Project / Custom +# Filters (see: +# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#custom-filters). +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_CUST_FILTER_ATTRS = + +# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this +# project's filter section matches. Qt Help Project / Filter Attributes (see: +# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#filter-attributes). +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_SECT_FILTER_ATTRS = + +# The QHG_LOCATION tag can be used to specify the location (absolute path +# including file name) of Qt's qhelpgenerator. If non-empty doxygen will try to +# run qhelpgenerator on the generated .qhp file. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHG_LOCATION = + +# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files will be +# generated, together with the HTML files, they form an Eclipse help plugin. To +# install this plugin and make it available under the help contents menu in +# Eclipse, the contents of the directory containing the HTML and XML files needs +# to be copied into the plugins directory of eclipse. The name of the directory +# within the plugins directory should be the same as the ECLIPSE_DOC_ID value. +# After copying Eclipse needs to be restarted before the help appears. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_ECLIPSEHELP = NO + +# A unique identifier for the Eclipse help plugin. When installing the plugin +# the directory name containing the HTML and XML files should also have this +# name. Each documentation set should have its own identifier. +# The default value is: org.doxygen.Project. +# This tag requires that the tag GENERATE_ECLIPSEHELP is set to YES. + +ECLIPSE_DOC_ID = org.doxygen.Project + +# If you want full control over the layout of the generated HTML pages it might +# be necessary to disable the index and replace it with your own. The +# DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) at top +# of each HTML page. A value of NO enables the index and the value YES disables +# it. Since the tabs in the index contain the same information as the navigation +# tree, you can set this option to YES if you also set GENERATE_TREEVIEW to YES. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +DISABLE_INDEX = NO + +# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index +# structure should be generated to display hierarchical information. If the tag +# value is set to YES, a side panel will be generated containing a tree-like +# index structure (just like the one that is generated for HTML Help). For this +# to work a browser that supports JavaScript, DHTML, CSS and frames is required +# (i.e. any modern browser). Windows users are probably better off using the +# HTML help feature. Via custom style sheets (see HTML_EXTRA_STYLESHEET) one can +# further fine tune the look of the index (see "Fine-tuning the output"). As an +# example, the default style sheet generated by doxygen has an example that +# shows how to put an image at the root of the tree instead of the PROJECT_NAME. +# Since the tree basically has the same information as the tab index, you could +# consider setting DISABLE_INDEX to YES when enabling this option. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_TREEVIEW = NO + +# When both GENERATE_TREEVIEW and DISABLE_INDEX are set to YES, then the +# FULL_SIDEBAR option determines if the side bar is limited to only the treeview +# area (value NO) or if it should extend to the full height of the window (value +# YES). Setting this to YES gives a layout similar to +# https://docs.readthedocs.io with more room for contents, but less room for the +# project logo, title, and description. If either GENERATE_TREEVIEW or +# DISABLE_INDEX is set to NO, this option has no effect. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +FULL_SIDEBAR = NO + +# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values that +# doxygen will group on one line in the generated HTML documentation. +# +# Note that a value of 0 will completely suppress the enum values from appearing +# in the overview section. +# Minimum value: 0, maximum value: 20, default value: 4. +# This tag requires that the tag GENERATE_HTML is set to YES. + +ENUM_VALUES_PER_LINE = 4 + +# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be used +# to set the initial width (in pixels) of the frame in which the tree is shown. +# Minimum value: 0, maximum value: 1500, default value: 250. +# This tag requires that the tag GENERATE_HTML is set to YES. + +TREEVIEW_WIDTH = 250 + +# If the EXT_LINKS_IN_WINDOW option is set to YES, doxygen will open links to +# external symbols imported via tag files in a separate window. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +EXT_LINKS_IN_WINDOW = NO + +# If the OBFUSCATE_EMAILS tag is set to YES, doxygen will obfuscate email +# addresses. +# The default value is: YES. +# This tag requires that the tag GENERATE_HTML is set to YES. + +OBFUSCATE_EMAILS = YES + +# If the HTML_FORMULA_FORMAT option is set to svg, doxygen will use the pdf2svg +# tool (see https://github.com/dawbarton/pdf2svg) or inkscape (see +# https://inkscape.org) to generate formulas as SVG images instead of PNGs for +# the HTML output. These images will generally look nicer at scaled resolutions. +# Possible values are: png (the default) and svg (looks nicer but requires the +# pdf2svg or inkscape tool). +# The default value is: png. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_FORMULA_FORMAT = png + +# Use this tag to change the font size of LaTeX formulas included as images in +# the HTML documentation. When you change the font size after a successful +# doxygen run you need to manually remove any form_*.png images from the HTML +# output directory to force them to be regenerated. +# Minimum value: 8, maximum value: 50, default value: 10. +# This tag requires that the tag GENERATE_HTML is set to YES. + +FORMULA_FONTSIZE = 10 + +# The FORMULA_MACROFILE can contain LaTeX \newcommand and \renewcommand commands +# to create new LaTeX commands to be used in formulas as building blocks. See +# the section "Including formulas" for details. + +FORMULA_MACROFILE = + +# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax (see +# https://www.mathjax.org) which uses client side JavaScript for the rendering +# instead of using pre-rendered bitmaps. Use this if you do not have LaTeX +# installed or if you want to formulas look prettier in the HTML output. When +# enabled you may also need to install MathJax separately and configure the path +# to it using the MATHJAX_RELPATH option. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +USE_MATHJAX = NO + +# With MATHJAX_VERSION it is possible to specify the MathJax version to be used. +# Note that the different versions of MathJax have different requirements with +# regards to the different settings, so it is possible that also other MathJax +# settings have to be changed when switching between the different MathJax +# versions. +# Possible values are: MathJax_2 and MathJax_3. +# The default value is: MathJax_2. +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_VERSION = MathJax_2 + +# When MathJax is enabled you can set the default output format to be used for +# the MathJax output. For more details about the output format see MathJax +# version 2 (see: +# http://docs.mathjax.org/en/v2.7-latest/output.html) and MathJax version 3 +# (see: +# http://docs.mathjax.org/en/latest/web/components/output.html). +# Possible values are: HTML-CSS (which is slower, but has the best +# compatibility. This is the name for Mathjax version 2, for MathJax version 3 +# this will be translated into chtml), NativeMML (i.e. MathML. Only supported +# for NathJax 2. For MathJax version 3 chtml will be used instead.), chtml (This +# is the name for Mathjax version 3, for MathJax version 2 this will be +# translated into HTML-CSS) and SVG. +# The default value is: HTML-CSS. +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_FORMAT = HTML-CSS + +# When MathJax is enabled you need to specify the location relative to the HTML +# output directory using the MATHJAX_RELPATH option. The destination directory +# should contain the MathJax.js script. For instance, if the mathjax directory +# is located at the same level as the HTML output directory, then +# MATHJAX_RELPATH should be ../mathjax. The default value points to the MathJax +# Content Delivery Network so you can quickly see the result without installing +# MathJax. However, it is strongly recommended to install a local copy of +# MathJax from https://www.mathjax.org before deployment. The default value is: +# - in case of MathJax version 2: https://cdn.jsdelivr.net/npm/mathjax@2 +# - in case of MathJax version 3: https://cdn.jsdelivr.net/npm/mathjax@3 +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_RELPATH = http://cdn.mathjax.org/mathjax/latest + +# The MATHJAX_EXTENSIONS tag can be used to specify one or more MathJax +# extension names that should be enabled during MathJax rendering. For example +# for MathJax version 2 (see +# https://docs.mathjax.org/en/v2.7-latest/tex.html#tex-and-latex-extensions): +# MATHJAX_EXTENSIONS = TeX/AMSmath TeX/AMSsymbols +# For example for MathJax version 3 (see +# http://docs.mathjax.org/en/latest/input/tex/extensions/index.html): +# MATHJAX_EXTENSIONS = ams +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_EXTENSIONS = + +# The MATHJAX_CODEFILE tag can be used to specify a file with javascript pieces +# of code that will be used on startup of the MathJax code. See the MathJax site +# (see: +# http://docs.mathjax.org/en/v2.7-latest/output.html) for more details. For an +# example see the documentation. +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_CODEFILE = + +# When the SEARCHENGINE tag is enabled doxygen will generate a search box for +# the HTML output. The underlying search engine uses javascript and DHTML and +# should work on any modern browser. Note that when using HTML help +# (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets (GENERATE_DOCSET) +# there is already a search function so this one should typically be disabled. +# For large projects the javascript based search engine can be slow, then +# enabling SERVER_BASED_SEARCH may provide a better solution. It is possible to +# search using the keyboard; to jump to the search box use + S +# (what the is depends on the OS and browser, but it is typically +# , /