diff --git a/.vscode/templates.code-snippets b/.vscode/templates.code-snippets index 7e2a6e9d..390f2cc2 100644 --- a/.vscode/templates.code-snippets +++ b/.vscode/templates.code-snippets @@ -1,209 +1,209 @@ -{ - // Place your RadonUlzer workspace snippets here. Each snippet is defined under a snippet name and has a scope, prefix, body and - // description. Add comma separated ids of the languages where the snippet is applicable in the scope field. If scope - // is left empty or omitted, the snippet gets applied to all languages. The prefix is what is - // used to trigger the snippet and the body will be expanded and inserted. Possible variables are: - // $1, $2 for tab stops, $0 for the final cursor position, and ${1:label}, ${2:another} for placeholders. - // Placeholders with the same ids are connected. - // Example: - // "Print to console": { - // "scope": "javascript,typescript", - // "prefix": "log", - // "body": [ - // "console.log('$1');", - // "$2" - // ], - // "description": "Log output to console" - // } - "Simple Comment": { - "prefix": "comment", - "scope": "c, cpp", - "body": [ - "/* ${1:Comment} */" - ], - "description": "Simple C-styled comment." - }, - "Doxygen In-Line Comment": { - "prefix": "doxy_inline", - "scope": "c, cpp", - "body": [ - "/**< ${1:Comment} */", - ], - "description": "Simple in-line Doxygen comment." - }, - "Doxygen Comment": { - "prefix": "doxy_comment", - "scope": "c, cpp", - "body": [ - "/** ${1:Comment} */", - "$0" - ], - "description": "Simple Doxygen comment." - }, - "Doxygen Block Comment/Description": { - "prefix": "doxy_block", - "scope": "c, cpp", - "body": [ - "/**", - " * ${1:description}", - " *", - " * ${2:parameters}", - " *", - " * ${3:returns}", - " */", - "$0" - ], - "description": "Doxygen comment/description block." - }, - "Template Header File": { - "prefix": "template_header", - "scope": "c, cpp", - "body": [ - "/* MIT License", - " *", - " * Copyright (c) $CURRENT_YEAR 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 ${1:Description}.", - " * @author ${2:Author}", - " *", - " * @addtogroup ${3:doxygen group}", - " *", - " * @{", - " */", - "#ifndef ${4:FILE_NAME}_H", - "#define ${4:FILE_NAME}_H", - "", - "/******************************************************************************", - " * Compile Switches", - " *****************************************************************************/", - "$0", - "/******************************************************************************", - " * Includes", - " *****************************************************************************/", - "", - "/******************************************************************************", - " * Macros", - " *****************************************************************************/", - "", - "/******************************************************************************", - " * Types and Classes", - " *****************************************************************************/", - "", - "/******************************************************************************", - " * Functions", - " *****************************************************************************/", - "", - "#endif /* ${4:FILE_NAME}_H */", - "/** @} */", - "" - ], - "description": "Template for a header file." - }, - "Template Source File": { - "prefix": "template_source", - "scope": "c, cpp", - "body": [ - "/* MIT License", - " *", - " * Copyright (c) $CURRENT_YEAR 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 ${1:Description}", - " * @author ${2:Author}", - " */", - "", - "/******************************************************************************", - " * Includes", - " *****************************************************************************/", - "", - "#include \"${3:File Name}.h\"", - "$0", - "/******************************************************************************", - " * Compiler Switches", - " *****************************************************************************/", - "", - "/******************************************************************************", - " * Macros", - " *****************************************************************************/", - "", - "/******************************************************************************", - " * Types and classes", - " *****************************************************************************/", - "", - "/******************************************************************************", - " * Prototypes", - " *****************************************************************************/", - "", - "/******************************************************************************", - " * Local Variables", - " *****************************************************************************/", - "", - "/******************************************************************************", - " * Public Methods", - " *****************************************************************************/", - "", - "/******************************************************************************", - " * Protected Methods", - " *****************************************************************************/", - "", - "/******************************************************************************", - " * Private Methods", - " *****************************************************************************/", - "", - "/******************************************************************************", - " * External Functions", - " *****************************************************************************/", - "", - "/******************************************************************************", - " * Local Functions", - " *****************************************************************************/", - "" - ], - "description": "Template for a source file." - } +{ + // Place your RadonUlzer workspace snippets here. Each snippet is defined under a snippet name and has a scope, prefix, body and + // description. Add comma separated ids of the languages where the snippet is applicable in the scope field. If scope + // is left empty or omitted, the snippet gets applied to all languages. The prefix is what is + // used to trigger the snippet and the body will be expanded and inserted. Possible variables are: + // $1, $2 for tab stops, $0 for the final cursor position, and ${1:label}, ${2:another} for placeholders. + // Placeholders with the same ids are connected. + // Example: + // "Print to console": { + // "scope": "javascript,typescript", + // "prefix": "log", + // "body": [ + // "console.log('$1');", + // "$2" + // ], + // "description": "Log output to console" + // } + "Simple Comment": { + "prefix": "comment", + "scope": "c, cpp", + "body": [ + "/* ${1:Comment} */" + ], + "description": "Simple C-styled comment." + }, + "Doxygen In-Line Comment": { + "prefix": "doxy_inline", + "scope": "c, cpp", + "body": [ + "/**< ${1:Comment} */", + ], + "description": "Simple in-line Doxygen comment." + }, + "Doxygen Comment": { + "prefix": "doxy_comment", + "scope": "c, cpp", + "body": [ + "/** ${1:Comment} */", + "$0" + ], + "description": "Simple Doxygen comment." + }, + "Doxygen Block Comment/Description": { + "prefix": "doxy_block", + "scope": "c, cpp", + "body": [ + "/**", + " * ${1:description}", + " *", + " * ${2:parameters}", + " *", + " * ${3:returns}", + " */", + "$0" + ], + "description": "Doxygen comment/description block." + }, + "Template Header File": { + "prefix": "template_header", + "scope": "c, cpp", + "body": [ + "/* MIT License", + " *", + " * Copyright (c) $CURRENT_YEAR 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 ${1:Description}.", + " * @author ${2:Author}", + " *", + " * @addtogroup ${3:doxygen group}", + " *", + " * @{", + " */", + "#ifndef ${4:FILE_NAME}_H", + "#define ${4:FILE_NAME}_H", + "", + "/******************************************************************************", + " * Compile Switches", + " *****************************************************************************/", + "$0", + "/******************************************************************************", + " * Includes", + " *****************************************************************************/", + "", + "/******************************************************************************", + " * Macros", + " *****************************************************************************/", + "", + "/******************************************************************************", + " * Types and Classes", + " *****************************************************************************/", + "", + "/******************************************************************************", + " * Functions", + " *****************************************************************************/", + "", + "#endif /* ${4:FILE_NAME}_H */", + "/** @} */", + "" + ], + "description": "Template for a header file." + }, + "Template Source File": { + "prefix": "template_source", + "scope": "c, cpp", + "body": [ + "/* MIT License", + " *", + " * Copyright (c) $CURRENT_YEAR 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 ${1:Description}", + " * @author ${2:Author}", + " */", + "", + "/******************************************************************************", + " * Includes", + " *****************************************************************************/", + "", + "#include \"${3:File Name}.h\"", + "$0", + "/******************************************************************************", + " * Compiler Switches", + " *****************************************************************************/", + "", + "/******************************************************************************", + " * Macros", + " *****************************************************************************/", + "", + "/******************************************************************************", + " * Types and classes", + " *****************************************************************************/", + "", + "/******************************************************************************", + " * Prototypes", + " *****************************************************************************/", + "", + "/******************************************************************************", + " * Local Variables", + " *****************************************************************************/", + "", + "/******************************************************************************", + " * Public Methods", + " *****************************************************************************/", + "", + "/******************************************************************************", + " * Protected Methods", + " *****************************************************************************/", + "", + "/******************************************************************************", + " * Private Methods", + " *****************************************************************************/", + "", + "/******************************************************************************", + " * External Functions", + " *****************************************************************************/", + "", + "/******************************************************************************", + " * Local Functions", + " *****************************************************************************/", + "" + ], + "description": "Template for a source file." + } } \ No newline at end of file diff --git a/doc/architecture/SENSORFUSION.md b/doc/architecture/SENSORFUSION.md index 73f44f2c..0089c24e 100644 --- a/doc/architecture/SENSORFUSION.md +++ b/doc/architecture/SENSORFUSION.md @@ -1,72 +1,72 @@ -# Radon Ulzer - SENSORFUSION - -* [General](#general) - * [SerialMuxProt Channels](#serialmuxprot-channels) - * [Tx channel "LINE\_SENS"](#tx-channel-sensor_data) -* [SW Architecture](#sw-architecture) - * [Logical View](#logical-view) - * [Application](#application) -* [Abbreviations](#abbreviations) -* [Issues, Ideas And Bugs](#issues-ideas-and-bugs) -* [License](#license) -* [Contribution](#contribution) - -# General - -The robot sends sensor data (Odometry data and IMU data) via SerialMuxProt. - -On target the physical communication uses the serial. - -On simulation the physical communication uses a socket connection. - -## SerialMuxProt Channels - - -### Tx channel "SENSOR_DATA" -This channel is used to send raw sensor data used for Sensor Fusion on the ZumoComSystem. - -* The datatypes can be found in SerialMuxChannel.h. -* Order: - * Acceleration in X (raw sensor value in digits) - * Acceleration in Y (raw sensor value in digits) - * turnRateZ around Z (raw sensor value in digits) - * Magnetometer value in X (raw sensor value in digits) - * Magnetometer value in Y (raw sensor value in digits) - * Angle calculated by Odometry (in mrad) - * Position in X calculated by Odometry (in mm) - * Position in Y calculated by Odometry (in mm) -* Endianess: Big endian - -# SW Architecture -The following part contains the specific details of the SensorFusion application. - -## Logical View - -### Application -The application uses the same [State Machine](https://github.com/BlueAndi/RadonUlzer/blob/main/doc/architecture/LINEFOLLOWER.md) as the Line Follower Application. - -### HAL -Some changes have been made to the HAL. -![HALSensorFusion](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/SensorFusion/base/doc/architecture/uml/LogicalView/SensorFusion/HAL_SensorFusion.puml) - -ButtonB, ButtonC, the ProximitySensor and the Buzzer have been removed. -An IMU has been added: -![HALIMU](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/SensorFusion/base/doc/architecture/uml/LogicalView/SensorFusion/HAL_IMU.puml) - -# Abbreviations - -| Abbreviation | Description | -| - | - | -| HAL | Hardware Abstraction Layer | -| IMU | Inertial Measurement Unit | - -# Issues, Ideas And Bugs -If you have further ideas or you found some bugs, great! Create a [issue](https://github.com/BlueAndi/RadonUlzer/issues) or if you are able and willing to fix it by yourself, clone the repository and create a pull request. - -# License -The whole source code is published under the [MIT license](http://choosealicense.com/licenses/mit/). -Consider the different licenses of the used third party libraries too! - -# Contribution -Unless you explicitly state otherwise, any contribution intentionally submitted for inclusion in the work by you, shall be licensed as above, without any -additional terms or conditions. +# Radon Ulzer - SENSORFUSION + +* [General](#general) + * [SerialMuxProt Channels](#serialmuxprot-channels) + * [Tx channel "LINE\_SENS"](#tx-channel-sensor_data) +* [SW Architecture](#sw-architecture) + * [Logical View](#logical-view) + * [Application](#application) +* [Abbreviations](#abbreviations) +* [Issues, Ideas And Bugs](#issues-ideas-and-bugs) +* [License](#license) +* [Contribution](#contribution) + +# General + +The robot sends sensor data (Odometry data and IMU data) via SerialMuxProt. + +On target the physical communication uses the serial. + +On simulation the physical communication uses a socket connection. + +## SerialMuxProt Channels + + +### Tx channel "SENSOR_DATA" +This channel is used to send raw sensor data used for Sensor Fusion on the ZumoComSystem. + +* The datatypes can be found in SerialMuxChannel.h. +* Order: + * Acceleration in X (raw sensor value in digits) + * Acceleration in Y (raw sensor value in digits) + * turnRateZ around Z (raw sensor value in digits) + * Magnetometer value in X (raw sensor value in digits) + * Magnetometer value in Y (raw sensor value in digits) + * Angle calculated by Odometry (in mrad) + * Position in X calculated by Odometry (in mm) + * Position in Y calculated by Odometry (in mm) +* Endianess: Big endian + +# SW Architecture +The following part contains the specific details of the SensorFusion application. + +## Logical View + +### Application +The application uses the same [State Machine](https://github.com/BlueAndi/RadonUlzer/blob/main/doc/architecture/LINEFOLLOWER.md) as the Line Follower Application. + +### HAL +Some changes have been made to the HAL. +![HALSensorFusion](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/SensorFusion/base/doc/architecture/uml/LogicalView/SensorFusion/HAL_SensorFusion.puml) + +ButtonB, ButtonC, the ProximitySensor and the Buzzer have been removed. +An IMU has been added: +![HALIMU](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/SensorFusion/base/doc/architecture/uml/LogicalView/SensorFusion/HAL_IMU.puml) + +# Abbreviations + +| Abbreviation | Description | +| - | - | +| HAL | Hardware Abstraction Layer | +| IMU | Inertial Measurement Unit | + +# Issues, Ideas And Bugs +If you have further ideas or you found some bugs, great! Create a [issue](https://github.com/BlueAndi/RadonUlzer/issues) or if you are able and willing to fix it by yourself, clone the repository and create a pull request. + +# License +The whole source code is published under the [MIT license](http://choosealicense.com/licenses/mit/). +Consider the different licenses of the used third party libraries too! + +# Contribution +Unless you explicitly state otherwise, any contribution intentionally submitted for inclusion in the work by you, shall be licensed as above, without any +additional terms or conditions. diff --git a/doc/architecture/uml/LogicalView/SensorFusion/HAL_IMU.puml b/doc/architecture/uml/LogicalView/SensorFusion/HAL_IMU.puml index f1163ca6..a0f520ab 100644 --- a/doc/architecture/uml/LogicalView/SensorFusion/HAL_IMU.puml +++ b/doc/architecture/uml/LogicalView/SensorFusion/HAL_IMU.puml @@ -1,119 +1,119 @@ -@startuml - -title RadonUlzer - Hardware Abstraction Layer - -package "HAL" as hal { - - package "HAL Interfaces" as halInterfaces { - interface "IIMU" as iIMU { - + {abstract} init() : bool - + {abstract} readAccelerometer() : void - + {abstract} readGyro() : void - + {abstract} readMagnetometer() : void - + {abstract} accelerometerDataReady() : bool - + {abstract} gyroDataReady() : bool - + {abstract} magnetometerDataReady() : bool - + {abstract} getAccelerationValues(IMUData* accelerationValues) : void - + {abstract} getTurnRates(IMUData* turnRates) : void - + {abstract} getMagnetometerValues(IMUData* magnetometerValues) : void - - } - struct IMUData { - + valueX: int16_t - + valueY: int16_t - + valueZ: int16_t - } - } - - package "HAL Simulation" as simulation { - class "IMU" as ImuSim { - - m_accelerationValues : IMUData - - m_gyroValues : IMUData - - m_magnetometerValues : IMUData - } - class "Board" as BoardSim { - - m_imu : IMU - + getIMU() : IMU& - + init() : void - } - } - - package "HAL Target" as target { - class "IMU" as ImuTarget { - - m_accelerationValues : IMUData - - m_gyroValues : IMUData - - m_magnetometerValues : IMUData - } - class "Board" as BoardTarget{ - - m_imu : IMU - + getIMU() : IMU& - + init() : void - } - } - - iIMU ..> IMUData: <> - iIMU <|... ImuSim: <> - ImuTarget *- BoardTarget - ImuSim *- BoardSim - - iIMU <|... ImuTarget: <> -} -package "Webots library" as webotsLib { - class Accelerometer { - + getValues() : double * - + enable(int samplingPeriod) : void - } - class Gyro { - + getValues() : double * - + enable(int samplingPeriod) : void - } - class Compass { - + getValues() : double * - + enable(int samplingPeriod) : void - } -} - -package "Zumo32U4 library" as zumo32u4Lib { - - class Zumo32U4IMU { - - a[3] : vector - - g[3] : vector - - m[3] : vector - + init() : bool - + readAcc() : void - + readGyro() : void - + readMag() : void - + accDataReady() : bool - + gyroDataReady() : bool - + magDataReady() : bool - } -} - -note bottom of zumo32u4Lib - Provided by Pololu. - https://pololu.github.io/zumo-32u4-arduino-library/index.html -end note - -note bottom of webotsLib - Provided by Cyberbotics. - https://cyberbotics.com/doc/reference/thanks?version=R2023b -end note - -ImuTarget *--> Zumo32U4IMU - -ImuSim *--> Accelerometer -ImuSim *--> Gyro -ImuSim *--> Compass - -note right of iIMU - IMU stands for Inertial Measurement Unit. -end note - -note left of hal - This diagram shows the added IMU component. - Classes like the LineSensors or Encoders - are missing. -end note - - +@startuml + +title RadonUlzer - Hardware Abstraction Layer + +package "HAL" as hal { + + package "HAL Interfaces" as halInterfaces { + interface "IIMU" as iIMU { + + {abstract} init() : bool + + {abstract} readAccelerometer() : void + + {abstract} readGyro() : void + + {abstract} readMagnetometer() : void + + {abstract} accelerometerDataReady() : bool + + {abstract} gyroDataReady() : bool + + {abstract} magnetometerDataReady() : bool + + {abstract} getAccelerationValues(IMUData* accelerationValues) : void + + {abstract} getTurnRates(IMUData* turnRates) : void + + {abstract} getMagnetometerValues(IMUData* magnetometerValues) : void + + } + struct IMUData { + + valueX: int16_t + + valueY: int16_t + + valueZ: int16_t + } + } + + package "HAL Simulation" as simulation { + class "IMU" as ImuSim { + - m_accelerationValues : IMUData + - m_gyroValues : IMUData + - m_magnetometerValues : IMUData + } + class "Board" as BoardSim { + - m_imu : IMU + + getIMU() : IMU& + + init() : void + } + } + + package "HAL Target" as target { + class "IMU" as ImuTarget { + - m_accelerationValues : IMUData + - m_gyroValues : IMUData + - m_magnetometerValues : IMUData + } + class "Board" as BoardTarget{ + - m_imu : IMU + + getIMU() : IMU& + + init() : void + } + } + + iIMU ..> IMUData: <> + iIMU <|... ImuSim: <> + ImuTarget *- BoardTarget + ImuSim *- BoardSim + + iIMU <|... ImuTarget: <> +} +package "Webots library" as webotsLib { + class Accelerometer { + + getValues() : double * + + enable(int samplingPeriod) : void + } + class Gyro { + + getValues() : double * + + enable(int samplingPeriod) : void + } + class Compass { + + getValues() : double * + + enable(int samplingPeriod) : void + } +} + +package "Zumo32U4 library" as zumo32u4Lib { + + class Zumo32U4IMU { + - a[3] : vector + - g[3] : vector + - m[3] : vector + + init() : bool + + readAcc() : void + + readGyro() : void + + readMag() : void + + accDataReady() : bool + + gyroDataReady() : bool + + magDataReady() : bool + } +} + +note bottom of zumo32u4Lib + Provided by Pololu. + https://pololu.github.io/zumo-32u4-arduino-library/index.html +end note + +note bottom of webotsLib + Provided by Cyberbotics. + https://cyberbotics.com/doc/reference/thanks?version=R2023b +end note + +ImuTarget *--> Zumo32U4IMU + +ImuSim *--> Accelerometer +ImuSim *--> Gyro +ImuSim *--> Compass + +note right of iIMU + IMU stands for Inertial Measurement Unit. +end note + +note left of hal + This diagram shows the added IMU component. + Classes like the LineSensors or Encoders + are missing. +end note + + @enduml \ No newline at end of file diff --git a/doc/architecture/uml/LogicalView/SensorFusion/HAL_SensorFusion.puml b/doc/architecture/uml/LogicalView/SensorFusion/HAL_SensorFusion.puml index b1e42583..39677c7d 100644 --- a/doc/architecture/uml/LogicalView/SensorFusion/HAL_SensorFusion.puml +++ b/doc/architecture/uml/LogicalView/SensorFusion/HAL_SensorFusion.puml @@ -1,136 +1,136 @@ -@startuml HAL SensorFusion - -title Hardware Abstraction Layer - SensorFusion App - -package "HAL" as hal { - - package "Interfaces" as halInterfaces { - - interface "IButton" as iButton { - + {abstract} isPressed() : bool - + {abstract} waitForRelease() : void - } - - interface "IEncoders" as iEncoders { - + {abstract} getCountsLeft() : int16_t - + {abstract} getCountsRight() : int16_t - + {abstract} getCountsAndResetLeft() : int16_t - + {abstract} getCountsAndResetRight() : int16_t - + {abstract} getResolution() const : uint16_t - } - - interface "ILed" as iLed { - + {abstract} enable(enableIt : bool) : void - } - - interface "ILineSensors" as 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 "IMotors" as iMotors { - + {abstract} setSpeeds(leftSpeed : int16_t, rightSpeed : int16_t) : void - + {abstract} getMaxSpeed() const : int16_t - } - - interface "IIMU" as iIMU { - + {abstract} init() : bool - + {abstract} readAccelerometer() : void - + {abstract} readGyro() : void - + {abstract} readMagnetometer() : void - + {abstract} accelerometerDataReady() : bool - + {abstract} gyroDataReady() : bool - + {abstract} magnetometerDataReady() : bool - + {abstract} getAccelerationValues(IMUData* accelerationValues) : void - + {abstract} getTurnRates(IMUData* turnRates) : void - + {abstract} getMagnetometerValues(IMUData* magnetometerValues) : void - - } - } - - class Board << namespace >> { - + getButtonA() : IButton& - + getEncoders() : IEncoders& - + getLedYellow() : ILed& - + getLineSensors() : ILineSensors& - + getMotors() : IMotors& - + getIMU() : IIMU& - } - - class RobotConstants { - + GEAR_RATIO : uint32_t - + ENCODER_RESOLUTION : uint16_t - + WHEEL_DIAMETER : uint32_t - + WHEEL_CIRCUMFERENCE : uint32_t - + ENCODER_STEPS_PER_MM : uint32_t - } - - note top of Board - Defines the physical board with all actor - and sensor instances. The application retrieves - every driver via the board. - end note - - note top of RobotConstants - Provides robot specific constants, e.g. given by - mechanic parts. - end note - - note top of iIMU - IMU stands for Inertial Measurement Unit. - end note - - package "Target" as target { - class LineSensors - class ButtonA - class Motors - class Encoders - class LedYellow - class IMU - } - - iLineSensors <|... LineSensors: <> - iButton <|... ButtonA: <> - iMotors <|... Motors: <> - iEncoders <|... Encoders: <> - iLed <|... LedYellow: <> - iIMU <|... IMU: <> -} - -package "Zumo32U4 library" as zumo32u4Lib { - class Zumo32U4LineSensors - class Zumo32U4ButtonA - class Zumo32U4Motors - class Zumo32U4LCD - class Zumo32U4Encoders - class Zumo32U4IMU - class Zumo32U4 <> -} - -note bottom of zumo32u4Lib - Provided by Pololu. - https://pololu.github.io/zumo-32u4-arduino-library/index.html -end note - -halInterfaces -[hidden]-- hal -hal -[hidden]-- zumo32u4Lib - -LineSensors *--> Zumo32U4LineSensors -ButtonA *--> Zumo32U4ButtonA -Motors *--> Zumo32U4Motors -Encoders *--> Zumo32U4Encoders -IMU *--> Zumo32U4IMU -LedYellow ..> Zumo32U4: <> - -note bottom of hal - The hardware abstraction layer contains - all drivers, provided by the Pololu Library. -end note - +@startuml HAL SensorFusion + +title Hardware Abstraction Layer - SensorFusion App + +package "HAL" as hal { + + package "Interfaces" as halInterfaces { + + interface "IButton" as iButton { + + {abstract} isPressed() : bool + + {abstract} waitForRelease() : void + } + + interface "IEncoders" as iEncoders { + + {abstract} getCountsLeft() : int16_t + + {abstract} getCountsRight() : int16_t + + {abstract} getCountsAndResetLeft() : int16_t + + {abstract} getCountsAndResetRight() : int16_t + + {abstract} getResolution() const : uint16_t + } + + interface "ILed" as iLed { + + {abstract} enable(enableIt : bool) : void + } + + interface "ILineSensors" as 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 "IMotors" as iMotors { + + {abstract} setSpeeds(leftSpeed : int16_t, rightSpeed : int16_t) : void + + {abstract} getMaxSpeed() const : int16_t + } + + interface "IIMU" as iIMU { + + {abstract} init() : bool + + {abstract} readAccelerometer() : void + + {abstract} readGyro() : void + + {abstract} readMagnetometer() : void + + {abstract} accelerometerDataReady() : bool + + {abstract} gyroDataReady() : bool + + {abstract} magnetometerDataReady() : bool + + {abstract} getAccelerationValues(IMUData* accelerationValues) : void + + {abstract} getTurnRates(IMUData* turnRates) : void + + {abstract} getMagnetometerValues(IMUData* magnetometerValues) : void + + } + } + + class Board << namespace >> { + + getButtonA() : IButton& + + getEncoders() : IEncoders& + + getLedYellow() : ILed& + + getLineSensors() : ILineSensors& + + getMotors() : IMotors& + + getIMU() : IIMU& + } + + class RobotConstants { + + GEAR_RATIO : uint32_t + + ENCODER_RESOLUTION : uint16_t + + WHEEL_DIAMETER : uint32_t + + WHEEL_CIRCUMFERENCE : uint32_t + + ENCODER_STEPS_PER_MM : uint32_t + } + + note top of Board + Defines the physical board with all actor + and sensor instances. The application retrieves + every driver via the board. + end note + + note top of RobotConstants + Provides robot specific constants, e.g. given by + mechanic parts. + end note + + note top of iIMU + IMU stands for Inertial Measurement Unit. + end note + + package "Target" as target { + class LineSensors + class ButtonA + class Motors + class Encoders + class LedYellow + class IMU + } + + iLineSensors <|... LineSensors: <> + iButton <|... ButtonA: <> + iMotors <|... Motors: <> + iEncoders <|... Encoders: <> + iLed <|... LedYellow: <> + iIMU <|... IMU: <> +} + +package "Zumo32U4 library" as zumo32u4Lib { + class Zumo32U4LineSensors + class Zumo32U4ButtonA + class Zumo32U4Motors + class Zumo32U4LCD + class Zumo32U4Encoders + class Zumo32U4IMU + class Zumo32U4 <> +} + +note bottom of zumo32u4Lib + Provided by Pololu. + https://pololu.github.io/zumo-32u4-arduino-library/index.html +end note + +halInterfaces -[hidden]-- hal +hal -[hidden]-- zumo32u4Lib + +LineSensors *--> Zumo32U4LineSensors +ButtonA *--> Zumo32U4ButtonA +Motors *--> Zumo32U4Motors +Encoders *--> Zumo32U4Encoders +IMU *--> Zumo32U4IMU +LedYellow ..> Zumo32U4: <> + +note bottom of hal + The hardware abstraction layer contains + all drivers, provided by the Pololu Library. +end note + @enduml \ No newline at end of file diff --git a/lib/APPConvoyLeader/App.cpp b/lib/APPConvoyLeader/App.cpp index 899490ca..7725b484 100644 --- a/lib/APPConvoyLeader/App.cpp +++ b/lib/APPConvoyLeader/App.cpp @@ -1,182 +1,182 @@ -/* MIT License - * - * Copyright (c) 2023 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 ConvoyLeader application - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "App.h" -#include "StartupState.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); - Logging::disable(); - Board::getInstance().init(); - m_systemStateMachine.setState(&StartupState::getInstance()); - m_controlInterval.start(DIFFERENTIAL_DRIVE_CONTROL_PERIOD); - - /* Setup SerialMuxProt Channels. */ - m_serialMuxProtChannelIdOdometry = m_smpServer.createChannel(ODOMETRY_CHANNEL_NAME, ODOMETRY_CHANNEL_DLC); - m_serialMuxProtChannelIdSpeed = m_smpServer.createChannel(SPEED_CHANNEL_NAME, SPEED_CHANNEL_DLC); - m_smpServer.subscribeToChannel(SPEED_SETPOINT_CHANNEL_NAME, App_motorSpeedSetpointsChannelCallback); - - /* Channel sucesfully created? */ - if ((0U != m_serialMuxProtChannelIdOdometry) && (0U != m_serialMuxProtChannelIdSpeed)) - { - m_reportTimer.start(REPORTING_PERIOD); - } -} - -void App::loop() -{ - m_smpServer.process(millis()); - 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(); - - m_controlInterval.restart(); - } - - if (true == m_reportTimer.isTimeout()) - { - /* Send current data to SerialMuxProt Client */ - reportOdometry(); - reportSpeed(); - - m_reportTimer.restart(); - } - - m_systemStateMachine.process(); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -void App::reportOdometry() -{ - Odometry& odometry = Odometry::getInstance(); - OdometryData payload; - int32_t xPos = 0; - int32_t yPos = 0; - - odometry.getPosition(xPos, yPos); - payload.xPos = xPos; - payload.yPos = yPos; - payload.orientation = odometry.getOrientation(); - - /* Ignoring return value, as error handling is not available. */ - (void)m_smpServer.sendData(m_serialMuxProtChannelIdOdometry, reinterpret_cast(&payload), sizeof(payload)); -} - -void App::reportSpeed() -{ - Speedometer& speedometer = Speedometer::getInstance(); - SpeedData payload; - payload.left = speedometer.getLinearSpeedLeft(); - payload.right = speedometer.getLinearSpeedRight(); - - /* Ignoring return value, as error handling is not available. */ - (void)m_smpServer.sendData(m_serialMuxProtChannelIdSpeed, reinterpret_cast(&payload), sizeof(payload)); -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ - -/** - * 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); - DifferentialDrive::getInstance().setLinearSpeed(motorSpeedData->left, motorSpeedData->right); - } +/* MIT License + * + * Copyright (c) 2023 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 ConvoyLeader application + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "App.h" +#include "StartupState.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); + Logging::disable(); + Board::getInstance().init(); + m_systemStateMachine.setState(&StartupState::getInstance()); + m_controlInterval.start(DIFFERENTIAL_DRIVE_CONTROL_PERIOD); + + /* Setup SerialMuxProt Channels. */ + m_serialMuxProtChannelIdOdometry = m_smpServer.createChannel(ODOMETRY_CHANNEL_NAME, ODOMETRY_CHANNEL_DLC); + m_serialMuxProtChannelIdSpeed = m_smpServer.createChannel(SPEED_CHANNEL_NAME, SPEED_CHANNEL_DLC); + m_smpServer.subscribeToChannel(SPEED_SETPOINT_CHANNEL_NAME, App_motorSpeedSetpointsChannelCallback); + + /* Channel sucesfully created? */ + if ((0U != m_serialMuxProtChannelIdOdometry) && (0U != m_serialMuxProtChannelIdSpeed)) + { + m_reportTimer.start(REPORTING_PERIOD); + } +} + +void App::loop() +{ + m_smpServer.process(millis()); + 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(); + + m_controlInterval.restart(); + } + + if (true == m_reportTimer.isTimeout()) + { + /* Send current data to SerialMuxProt Client */ + reportOdometry(); + reportSpeed(); + + m_reportTimer.restart(); + } + + m_systemStateMachine.process(); +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +void App::reportOdometry() +{ + Odometry& odometry = Odometry::getInstance(); + OdometryData payload; + int32_t xPos = 0; + int32_t yPos = 0; + + odometry.getPosition(xPos, yPos); + payload.xPos = xPos; + payload.yPos = yPos; + payload.orientation = odometry.getOrientation(); + + /* Ignoring return value, as error handling is not available. */ + (void)m_smpServer.sendData(m_serialMuxProtChannelIdOdometry, reinterpret_cast(&payload), sizeof(payload)); +} + +void App::reportSpeed() +{ + Speedometer& speedometer = Speedometer::getInstance(); + SpeedData payload; + payload.left = speedometer.getLinearSpeedLeft(); + payload.right = speedometer.getLinearSpeedRight(); + + /* Ignoring return value, as error handling is not available. */ + (void)m_smpServer.sendData(m_serialMuxProtChannelIdSpeed, reinterpret_cast(&payload), sizeof(payload)); +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ + +/** + * 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); + DifferentialDrive::getInstance().setLinearSpeed(motorSpeedData->left, motorSpeedData->right); + } } \ No newline at end of file diff --git a/lib/APPConvoyLeader/App.h b/lib/APPConvoyLeader/App.h index 2dd8e7c4..6c63928d 100644 --- a/lib/APPConvoyLeader/App.h +++ b/lib/APPConvoyLeader/App.h @@ -1,148 +1,148 @@ -/* MIT License - * - * Copyright (c) 2023 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 ConvoyLeader application - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef APP_H -#define APP_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include -#include "SerialMuxChannels.h" -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The convoy leader application. */ -class App -{ -public: - /** - * Construct the convoy leader application. - */ - App() : - m_serialMuxProtChannelIdOdometry(0U), - m_serialMuxProtChannelIdSpeed(0U), - m_systemStateMachine(), - m_controlInterval(), - m_reportTimer(), - m_smpServer(Serial) - { - } - - /** - * Destroy the convoy leader 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; - - /** Current data reporting period in ms. */ - static const uint32_t REPORTING_PERIOD = 50U; - - /** Baudrate for Serial Communication */ - static const uint32_t SERIAL_BAUDRATE = 115200U; - - /** SerialMuxProt Channel id for sending the current odometry. */ - uint8_t m_serialMuxProtChannelIdOdometry; - - /** SerialMuxProt Channel id for sending the current speed. */ - uint8_t m_serialMuxProtChannelIdSpeed; - - /** The system state machine. */ - StateMachine m_systemStateMachine; - - /** Timer used for differential drive control processing. */ - SimpleTimer m_controlInterval; - - /** Timer for reporting current data through SerialMuxProt. */ - SimpleTimer m_reportTimer; - - /** - * SerialMuxProt Server Instance - * - * @tparam tMaxChannels set to MAX_CHANNELS, defined in SerialMuxChannels.h. - */ - SerialMuxProtServer m_smpServer; - - /** - * Report the current position and heading of the robot using the Odometry data. - * Sends data through the SerialMuxProtServer. - */ - void reportOdometry(); - - /** - * Report the current motor speeds of the robot using the Speedometer data. - * Sends data through the SerialMuxProtServer. - */ - void reportSpeed(); - - /* Not allowed. */ - App(const App& app); /**< Copy construction of an instance. */ - App& operator=(const App& app); /**< Assignment of an instance. */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* APP_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 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 ConvoyLeader application + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef APP_H +#define APP_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include +#include "SerialMuxChannels.h" +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The convoy leader application. */ +class App +{ +public: + /** + * Construct the convoy leader application. + */ + App() : + m_serialMuxProtChannelIdOdometry(0U), + m_serialMuxProtChannelIdSpeed(0U), + m_systemStateMachine(), + m_controlInterval(), + m_reportTimer(), + m_smpServer(Serial) + { + } + + /** + * Destroy the convoy leader 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; + + /** Current data reporting period in ms. */ + static const uint32_t REPORTING_PERIOD = 50U; + + /** Baudrate for Serial Communication */ + static const uint32_t SERIAL_BAUDRATE = 115200U; + + /** SerialMuxProt Channel id for sending the current odometry. */ + uint8_t m_serialMuxProtChannelIdOdometry; + + /** SerialMuxProt Channel id for sending the current speed. */ + uint8_t m_serialMuxProtChannelIdSpeed; + + /** The system state machine. */ + StateMachine m_systemStateMachine; + + /** Timer used for differential drive control processing. */ + SimpleTimer m_controlInterval; + + /** Timer for reporting current data through SerialMuxProt. */ + SimpleTimer m_reportTimer; + + /** + * SerialMuxProt Server Instance + * + * @tparam tMaxChannels set to MAX_CHANNELS, defined in SerialMuxChannels.h. + */ + SerialMuxProtServer m_smpServer; + + /** + * Report the current position and heading of the robot using the Odometry data. + * Sends data through the SerialMuxProtServer. + */ + void reportOdometry(); + + /** + * Report the current motor speeds of the robot using the Speedometer data. + * Sends data through the SerialMuxProtServer. + */ + void reportSpeed(); + + /* 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/APPConvoyLeader/DrivingState.cpp b/lib/APPConvoyLeader/DrivingState.cpp index e000bb2b..5234d0d5 100644 --- a/lib/APPConvoyLeader/DrivingState.cpp +++ b/lib/APPConvoyLeader/DrivingState.cpp @@ -1,371 +1,371 @@ -/* MIT License - * - * Copyright (c) 2023 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" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void DrivingState::entry() -{ - 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_FIND_START_LINE; - m_trackStatus = TRACK_STATUS_ON_TRACK; /* Assume that the robot is placed on track. */ - m_posMovAvg.clear(); - - /* 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(true); -} - -void DrivingState::process(StateMachine& sm) -{ - ILineSensors& lineSensors = Board::getInstance().getLineSensors(); - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - int16_t position = 0; - - /* Get the position of the line. */ - position = lineSensors.readLine(); - - (void)m_posMovAvg.write(position); - - switch (m_trackStatus) - { - case TRACK_STATUS_ON_TRACK: - processOnTrack(position, lineSensors.getSensorValues()); - break; - - case TRACK_STATUS_LOST: - processTrackLost(position, lineSensors.getSensorValues()); - break; - - case TRACK_STATUS_FINISHED: - /* Change to ready state. */ - sm.setState(&ReadyState::getInstance()); - break; - - default: - /* Fatal error */ - diffDrive.setLinearSpeed(0, 0); - Sound::playAlarm(); - sm.setState(&ReadyState::getInstance()); - break; - } - - /* Max. time for finishing the track over? */ - if ((TRACK_STATUS_FINISHED != m_trackStatus) && (true == m_observationTimer.isTimeout())) - { - m_trackStatus = TRACK_STATUS_FINISHED; - - /* Stop motors immediately. Don't move this to a later position, - * as this would extend the driven length. - */ - diffDrive.setLinearSpeed(0, 0); - - Sound::playAlarm(); - } -} - -void DrivingState::exit() -{ - m_observationTimer.stop(); - Board::getInstance().getYellowLed().enable(false); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -void DrivingState::processOnTrack(int16_t position, const uint16_t* lineSensorValues) -{ - if (nullptr == lineSensorValues) - { - return; - } - - /* Track lost just in this moment? */ - if (true == isTrackGapDetected(m_posMovAvg.getResult())) - { - m_trackStatus = TRACK_STATUS_LOST; - - /* Set mileage to 0, to be able to measure the max. distance, till - * the track must be found again. - */ - Odometry::getInstance().clearMileage(); - - /* Show the operator that the track is lost visual. */ - Board::getInstance().getYellowLed().enable(true); - } - else - { - /* Detect start-/endline */ - if (true == isStartEndLineDetected(lineSensorValues)) - { - /* Start line detected? */ - if (LINE_STATUS_FIND_START_LINE == m_lineStatus) - { - m_lineStatus = LINE_STATUS_START_LINE_DETECTED; - - Sound::playBeep(); - - /* Measure the lap time and use as start point the detected start line. */ - m_lapTime.start(0); - } - /* End line detected */ - else if (LINE_STATUS_FIND_END_LINE == m_lineStatus) - { - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - - /* Stop motors immediately. Don't move this to a later position, - * as this would extend the driven length. - */ - diffDrive.setLinearSpeed(0, 0); - - Sound::playBeep(); - m_trackStatus = TRACK_STATUS_FINISHED; - - /* Calculate lap time and show it*/ - ReadyState::getInstance().setLapTime(m_lapTime.getCurrentDuration()); - } - else - { - ; - } - } - else if (LINE_STATUS_START_LINE_DETECTED == m_lineStatus) - { - m_lineStatus = LINE_STATUS_FIND_END_LINE; - } - else - { - ; - } - - if (TRACK_STATUS_FINISHED != m_trackStatus) - { - if (true == m_pidProcessTime.isTimeout()) - { - adaptDriving(position); - - m_pidProcessTime.start(PID_PROCESS_PERIOD); - } - } - } -} - -void DrivingState::processTrackLost(int16_t position, const uint16_t* lineSensorValues) -{ - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - - if (nullptr == lineSensorValues) - { - return; - } - - /* Back on track? */ - if (false == isTrackGapDetected(position)) - { - m_trackStatus = TRACK_STATUS_ON_TRACK; - m_pidCtrl.resync(); - - Board::getInstance().getYellowLed().enable(false); - } - /* Max. distance driven, but track still not found? */ - else if (MAX_DISTANCE < Odometry::getInstance().getMileageCenter()) - { - /* Stop motors immediately. Don't move this to a later position, - * as this would extend the driven length. - */ - diffDrive.setLinearSpeed(0, 0); - - Sound::playAlarm(); - m_trackStatus = TRACK_STATUS_FINISHED; - } - else - { - /* Drive straight on. */ - diffDrive.setLinearSpeed(m_topSpeed, m_topSpeed); - } -} - -bool DrivingState::isStartEndLineDetected(const uint16_t* lineSensorValues) -{ - bool isDetected = false; - uint16_t leftSensor = lineSensorValues[0]; - uint16_t middleSensor = (lineSensorValues[1] + lineSensorValues[2] + lineSensorValues[3]) / 3; - uint16_t rightSensor = lineSensorValues[4]; - const uint8_t DEBOUNCE_CNT = 3; - const uint16_t LINE_SENSOR_OFF_TRACK_VALUE = 200; - - /* Note, the start-/end line detection must be debounced. Otherwise - * especially in low speed use cases, the line may be in one cycle - * detected, in the next not and then detected again. This would lead - * to a start line detection and afterwards to a end line detection, - * which would be wrong. - * - * Note the three sensors in the middle are handled as one sensor to - * avoid detection problems with different kind of line widths. - */ - if ((LINE_SENSOR_OFF_TRACK_VALUE <= leftSensor) && (LINE_SENSOR_OFF_TRACK_VALUE <= middleSensor) && - (LINE_SENSOR_OFF_TRACK_VALUE <= rightSensor)) - { - if (DEBOUNCE_CNT > m_startEndLineDebounce) - { - ++m_startEndLineDebounce; - } - - if (DEBOUNCE_CNT <= m_startEndLineDebounce) - { - isDetected = true; - } - } - else - { - m_startEndLineDebounce = 0; - } - - return isDetected; -} - -bool DrivingState::isTrackGapDetected(int16_t position) const -{ - bool isDetected = false; - const ILineSensors& lineSensors = Board::getInstance().getLineSensors(); - - /* Position value after loosing the track and sensor 0 saw it as last. - * It depends on the Zumo32U4LineSensors::readLine() implementation. - */ - const int16_t POS_MIN = 0; - - /* Position value after loosing the track and sensor N saw it as last. - * It depends on the Zumo32U4LineSensors::readLine() implementation. - */ - const int16_t POS_MAX = (lineSensors.getNumLineSensors() - 1) * 1000; - - /* Note, no debouncing is done here. If necessary, it shall be done - * outside this method. - */ - if ((POS_MIN >= position) || (POS_MAX <= position)) - { - isDetected = true; - } - - return isDetected; -} - -void DrivingState::adaptDriving(int16_t position) -{ - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - const ILineSensors& lineSensors = Board::getInstance().getLineSensors(); - 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(lineSensors.getSensorValueMax() * 2, 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, 0, m_topSpeed); - rightSpeed = constrain(rightSpeed, 0, m_topSpeed); - - diffDrive.setLinearSpeed(leftSpeed, rightSpeed); -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 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" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void DrivingState::entry() +{ + 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_FIND_START_LINE; + m_trackStatus = TRACK_STATUS_ON_TRACK; /* Assume that the robot is placed on track. */ + m_posMovAvg.clear(); + + /* 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(true); +} + +void DrivingState::process(StateMachine& sm) +{ + ILineSensors& lineSensors = Board::getInstance().getLineSensors(); + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + int16_t position = 0; + + /* Get the position of the line. */ + position = lineSensors.readLine(); + + (void)m_posMovAvg.write(position); + + switch (m_trackStatus) + { + case TRACK_STATUS_ON_TRACK: + processOnTrack(position, lineSensors.getSensorValues()); + break; + + case TRACK_STATUS_LOST: + processTrackLost(position, lineSensors.getSensorValues()); + break; + + case TRACK_STATUS_FINISHED: + /* Change to ready state. */ + sm.setState(&ReadyState::getInstance()); + break; + + default: + /* Fatal error */ + diffDrive.setLinearSpeed(0, 0); + Sound::playAlarm(); + sm.setState(&ReadyState::getInstance()); + break; + } + + /* Max. time for finishing the track over? */ + if ((TRACK_STATUS_FINISHED != m_trackStatus) && (true == m_observationTimer.isTimeout())) + { + m_trackStatus = TRACK_STATUS_FINISHED; + + /* Stop motors immediately. Don't move this to a later position, + * as this would extend the driven length. + */ + diffDrive.setLinearSpeed(0, 0); + + Sound::playAlarm(); + } +} + +void DrivingState::exit() +{ + m_observationTimer.stop(); + Board::getInstance().getYellowLed().enable(false); +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +void DrivingState::processOnTrack(int16_t position, const uint16_t* lineSensorValues) +{ + if (nullptr == lineSensorValues) + { + return; + } + + /* Track lost just in this moment? */ + if (true == isTrackGapDetected(m_posMovAvg.getResult())) + { + m_trackStatus = TRACK_STATUS_LOST; + + /* Set mileage to 0, to be able to measure the max. distance, till + * the track must be found again. + */ + Odometry::getInstance().clearMileage(); + + /* Show the operator that the track is lost visual. */ + Board::getInstance().getYellowLed().enable(true); + } + else + { + /* Detect start-/endline */ + if (true == isStartEndLineDetected(lineSensorValues)) + { + /* Start line detected? */ + if (LINE_STATUS_FIND_START_LINE == m_lineStatus) + { + m_lineStatus = LINE_STATUS_START_LINE_DETECTED; + + Sound::playBeep(); + + /* Measure the lap time and use as start point the detected start line. */ + m_lapTime.start(0); + } + /* End line detected */ + else if (LINE_STATUS_FIND_END_LINE == m_lineStatus) + { + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + + /* Stop motors immediately. Don't move this to a later position, + * as this would extend the driven length. + */ + diffDrive.setLinearSpeed(0, 0); + + Sound::playBeep(); + m_trackStatus = TRACK_STATUS_FINISHED; + + /* Calculate lap time and show it*/ + ReadyState::getInstance().setLapTime(m_lapTime.getCurrentDuration()); + } + else + { + ; + } + } + else if (LINE_STATUS_START_LINE_DETECTED == m_lineStatus) + { + m_lineStatus = LINE_STATUS_FIND_END_LINE; + } + else + { + ; + } + + if (TRACK_STATUS_FINISHED != m_trackStatus) + { + if (true == m_pidProcessTime.isTimeout()) + { + adaptDriving(position); + + m_pidProcessTime.start(PID_PROCESS_PERIOD); + } + } + } +} + +void DrivingState::processTrackLost(int16_t position, const uint16_t* lineSensorValues) +{ + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + + if (nullptr == lineSensorValues) + { + return; + } + + /* Back on track? */ + if (false == isTrackGapDetected(position)) + { + m_trackStatus = TRACK_STATUS_ON_TRACK; + m_pidCtrl.resync(); + + Board::getInstance().getYellowLed().enable(false); + } + /* Max. distance driven, but track still not found? */ + else if (MAX_DISTANCE < Odometry::getInstance().getMileageCenter()) + { + /* Stop motors immediately. Don't move this to a later position, + * as this would extend the driven length. + */ + diffDrive.setLinearSpeed(0, 0); + + Sound::playAlarm(); + m_trackStatus = TRACK_STATUS_FINISHED; + } + else + { + /* Drive straight on. */ + diffDrive.setLinearSpeed(m_topSpeed, m_topSpeed); + } +} + +bool DrivingState::isStartEndLineDetected(const uint16_t* lineSensorValues) +{ + bool isDetected = false; + uint16_t leftSensor = lineSensorValues[0]; + uint16_t middleSensor = (lineSensorValues[1] + lineSensorValues[2] + lineSensorValues[3]) / 3; + uint16_t rightSensor = lineSensorValues[4]; + const uint8_t DEBOUNCE_CNT = 3; + const uint16_t LINE_SENSOR_OFF_TRACK_VALUE = 200; + + /* Note, the start-/end line detection must be debounced. Otherwise + * especially in low speed use cases, the line may be in one cycle + * detected, in the next not and then detected again. This would lead + * to a start line detection and afterwards to a end line detection, + * which would be wrong. + * + * Note the three sensors in the middle are handled as one sensor to + * avoid detection problems with different kind of line widths. + */ + if ((LINE_SENSOR_OFF_TRACK_VALUE <= leftSensor) && (LINE_SENSOR_OFF_TRACK_VALUE <= middleSensor) && + (LINE_SENSOR_OFF_TRACK_VALUE <= rightSensor)) + { + if (DEBOUNCE_CNT > m_startEndLineDebounce) + { + ++m_startEndLineDebounce; + } + + if (DEBOUNCE_CNT <= m_startEndLineDebounce) + { + isDetected = true; + } + } + else + { + m_startEndLineDebounce = 0; + } + + return isDetected; +} + +bool DrivingState::isTrackGapDetected(int16_t position) const +{ + bool isDetected = false; + const ILineSensors& lineSensors = Board::getInstance().getLineSensors(); + + /* Position value after loosing the track and sensor 0 saw it as last. + * It depends on the Zumo32U4LineSensors::readLine() implementation. + */ + const int16_t POS_MIN = 0; + + /* Position value after loosing the track and sensor N saw it as last. + * It depends on the Zumo32U4LineSensors::readLine() implementation. + */ + const int16_t POS_MAX = (lineSensors.getNumLineSensors() - 1) * 1000; + + /* Note, no debouncing is done here. If necessary, it shall be done + * outside this method. + */ + if ((POS_MIN >= position) || (POS_MAX <= position)) + { + isDetected = true; + } + + return isDetected; +} + +void DrivingState::adaptDriving(int16_t position) +{ + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + const ILineSensors& lineSensors = Board::getInstance().getLineSensors(); + 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(lineSensors.getSensorValueMax() * 2, 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, 0, m_topSpeed); + rightSpeed = constrain(rightSpeed, 0, m_topSpeed); + + diffDrive.setLinearSpeed(leftSpeed, rightSpeed); +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPConvoyLeader/DrivingState.h b/lib/APPConvoyLeader/DrivingState.h index 0a8d4fbe..45433d88 100644 --- a/lib/APPConvoyLeader/DrivingState.h +++ b/lib/APPConvoyLeader/DrivingState.h @@ -1,214 +1,214 @@ -/* MIT License - * - * Copyright (c) 2023 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 -#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_FIND_START_LINE = 0, /**< Find the start line. */ - LINE_STATUS_START_LINE_DETECTED, /**< Start line detected. */ - LINE_STATUS_FIND_END_LINE /**< Find the end line. */ - }; - - /** - * The track status. - */ - enum TrackStatus - { - TRACK_STATUS_ON_TRACK = 0, /**< Robot is on track */ - TRACK_STATUS_LOST, /**< Robot lost track */ - 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; - - 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. */ - uint8_t m_startEndLineDebounce; /**< Counter used for easys debouncing of the start-/end line detection. */ - MovAvg m_posMovAvg; /**< The moving average of the position over 2 calling cycles. */ - - /** - * Default constructor. - */ - DrivingState() : - m_observationTimer(), - m_lapTime(), - m_pidProcessTime(), - m_pidCtrl(), - m_topSpeed(0), - m_lineStatus(LINE_STATUS_FIND_START_LINE), - m_trackStatus(TRACK_STATUS_ON_TRACK), - m_startEndLineDebounce(0), - m_posMovAvg() - { - } - - /** - * Default destructor. - */ - ~DrivingState() - { - } - - /* Not allowed. */ - DrivingState(const DrivingState& state); /**< Copy construction of an instance. */ - DrivingState& operator=(const DrivingState& state); /**< Assignment of an instance. */ - - /** - * Control driving in case the robot is on track. - * - * @param[in] position Current position on track - * @param[in] lineSensorValues Value of each line sensor - */ - void processOnTrack(int16_t position, const uint16_t* lineSensorValues); - - /** - * Control driving in case the robot lost the track. - * It handles the track search algorithm. - * - * @param[in] position Current position on track - * @param[in] lineSensorValues Value of each line sensor - */ - void processTrackLost(int16_t position, const uint16_t* lineSensorValues); - - /** - * Is start-/endline detected? - * - * @param[in] lineSensorValues Line sensor values - * - * @return If a start-/endline is detected, it will return true otherwise false. - */ - bool isStartEndLineDetected(const uint16_t* lineSensorValues); - - /** - * Is a track gap detected? - * Note, it contains no debouncing inside. If necessary, it shall be - * done outside. - * - * @param[in] position Determined position in digits - * - * @return If a track gap is detected, it will return true otherwise false. - */ - bool isTrackGapDetected(int16_t position) const; - - /** - * Adapt driving by using a PID algorithm, depended on the position - * input. - * - * @param[in] position Position in digits - */ - void adaptDriving(int16_t position); -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* DRIVING_STATE_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 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 +#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_FIND_START_LINE = 0, /**< Find the start line. */ + LINE_STATUS_START_LINE_DETECTED, /**< Start line detected. */ + LINE_STATUS_FIND_END_LINE /**< Find the end line. */ + }; + + /** + * The track status. + */ + enum TrackStatus + { + TRACK_STATUS_ON_TRACK = 0, /**< Robot is on track */ + TRACK_STATUS_LOST, /**< Robot lost track */ + 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; + + 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. */ + uint8_t m_startEndLineDebounce; /**< Counter used for easys debouncing of the start-/end line detection. */ + MovAvg m_posMovAvg; /**< The moving average of the position over 2 calling cycles. */ + + /** + * Default constructor. + */ + DrivingState() : + m_observationTimer(), + m_lapTime(), + m_pidProcessTime(), + m_pidCtrl(), + m_topSpeed(0), + m_lineStatus(LINE_STATUS_FIND_START_LINE), + m_trackStatus(TRACK_STATUS_ON_TRACK), + m_startEndLineDebounce(0), + m_posMovAvg() + { + } + + /** + * Default destructor. + */ + ~DrivingState() + { + } + + /* Not allowed. */ + DrivingState(const DrivingState& state); /**< Copy construction of an instance. */ + DrivingState& operator=(const DrivingState& state); /**< Assignment of an instance. */ + + /** + * Control driving in case the robot is on track. + * + * @param[in] position Current position on track + * @param[in] lineSensorValues Value of each line sensor + */ + void processOnTrack(int16_t position, const uint16_t* lineSensorValues); + + /** + * Control driving in case the robot lost the track. + * It handles the track search algorithm. + * + * @param[in] position Current position on track + * @param[in] lineSensorValues Value of each line sensor + */ + void processTrackLost(int16_t position, const uint16_t* lineSensorValues); + + /** + * Is start-/endline detected? + * + * @param[in] lineSensorValues Line sensor values + * + * @return If a start-/endline is detected, it will return true otherwise false. + */ + bool isStartEndLineDetected(const uint16_t* lineSensorValues); + + /** + * Is a track gap detected? + * Note, it contains no debouncing inside. If necessary, it shall be + * done outside. + * + * @param[in] position Determined position in digits + * + * @return If a track gap is detected, it will return true otherwise false. + */ + bool isTrackGapDetected(int16_t position) const; + + /** + * Adapt driving by using a PID algorithm, depended on the position + * input. + * + * @param[in] position Position in digits + */ + void adaptDriving(int16_t position); +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* DRIVING_STATE_H */ +/** @} */ diff --git a/lib/APPConvoyLeader/ErrorState.cpp b/lib/APPConvoyLeader/ErrorState.cpp index 111e7aff..2bbc056a 100644 --- a/lib/APPConvoyLeader/ErrorState.cpp +++ b/lib/APPConvoyLeader/ErrorState.cpp @@ -1,118 +1,118 @@ -/* MIT License - * - * Copyright (c) 2023 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 "MotorSpeedCalibrationState.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void ErrorState::entry() -{ - IDisplay& display = Board::getInstance().getDisplay(); - - display.clear(); - display.print("Error"); - display.gotoXY(0, 1); - display.print(m_errorMsg); -} - -void ErrorState::process(StateMachine& sm) -{ - IButton& buttonA = Board::getInstance().getButtonA(); - - /* Restart calibration? */ - if (true == buttonA.isPressed()) - { - buttonA.waitForRelease(); - sm.setState(&MotorSpeedCalibrationState::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 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 "MotorSpeedCalibrationState.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void ErrorState::entry() +{ + IDisplay& display = Board::getInstance().getDisplay(); + + display.clear(); + display.print("Error"); + display.gotoXY(0, 1); + display.print(m_errorMsg); +} + +void ErrorState::process(StateMachine& sm) +{ + IButton& buttonA = Board::getInstance().getButtonA(); + + /* Restart calibration? */ + if (true == buttonA.isPressed()) + { + buttonA.waitForRelease(); + sm.setState(&MotorSpeedCalibrationState::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/APPConvoyLeader/ErrorState.h b/lib/APPConvoyLeader/ErrorState.h index 4f9c592e..3ded9542 100644 --- a/lib/APPConvoyLeader/ErrorState.h +++ b/lib/APPConvoyLeader/ErrorState.h @@ -1,134 +1,134 @@ -/* MIT License - * - * Copyright (c) 2023 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 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 */ +/** @} */ diff --git a/lib/APPConvoyLeader/LineSensorsCalibrationState.cpp b/lib/APPConvoyLeader/LineSensorsCalibrationState.cpp index e4be074a..31c8591d 100644 --- a/lib/APPConvoyLeader/LineSensorsCalibrationState.cpp +++ b/lib/APPConvoyLeader/LineSensorsCalibrationState.cpp @@ -1,209 +1,209 @@ -/* MIT License - * - * Copyright (c) 2023 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(); - - display.clear(); - display.print("Calib"); - display.gotoXY(0, 1); - display.print("LineS"); - - /* Prepare calibration drive. */ - m_calibrationSpeed = diffDrive.getMaxMotorSpeed() / 3; - m_orientation = odometry.getOrientation(); - - /* 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, "Cal=", 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 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(); + + display.clear(); + display.print("Calib"); + display.gotoXY(0, 1); + display.print("LineS"); + + /* Prepare calibration drive. */ + m_calibrationSpeed = diffDrive.getMaxMotorSpeed() / 3; + m_orientation = odometry.getOrientation(); + + /* 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, "Cal=", 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/APPConvoyLeader/LineSensorsCalibrationState.h b/lib/APPConvoyLeader/LineSensorsCalibrationState.h index 075577f1..3f695436 100644 --- a/lib/APPConvoyLeader/LineSensorsCalibrationState.h +++ b/lib/APPConvoyLeader/LineSensorsCalibrationState.h @@ -1,161 +1,161 @@ -/* MIT License - * - * Copyright (c) 2023 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 60°). - */ - static const int32_t CALIB_ANGLE = (FP_2PI() / 6); - - 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 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 60°). + */ + static const int32_t CALIB_ANGLE = (FP_2PI() / 6); + + 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/APPConvoyLeader/MotorSpeedCalibrationState.cpp b/lib/APPConvoyLeader/MotorSpeedCalibrationState.cpp index 8bdec403..e87babf6 100644 --- a/lib/APPConvoyLeader/MotorSpeedCalibrationState.cpp +++ b/lib/APPConvoyLeader/MotorSpeedCalibrationState.cpp @@ -1,234 +1,234 @@ -/* MIT License - * - * Copyright (c) 2023 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 "ReadyState.h" -#include "ErrorState.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/** - * Logging source. - */ -LOG_TAG("MSCState"); - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void MotorSpeedCalibrationState::entry() -{ - IDisplay& display = Board::getInstance().getDisplay(); - - display.clear(); - display.print("Calib"); - display.gotoXY(0, 1); - display.print("MSpeed"); - - /* 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(); - - /* 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; - - /* 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("MS=0"); - sm.setState(&ErrorState::getInstance()); - } - else - { - LOG_INFO_VAL("Calibrated max. speed (steps/s): ", maxSpeed); - LOG_INFO_VAL("Calibrated max. speed (mm/s): ", maxSpeed / RobotConstants::ENCODER_STEPS_PER_MM); - - sm.setState(&ReadyState::getInstance()); - } -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 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 "ReadyState.h" +#include "ErrorState.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/** + * Logging source. + */ +LOG_TAG("MSCState"); + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void MotorSpeedCalibrationState::entry() +{ + IDisplay& display = Board::getInstance().getDisplay(); + + display.clear(); + display.print("Calib"); + display.gotoXY(0, 1); + display.print("MSpeed"); + + /* 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(); + + /* 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; + + /* 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("MS=0"); + sm.setState(&ErrorState::getInstance()); + } + else + { + LOG_INFO_VAL("Calibrated max. speed (steps/s): ", maxSpeed); + LOG_INFO_VAL("Calibrated max. speed (mm/s): ", maxSpeed / RobotConstants::ENCODER_STEPS_PER_MM); + + sm.setState(&ReadyState::getInstance()); + } +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPConvoyLeader/MotorSpeedCalibrationState.h b/lib/APPConvoyLeader/MotorSpeedCalibrationState.h index 4dbca55e..356d0204 100644 --- a/lib/APPConvoyLeader/MotorSpeedCalibrationState.h +++ b/lib/APPConvoyLeader/MotorSpeedCalibrationState.h @@ -1,165 +1,165 @@ -/* MIT License - * - * Copyright (c) 2023 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 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/APPConvoyLeader/ParameterSets.cpp b/lib/APPConvoyLeader/ParameterSets.cpp index f615a823..fd36c3e7 100644 --- a/lib/APPConvoyLeader/ParameterSets.cpp +++ b/lib/APPConvoyLeader/ParameterSets.cpp @@ -1,139 +1,139 @@ -/* MIT License - * - * Copyright (c) 2023 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] = { - "PID Slow", /* Name */ - 1920, /* Top speed in steps/s */ - 3, /* Kp Numerator */ - 2, /* Kp Denominator */ - 1, /* Ki Numerator */ - 60, /* Ki Denominator */ - 4, /* Kd Numerator */ - 1 /* Kd Denominator */ - }; - - m_parSets[1] = { - "PID Fast", /* Name */ - 2400, /* Top speed in steps/s */ - 3, /* Kp Numerator */ - 2, /* Kp Denominator */ - 1, /* Ki Numerator */ - 40, /* Ki Denominator */ - 10, /* Kd Numerator */ - 1 /* Kd Denominator */ - }; - - m_parSets[2] = { - "PD Fast", /* Name */ - 2400, /* Top speed in steps/s */ - 3, /* Kp Numerator */ - 1, /* Kp Denominator */ - 0, /* Ki Numerator */ - 1, /* Ki Denominator */ - 10, /* Kd Numerator */ - 1 /* Kd Denominator */ - }; -} - -ParameterSets::~ParameterSets() -{ -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 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] = { + "PID Slow", /* Name */ + 1920, /* Top speed in steps/s */ + 3, /* Kp Numerator */ + 2, /* Kp Denominator */ + 1, /* Ki Numerator */ + 60, /* Ki Denominator */ + 4, /* Kd Numerator */ + 1 /* Kd Denominator */ + }; + + m_parSets[1] = { + "PID Fast", /* Name */ + 2400, /* Top speed in steps/s */ + 3, /* Kp Numerator */ + 2, /* Kp Denominator */ + 1, /* Ki Numerator */ + 40, /* Ki Denominator */ + 10, /* Kd Numerator */ + 1 /* Kd Denominator */ + }; + + m_parSets[2] = { + "PD Fast", /* Name */ + 2400, /* Top speed in steps/s */ + 3, /* Kp Numerator */ + 1, /* Kp Denominator */ + 0, /* Ki Numerator */ + 1, /* Ki Denominator */ + 10, /* Kd Numerator */ + 1 /* Kd Denominator */ + }; +} + +ParameterSets::~ParameterSets() +{ +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPConvoyLeader/ParameterSets.h b/lib/APPConvoyLeader/ParameterSets.h index b3684da4..28b3bd3c 100644 --- a/lib/APPConvoyLeader/ParameterSets.h +++ b/lib/APPConvoyLeader/ParameterSets.h @@ -1,145 +1,145 @@ -/* MIT License - * - * Copyright (c) 2023 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 = 3; - -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(); - - /* Not allowed. */ - ParameterSets(const ParameterSets& set); /**< Copy construction of an instance. */ - ParameterSets& operator=(const ParameterSets& set); /**< Assignment of an instance. */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* PARAMETER_SET_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 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 = 3; + +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(); + + /* Not allowed. */ + ParameterSets(const ParameterSets& set); /**< Copy construction of an instance. */ + ParameterSets& operator=(const ParameterSets& set); /**< Assignment of an instance. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* PARAMETER_SET_H */ +/** @} */ diff --git a/lib/APPConvoyLeader/ReadyState.cpp b/lib/APPConvoyLeader/ReadyState.cpp index 7463a689..3174cff9 100644 --- a/lib/APPConvoyLeader/ReadyState.cpp +++ b/lib/APPConvoyLeader/ReadyState.cpp @@ -1,117 +1,117 @@ -/* MIT License - * - * Copyright (c) 2023 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 -#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 */ - - display.clear(); - display.print("Rdy."); - - if (true == m_isLapTimeAvailable) - { - display.gotoXY(0, 1); - display.print(m_lapTime); - } - - /* The line sensor value shall be output on console cyclic. */ - m_timer.start(SENSOR_VALUE_OUT_PERIOD); - DifferentialDrive::getInstance().enable(); -} - -void ReadyState::process(StateMachine& sm) -{ - /* Do nothing. */ -} - -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 - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 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 +#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 */ + + display.clear(); + display.print("Rdy."); + + if (true == m_isLapTimeAvailable) + { + display.gotoXY(0, 1); + display.print(m_lapTime); + } + + /* The line sensor value shall be output on console cyclic. */ + m_timer.start(SENSOR_VALUE_OUT_PERIOD); + DifferentialDrive::getInstance().enable(); +} + +void ReadyState::process(StateMachine& sm) +{ + /* Do nothing. */ +} + +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/APPConvoyLeader/ReadyState.h b/lib/APPConvoyLeader/ReadyState.h index 1553f1b2..25efd942 100644 --- a/lib/APPConvoyLeader/ReadyState.h +++ b/lib/APPConvoyLeader/ReadyState.h @@ -1,129 +1,129 @@ -/* MIT License - * - * Copyright (c) 2023 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. */ - - /** - * Default constructor. - */ - ReadyState() : m_timer(), m_isLapTimeAvailable(false), m_lapTime(0) - { - } - - /** - * 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 */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 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. */ + + /** + * Default constructor. + */ + ReadyState() : m_timer(), m_isLapTimeAvailable(false), m_lapTime(0) + { + } + + /** + * 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/APPConvoyLeader/ReleaseTrackState.cpp b/lib/APPConvoyLeader/ReleaseTrackState.cpp index 9e9592ae..646f4f25 100644 --- a/lib/APPConvoyLeader/ReleaseTrackState.cpp +++ b/lib/APPConvoyLeader/ReleaseTrackState.cpp @@ -1,129 +1,129 @@ -/* MIT License - * - * Copyright (c) 2023 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 "ReleaseTrackState.h" -#include -#include -#include "DrivingState.h" -#include "ParameterSets.h" - -/****************************************************************************** - * 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 == buttonA.isPressed()) - { - /* Choose next parameter set (round-robin) */ - ParameterSets::getInstance().next(); - showParSet(); - - buttonA.waitForRelease(); - 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("Set "); - display.print(parSetId); - display.gotoXY(0, 1); - display.print(parSetName); -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 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 "ReleaseTrackState.h" +#include +#include +#include "DrivingState.h" +#include "ParameterSets.h" + +/****************************************************************************** + * 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 == buttonA.isPressed()) + { + /* Choose next parameter set (round-robin) */ + ParameterSets::getInstance().next(); + showParSet(); + + buttonA.waitForRelease(); + 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("Set "); + display.print(parSetId); + display.gotoXY(0, 1); + display.print(parSetName); +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPConvoyLeader/ReleaseTrackState.h b/lib/APPConvoyLeader/ReleaseTrackState.h index 56f2a636..3c408768 100644 --- a/lib/APPConvoyLeader/ReleaseTrackState.h +++ b/lib/APPConvoyLeader/ReleaseTrackState.h @@ -1,129 +1,129 @@ -/* MIT License - * - * Copyright (c) 2023 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 */ - - /** - * Default constructor. - */ - ReleaseTrackState() - { - } - - /** - * Default destructor. - */ - ~ReleaseTrackState() - { - } - - /* Not allowed. */ - ReleaseTrackState(const ReleaseTrackState& state); /**< Copy construction of an instance. */ - ReleaseTrackState& operator=(const ReleaseTrackState& state); /**< Assignment of an instance. */ - - /** - * Show choosen parameter set on LCD. - */ - void showParSet() const; -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* RELEASE_TRACK_STATE_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 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 */ + + /** + * Default constructor. + */ + ReleaseTrackState() + { + } + + /** + * Default destructor. + */ + ~ReleaseTrackState() + { + } + + /* Not allowed. */ + ReleaseTrackState(const ReleaseTrackState& state); /**< Copy construction of an instance. */ + ReleaseTrackState& operator=(const ReleaseTrackState& state); /**< Assignment of an instance. */ + + /** + * Show choosen parameter set on LCD. + */ + void showParSet() const; +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* RELEASE_TRACK_STATE_H */ +/** @} */ diff --git a/lib/APPConvoyLeader/SerialMuxChannels.h b/lib/APPConvoyLeader/SerialMuxChannels.h index 558bd541..5bd64310 100644 --- a/lib/APPConvoyLeader/SerialMuxChannels.h +++ b/lib/APPConvoyLeader/SerialMuxChannels.h @@ -1,97 +1,97 @@ -/* MIT License - * - * Copyright (c) 2023 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 - * - * @addtogroup Application - * - * @{ - */ -#ifndef SERIAL_MUX_CHANNELS_H -#define SERIAL_MUX_CHANNELS_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ - -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/** Maximum number of SerialMuxProt Channels. */ -#define MAX_CHANNELS (10U) - -/** Name of Channel to send Odometry Data to. */ -#define ODOMETRY_CHANNEL_NAME "ODOMETRY" - -/** DLC of Odometry Channel */ -#define ODOMETRY_CHANNEL_DLC (sizeof(OdometryData)) - -/** Name of Channel to send Speedometer Data to. */ -#define SPEED_CHANNEL_NAME "SPEED" - -/** DLC of Speedometer Channel */ -#define SPEED_CHANNEL_DLC (sizeof(SpeedData)) - -/** Name of Channel to send Motor Speed Setpoints to. */ -#define SPEED_SETPOINT_CHANNEL_NAME "SPEED_SET" - -/** DLC of Speedometer Channel */ -#define SPEED_SETPOINT_CHANNEL_DLC (sizeof(SpeedData)) - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** Struct of the "Odometry" channel payload. */ -typedef struct _OdometryData -{ - int32_t xPos; /**< X position [mm]. */ - int32_t yPos; /**< Y position [mm]. */ - int32_t orientation; /**< Orientation [mrad]. */ -} __attribute__((packed)) OdometryData; - -/** 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; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* SERIAL_MUX_CHANNELS_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 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 + * + * @addtogroup Application + * + * @{ + */ +#ifndef SERIAL_MUX_CHANNELS_H +#define SERIAL_MUX_CHANNELS_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ + +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/** Maximum number of SerialMuxProt Channels. */ +#define MAX_CHANNELS (10U) + +/** Name of Channel to send Odometry Data to. */ +#define ODOMETRY_CHANNEL_NAME "ODOMETRY" + +/** DLC of Odometry Channel */ +#define ODOMETRY_CHANNEL_DLC (sizeof(OdometryData)) + +/** Name of Channel to send Speedometer Data to. */ +#define SPEED_CHANNEL_NAME "SPEED" + +/** DLC of Speedometer Channel */ +#define SPEED_CHANNEL_DLC (sizeof(SpeedData)) + +/** Name of Channel to send Motor Speed Setpoints to. */ +#define SPEED_SETPOINT_CHANNEL_NAME "SPEED_SET" + +/** DLC of Speedometer Channel */ +#define SPEED_SETPOINT_CHANNEL_DLC (sizeof(SpeedData)) + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** Struct of the "Odometry" channel payload. */ +typedef struct _OdometryData +{ + int32_t xPos; /**< X position [mm]. */ + int32_t yPos; /**< Y position [mm]. */ + int32_t orientation; /**< Orientation [mrad]. */ +} __attribute__((packed)) OdometryData; + +/** 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; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* SERIAL_MUX_CHANNELS_H */ +/** @} */ diff --git a/lib/APPConvoyLeader/StartupState.cpp b/lib/APPConvoyLeader/StartupState.cpp index 1e3552a9..c6e4161a 100644 --- a/lib/APPConvoyLeader/StartupState.cpp +++ b/lib/APPConvoyLeader/StartupState.cpp @@ -1,113 +1,113 @@ -/* MIT License - * - * Copyright (c) 2023 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 "Sound.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void StartupState::entry() -{ - IDisplay& display = Board::getInstance().getDisplay(); - - /* Initialize HAL */ - Board::getInstance().init(); - - /* Surprise the audience. */ - Sound::playStartup(); - - /* Show team id / team name */ - display.clear(); - display.print(TEAM_NAME_LINE_1); - display.gotoXY(0, 1); - display.print(TEAM_NAME_LINE_2); - delay(TEAM_NAME_DURATION); - - /* Show operator info on LCD */ - display.clear(); - display.print("Press A"); - display.gotoXY(0, 1); - display.print("to calib"); -} - -void StartupState::process(StateMachine& sm) -{ - sm.setState(&MotorSpeedCalibrationState::getInstance()); -} - -void StartupState::exit() -{ - /* Nothing to do */ -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 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 "Sound.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void StartupState::entry() +{ + IDisplay& display = Board::getInstance().getDisplay(); + + /* Initialize HAL */ + Board::getInstance().init(); + + /* Surprise the audience. */ + Sound::playStartup(); + + /* Show team id / team name */ + display.clear(); + display.print(TEAM_NAME_LINE_1); + display.gotoXY(0, 1); + display.print(TEAM_NAME_LINE_2); + delay(TEAM_NAME_DURATION); + + /* Show operator info on LCD */ + display.clear(); + display.print("Press A"); + display.gotoXY(0, 1); + display.print("to calib"); +} + +void StartupState::process(StateMachine& sm) +{ + sm.setState(&MotorSpeedCalibrationState::getInstance()); +} + +void StartupState::exit() +{ + /* Nothing to do */ +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPConvoyLeader/StartupState.h b/lib/APPConvoyLeader/StartupState.h index 589eb4f3..8f3d8b5a 100644 --- a/lib/APPConvoyLeader/StartupState.h +++ b/lib/APPConvoyLeader/StartupState.h @@ -1,123 +1,123 @@ -/* MIT License - * - * Copyright (c) 2023 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 - -/****************************************************************************** - * 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: - /** - * Duration in ms how long the team id or team name shall be shown at startup. - */ - static const uint32_t TEAM_NAME_DURATION = 2000; - - /** - * Default constructor. - */ - StartupState() - { - } - - /** - * Default destructor. - */ - ~StartupState() - { - } - - /* Not allowed. */ - StartupState(const StartupState& state); /**< Copy construction of an instance. */ - StartupState& operator=(const StartupState& state); /**< Assignment of an instance. */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* STARTUP_STATE_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 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 + +/****************************************************************************** + * 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: + /** + * Duration in ms how long the team id or team name shall be shown at startup. + */ + static const uint32_t TEAM_NAME_DURATION = 2000; + + /** + * Default constructor. + */ + StartupState() + { + } + + /** + * Default destructor. + */ + ~StartupState() + { + } + + /* Not allowed. */ + StartupState(const StartupState& state); /**< Copy construction of an instance. */ + StartupState& operator=(const StartupState& state); /**< Assignment of an instance. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* STARTUP_STATE_H */ +/** @} */ diff --git a/lib/APPConvoyLeader/library.json b/lib/APPConvoyLeader/library.json index 7cade5d2..f9e59335 100644 --- a/lib/APPConvoyLeader/library.json +++ b/lib/APPConvoyLeader/library.json @@ -1,29 +1,29 @@ -{ - "name": "APPConvoyLeader", - "version": "0.1.0", - "description": "Convoy leader application", - "authors": [ - { - "name": "Andreas Merkle", - "email": "web@blue-andi.de", - "url": "https://github.com/BlueAndi", - "maintainer": true - } - ], - "license": "MIT", - "dependencies": [ - { - "name": "Service" - }, - { - "name": "HALInterfaces" - }, - { - "owner": "gabryelreyes", - "name": "SerialMuxProt", - "version": "^2.0.0" - } - ], - "frameworks": "*", - "platforms": "*" -} +{ + "name": "APPConvoyLeader", + "version": "0.1.0", + "description": "Convoy leader application", + "authors": [ + { + "name": "Andreas Merkle", + "email": "web@blue-andi.de", + "url": "https://github.com/BlueAndi", + "maintainer": true + } + ], + "license": "MIT", + "dependencies": [ + { + "name": "Service" + }, + { + "name": "HALInterfaces" + }, + { + "owner": "gabryelreyes", + "name": "SerialMuxProt", + "version": "^2.0.0" + } + ], + "frameworks": "*", + "platforms": "*" +} diff --git a/lib/APPLineFollower/App.cpp b/lib/APPLineFollower/App.cpp index 54437b05..47e2fc0e 100644 --- a/lib/APPLineFollower/App.cpp +++ b/lib/APPLineFollower/App.cpp @@ -1,113 +1,113 @@ -/* MIT License - * - * Copyright (c) 2023 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 - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void App::setup() -{ - Serial.begin(SERIAL_BAUDRATE); - Board::getInstance().init(); - m_systemStateMachine.setState(&StartupState::getInstance()); - m_controlInterval.start(DIFFERENTIAL_DRIVE_CONTROL_PERIOD); -} - -void App::loop() -{ - 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(); - - m_controlInterval.restart(); - } - - m_systemStateMachine.process(); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions +/* MIT License + * + * Copyright (c) 2023 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 + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void App::setup() +{ + Serial.begin(SERIAL_BAUDRATE); + Board::getInstance().init(); + m_systemStateMachine.setState(&StartupState::getInstance()); + m_controlInterval.start(DIFFERENTIAL_DRIVE_CONTROL_PERIOD); +} + +void App::loop() +{ + 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(); + + m_controlInterval.restart(); + } + + m_systemStateMachine.process(); +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions *****************************************************************************/ \ No newline at end of file diff --git a/lib/APPLineFollower/App.h b/lib/APPLineFollower/App.h index 49c10039..47408043 100644 --- a/lib/APPLineFollower/App.h +++ b/lib/APPLineFollower/App.h @@ -1,109 +1,109 @@ -/* MIT License - * - * Copyright (c) 2023 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; - - /* Not allowed. */ - App(const App& app); /**< Copy construction of an instance. */ - App& operator=(const App& app); /**< Assignment of an instance. */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* APP_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 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; + + /* 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/APPLineFollower/ErrorState.cpp b/lib/APPLineFollower/ErrorState.cpp index 111e7aff..2bbc056a 100644 --- a/lib/APPLineFollower/ErrorState.cpp +++ b/lib/APPLineFollower/ErrorState.cpp @@ -1,118 +1,118 @@ -/* MIT License - * - * Copyright (c) 2023 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 "MotorSpeedCalibrationState.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void ErrorState::entry() -{ - IDisplay& display = Board::getInstance().getDisplay(); - - display.clear(); - display.print("Error"); - display.gotoXY(0, 1); - display.print(m_errorMsg); -} - -void ErrorState::process(StateMachine& sm) -{ - IButton& buttonA = Board::getInstance().getButtonA(); - - /* Restart calibration? */ - if (true == buttonA.isPressed()) - { - buttonA.waitForRelease(); - sm.setState(&MotorSpeedCalibrationState::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 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 "MotorSpeedCalibrationState.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void ErrorState::entry() +{ + IDisplay& display = Board::getInstance().getDisplay(); + + display.clear(); + display.print("Error"); + display.gotoXY(0, 1); + display.print(m_errorMsg); +} + +void ErrorState::process(StateMachine& sm) +{ + IButton& buttonA = Board::getInstance().getButtonA(); + + /* Restart calibration? */ + if (true == buttonA.isPressed()) + { + buttonA.waitForRelease(); + sm.setState(&MotorSpeedCalibrationState::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/APPLineFollower/ErrorState.h b/lib/APPLineFollower/ErrorState.h index 4f9c592e..3ded9542 100644 --- a/lib/APPLineFollower/ErrorState.h +++ b/lib/APPLineFollower/ErrorState.h @@ -1,134 +1,134 @@ -/* MIT License - * - * Copyright (c) 2023 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 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 */ +/** @} */ diff --git a/lib/APPLineFollower/LineSensorsCalibrationState.cpp b/lib/APPLineFollower/LineSensorsCalibrationState.cpp index e4be074a..31c8591d 100644 --- a/lib/APPLineFollower/LineSensorsCalibrationState.cpp +++ b/lib/APPLineFollower/LineSensorsCalibrationState.cpp @@ -1,209 +1,209 @@ -/* MIT License - * - * Copyright (c) 2023 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(); - - display.clear(); - display.print("Calib"); - display.gotoXY(0, 1); - display.print("LineS"); - - /* Prepare calibration drive. */ - m_calibrationSpeed = diffDrive.getMaxMotorSpeed() / 3; - m_orientation = odometry.getOrientation(); - - /* 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, "Cal=", 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 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(); + + display.clear(); + display.print("Calib"); + display.gotoXY(0, 1); + display.print("LineS"); + + /* Prepare calibration drive. */ + m_calibrationSpeed = diffDrive.getMaxMotorSpeed() / 3; + m_orientation = odometry.getOrientation(); + + /* 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, "Cal=", 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/APPLineFollower/LineSensorsCalibrationState.h b/lib/APPLineFollower/LineSensorsCalibrationState.h index 075577f1..3f695436 100644 --- a/lib/APPLineFollower/LineSensorsCalibrationState.h +++ b/lib/APPLineFollower/LineSensorsCalibrationState.h @@ -1,161 +1,161 @@ -/* MIT License - * - * Copyright (c) 2023 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 60°). - */ - static const int32_t CALIB_ANGLE = (FP_2PI() / 6); - - 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 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 60°). + */ + static const int32_t CALIB_ANGLE = (FP_2PI() / 6); + + 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/APPLineFollower/MotorSpeedCalibrationState.cpp b/lib/APPLineFollower/MotorSpeedCalibrationState.cpp index c4fb12b1..c54f0bc9 100644 --- a/lib/APPLineFollower/MotorSpeedCalibrationState.cpp +++ b/lib/APPLineFollower/MotorSpeedCalibrationState.cpp @@ -1,234 +1,234 @@ -/* MIT License - * - * Copyright (c) 2023 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" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/** - * Logging source. - */ -LOG_TAG("MSCState"); - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void MotorSpeedCalibrationState::entry() -{ - IDisplay& display = Board::getInstance().getDisplay(); - - display.clear(); - display.print("Calib"); - display.gotoXY(0, 1); - display.print("MSpeed"); - - /* 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(); - - /* 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; - - /* 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("MS=0"); - sm.setState(&ErrorState::getInstance()); - } - else - { - LOG_INFO_VAL("Calibrated max. speed (steps/s): ", maxSpeed); - LOG_INFO_VAL("Calibrated max. speed (mm/s): ", maxSpeed / RobotConstants::ENCODER_STEPS_PER_MM); - - sm.setState(&LineSensorsCalibrationState::getInstance()); - } -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 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" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/** + * Logging source. + */ +LOG_TAG("MSCState"); + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void MotorSpeedCalibrationState::entry() +{ + IDisplay& display = Board::getInstance().getDisplay(); + + display.clear(); + display.print("Calib"); + display.gotoXY(0, 1); + display.print("MSpeed"); + + /* 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(); + + /* 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; + + /* 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("MS=0"); + sm.setState(&ErrorState::getInstance()); + } + else + { + LOG_INFO_VAL("Calibrated max. speed (steps/s): ", maxSpeed); + LOG_INFO_VAL("Calibrated max. speed (mm/s): ", maxSpeed / RobotConstants::ENCODER_STEPS_PER_MM); + + sm.setState(&LineSensorsCalibrationState::getInstance()); + } +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPLineFollower/MotorSpeedCalibrationState.h b/lib/APPLineFollower/MotorSpeedCalibrationState.h index 4dbca55e..356d0204 100644 --- a/lib/APPLineFollower/MotorSpeedCalibrationState.h +++ b/lib/APPLineFollower/MotorSpeedCalibrationState.h @@ -1,165 +1,165 @@ -/* MIT License - * - * Copyright (c) 2023 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 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/APPLineFollower/ParameterSets.h b/lib/APPLineFollower/ParameterSets.h index b3684da4..28b3bd3c 100644 --- a/lib/APPLineFollower/ParameterSets.h +++ b/lib/APPLineFollower/ParameterSets.h @@ -1,145 +1,145 @@ -/* MIT License - * - * Copyright (c) 2023 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 = 3; - -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(); - - /* Not allowed. */ - ParameterSets(const ParameterSets& set); /**< Copy construction of an instance. */ - ParameterSets& operator=(const ParameterSets& set); /**< Assignment of an instance. */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* PARAMETER_SET_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 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 = 3; + +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(); + + /* Not allowed. */ + ParameterSets(const ParameterSets& set); /**< Copy construction of an instance. */ + ParameterSets& operator=(const ParameterSets& set); /**< Assignment of an instance. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* PARAMETER_SET_H */ +/** @} */ diff --git a/lib/APPLineFollower/ReadyState.cpp b/lib/APPLineFollower/ReadyState.cpp index 9760ecd8..7221d71d 100644 --- a/lib/APPLineFollower/ReadyState.cpp +++ b/lib/APPLineFollower/ReadyState.cpp @@ -1,160 +1,160 @@ -/* MIT License - * - * Copyright (c) 2023 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("Rdy."); - - if (true == m_isLapTimeAvailable) - { - display.gotoXY(0, 1); - display.print(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 == buttonA.isPressed()) - { - buttonA.waitForRelease(); - sm.setState(&ReleaseTrackState::getInstance()); - } - /* Shall the line sensor values be printed out on console? */ - else 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(); - } -} - -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 - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 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("Rdy."); + + if (true == m_isLapTimeAvailable) + { + display.gotoXY(0, 1); + display.print(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 == buttonA.isPressed()) + { + buttonA.waitForRelease(); + sm.setState(&ReleaseTrackState::getInstance()); + } + /* Shall the line sensor values be printed out on console? */ + else 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(); + } +} + +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/APPLineFollower/ReadyState.h b/lib/APPLineFollower/ReadyState.h index 1553f1b2..25efd942 100644 --- a/lib/APPLineFollower/ReadyState.h +++ b/lib/APPLineFollower/ReadyState.h @@ -1,129 +1,129 @@ -/* MIT License - * - * Copyright (c) 2023 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. */ - - /** - * Default constructor. - */ - ReadyState() : m_timer(), m_isLapTimeAvailable(false), m_lapTime(0) - { - } - - /** - * 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 */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 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. */ + + /** + * Default constructor. + */ + ReadyState() : m_timer(), m_isLapTimeAvailable(false), m_lapTime(0) + { + } + + /** + * 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/APPLineFollower/ReleaseTrackState.cpp b/lib/APPLineFollower/ReleaseTrackState.cpp index 67351a9d..a485d540 100644 --- a/lib/APPLineFollower/ReleaseTrackState.cpp +++ b/lib/APPLineFollower/ReleaseTrackState.cpp @@ -1,129 +1,129 @@ -/* MIT License - * - * Copyright (c) 2023 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" - -/****************************************************************************** - * 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 == buttonA.isPressed()) - { - /* Choose next parameter set (round-robin) */ - ParameterSets::getInstance().next(); - showParSet(); - - buttonA.waitForRelease(); - 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("Set "); - display.print(parSetId); - display.gotoXY(0, 1); - display.print(parSetName); -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 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" + +/****************************************************************************** + * 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 == buttonA.isPressed()) + { + /* Choose next parameter set (round-robin) */ + ParameterSets::getInstance().next(); + showParSet(); + + buttonA.waitForRelease(); + 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("Set "); + display.print(parSetId); + display.gotoXY(0, 1); + display.print(parSetName); +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPLineFollower/ReleaseTrackState.h b/lib/APPLineFollower/ReleaseTrackState.h index 56f2a636..3c408768 100644 --- a/lib/APPLineFollower/ReleaseTrackState.h +++ b/lib/APPLineFollower/ReleaseTrackState.h @@ -1,129 +1,129 @@ -/* MIT License - * - * Copyright (c) 2023 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 */ - - /** - * Default constructor. - */ - ReleaseTrackState() - { - } - - /** - * Default destructor. - */ - ~ReleaseTrackState() - { - } - - /* Not allowed. */ - ReleaseTrackState(const ReleaseTrackState& state); /**< Copy construction of an instance. */ - ReleaseTrackState& operator=(const ReleaseTrackState& state); /**< Assignment of an instance. */ - - /** - * Show choosen parameter set on LCD. - */ - void showParSet() const; -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* RELEASE_TRACK_STATE_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 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 */ + + /** + * Default constructor. + */ + ReleaseTrackState() + { + } + + /** + * Default destructor. + */ + ~ReleaseTrackState() + { + } + + /* Not allowed. */ + ReleaseTrackState(const ReleaseTrackState& state); /**< Copy construction of an instance. */ + ReleaseTrackState& operator=(const ReleaseTrackState& state); /**< Assignment of an instance. */ + + /** + * Show choosen parameter set on LCD. + */ + void showParSet() const; +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* RELEASE_TRACK_STATE_H */ +/** @} */ diff --git a/lib/APPLineFollower/StartupState.cpp b/lib/APPLineFollower/StartupState.cpp index 2912110f..10ffefaf 100644 --- a/lib/APPLineFollower/StartupState.cpp +++ b/lib/APPLineFollower/StartupState.cpp @@ -1,119 +1,119 @@ -/* MIT License - * - * Copyright (c) 2023 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 "StartupState.h" -#include -#include -#include "MotorSpeedCalibrationState.h" -#include "Sound.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void StartupState::entry() -{ - IDisplay& display = Board::getInstance().getDisplay(); - - /* Initialize HAL */ - Board::getInstance().init(); - - /* Surprise the audience. */ - Sound::playStartup(); - - /* Show team id / team name */ - display.clear(); - display.print(TEAM_NAME_LINE_1); - display.gotoXY(0, 1); - display.print(TEAM_NAME_LINE_2); - delay(TEAM_NAME_DURATION); - - /* Show operator info on LCD */ - display.clear(); - display.print("Press A"); - display.gotoXY(0, 1); - display.print("to calib"); -} - -void StartupState::process(StateMachine& sm) -{ - IButton& buttonA = Board::getInstance().getButtonA(); - - if (true == buttonA.isPressed()) - { - buttonA.waitForRelease(); - sm.setState(&MotorSpeedCalibrationState::getInstance()); - } -} - -void StartupState::exit() -{ - /* Nothing to do */ -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 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 "StartupState.h" +#include +#include +#include "MotorSpeedCalibrationState.h" +#include "Sound.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void StartupState::entry() +{ + IDisplay& display = Board::getInstance().getDisplay(); + + /* Initialize HAL */ + Board::getInstance().init(); + + /* Surprise the audience. */ + Sound::playStartup(); + + /* Show team id / team name */ + display.clear(); + display.print(TEAM_NAME_LINE_1); + display.gotoXY(0, 1); + display.print(TEAM_NAME_LINE_2); + delay(TEAM_NAME_DURATION); + + /* Show operator info on LCD */ + display.clear(); + display.print("Press A"); + display.gotoXY(0, 1); + display.print("to calib"); +} + +void StartupState::process(StateMachine& sm) +{ + IButton& buttonA = Board::getInstance().getButtonA(); + + if (true == buttonA.isPressed()) + { + buttonA.waitForRelease(); + sm.setState(&MotorSpeedCalibrationState::getInstance()); + } +} + +void StartupState::exit() +{ + /* Nothing to do */ +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPLineFollower/StartupState.h b/lib/APPLineFollower/StartupState.h index 589eb4f3..8f3d8b5a 100644 --- a/lib/APPLineFollower/StartupState.h +++ b/lib/APPLineFollower/StartupState.h @@ -1,123 +1,123 @@ -/* MIT License - * - * Copyright (c) 2023 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 - -/****************************************************************************** - * 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: - /** - * Duration in ms how long the team id or team name shall be shown at startup. - */ - static const uint32_t TEAM_NAME_DURATION = 2000; - - /** - * Default constructor. - */ - StartupState() - { - } - - /** - * Default destructor. - */ - ~StartupState() - { - } - - /* Not allowed. */ - StartupState(const StartupState& state); /**< Copy construction of an instance. */ - StartupState& operator=(const StartupState& state); /**< Assignment of an instance. */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* STARTUP_STATE_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 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 + +/****************************************************************************** + * 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: + /** + * Duration in ms how long the team id or team name shall be shown at startup. + */ + static const uint32_t TEAM_NAME_DURATION = 2000; + + /** + * Default constructor. + */ + StartupState() + { + } + + /** + * Default destructor. + */ + ~StartupState() + { + } + + /* Not allowed. */ + StartupState(const StartupState& state); /**< Copy construction of an instance. */ + StartupState& operator=(const StartupState& state); /**< Assignment of an instance. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* STARTUP_STATE_H */ +/** @} */ diff --git a/lib/APPLineFollower/library.json b/lib/APPLineFollower/library.json index e52e90e1..e23ddbda 100644 --- a/lib/APPLineFollower/library.json +++ b/lib/APPLineFollower/library.json @@ -1,19 +1,19 @@ -{ - "name": "APPLineFollower", - "version": "0.1.0", - "description": "LineFollower application", - "authors": [{ - "name": "Andreas Merkle", - "email": "web@blue-andi.de", - "url": "https://github.com/BlueAndi", - "maintainer": true - }], - "license": "MIT", - "dependencies": [{ - "name": "Service" - }, { - "name": "HALInterfaces" - }], - "frameworks": "*", - "platforms": "*" -} +{ + "name": "APPLineFollower", + "version": "0.1.0", + "description": "LineFollower application", + "authors": [{ + "name": "Andreas Merkle", + "email": "web@blue-andi.de", + "url": "https://github.com/BlueAndi", + "maintainer": true + }], + "license": "MIT", + "dependencies": [{ + "name": "Service" + }, { + "name": "HALInterfaces" + }], + "frameworks": "*", + "platforms": "*" +} diff --git a/lib/APPRemoteControl/App.cpp b/lib/APPRemoteControl/App.cpp index 5cdad406..5995a20a 100644 --- a/lib/APPRemoteControl/App.cpp +++ b/lib/APPRemoteControl/App.cpp @@ -1,225 +1,225 @@ -/* MIT License - * - * Copyright (c) 2023 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 RemoteControl application - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "App.h" -#include "StartupState.h" -#include "RemoteCtrlState.h" -#include -#include -#include -#include -#include -#include -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -static void App_cmdChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData); -static void App_motorSpeedsChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData); - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/** Only in remote control state its possible to control the robot. */ -static bool gIsRemoteCtrlActive = false; - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void App::setup() -{ - Serial.begin(SERIAL_BAUDRATE); - Logging::disable(); - Board::getInstance().init(); - - m_systemStateMachine.setState(&StartupState::getInstance()); - m_controlInterval.start(DIFFERENTIAL_DRIVE_CONTROL_PERIOD); - m_sendLineSensorsDataInterval.start(SEND_LINE_SENSORS_DATA_PERIOD); - - /* Remote control commands/responses */ - m_smpServer.subscribeToChannel(COMMAND_CHANNEL_NAME, App_cmdChannelCallback); - m_smpChannelIdRemoteCtrlRsp = - m_smpServer.createChannel(COMMAND_RESPONSE_CHANNEL_NAME, COMMAND_RESPONSE_CHANNEL_DLC); - - /* Receiving linear motor speed left/right */ - m_smpServer.subscribeToChannel(SPEED_SETPOINT_CHANNEL_NAME, App_motorSpeedsChannelCallback); - - /* Providing line sensor data */ - m_smpChannelIdLineSensors = m_smpServer.createChannel(LINE_SENSOR_CHANNEL_NAME, LINE_SENSOR_CHANNEL_DLC); -} - -void App::loop() -{ - m_smpServer.process(millis()); - Speedometer::getInstance().process(); - - if (true == m_controlInterval.isTimeout()) - { - 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(); - - m_controlInterval.restart(); - } - - m_systemStateMachine.process(); - - /* Determine whether the robot can be remote controlled or not. */ - if (&RemoteCtrlState::getInstance() == m_systemStateMachine.getState()) - { - gIsRemoteCtrlActive = true; - } - else - { - gIsRemoteCtrlActive = false; - } - - /* Send periodically line sensor data. */ - if (true == m_sendLineSensorsDataInterval.isTimeout()) - { - sendLineSensorsData(); - - m_sendLineSensorsDataInterval.restart(); - } - - /* Send remote control command responses. */ - sendRemoteControlResponses(); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -void App::sendRemoteControlResponses() -{ - RemoteCtrlState::RspId remoteControlRspId = RemoteCtrlState::getInstance().getCmdRsp(); - - /* Send only on change. */ - if (remoteControlRspId != m_lastRemoteControlRspId) - { - const CommandResponse* payload = reinterpret_cast(&remoteControlRspId); - - (void)m_smpServer.sendData(m_smpChannelIdRemoteCtrlRsp, reinterpret_cast(&payload), sizeof(payload)); - - m_lastRemoteControlRspId = remoteControlRspId; - } -} - -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_smpChannelIdLineSensors, reinterpret_cast(&payload), sizeof(payload)); -} - -/****************************************************************************** - * 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) -{ - (void)userData; - if ((nullptr != payload) && (sizeof(RemoteCtrlState::CmdId) == payloadSize)) - { - RemoteCtrlState::CmdId cmdId = *reinterpret_cast(payload); - - RemoteCtrlState::getInstance().execute(cmdId); - } -} - -/** - * Receives motor speeds over SerialMuxProt channel. - * - * @param[in] payload Motor speed left/right - * @param[in] payloadSize Size of twice motor speeds - * @param[in] userData User data - */ -static void App_motorSpeedsChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData) -{ - (void)userData; - if ((nullptr != payload) && (SPEED_SETPOINT_CHANNEL_DLC == payloadSize) && (true == gIsRemoteCtrlActive)) - { - const SpeedData* motorSpeedData = reinterpret_cast(payload); - DifferentialDrive::getInstance().setLinearSpeed(motorSpeedData->left, motorSpeedData->right); - } -} +/* MIT License + * + * Copyright (c) 2023 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 RemoteControl application + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "App.h" +#include "StartupState.h" +#include "RemoteCtrlState.h" +#include +#include +#include +#include +#include +#include +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +static void App_cmdChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData); +static void App_motorSpeedsChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData); + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/** Only in remote control state its possible to control the robot. */ +static bool gIsRemoteCtrlActive = false; + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void App::setup() +{ + Serial.begin(SERIAL_BAUDRATE); + Logging::disable(); + Board::getInstance().init(); + + m_systemStateMachine.setState(&StartupState::getInstance()); + m_controlInterval.start(DIFFERENTIAL_DRIVE_CONTROL_PERIOD); + m_sendLineSensorsDataInterval.start(SEND_LINE_SENSORS_DATA_PERIOD); + + /* Remote control commands/responses */ + m_smpServer.subscribeToChannel(COMMAND_CHANNEL_NAME, App_cmdChannelCallback); + m_smpChannelIdRemoteCtrlRsp = + m_smpServer.createChannel(COMMAND_RESPONSE_CHANNEL_NAME, COMMAND_RESPONSE_CHANNEL_DLC); + + /* Receiving linear motor speed left/right */ + m_smpServer.subscribeToChannel(SPEED_SETPOINT_CHANNEL_NAME, App_motorSpeedsChannelCallback); + + /* Providing line sensor data */ + m_smpChannelIdLineSensors = m_smpServer.createChannel(LINE_SENSOR_CHANNEL_NAME, LINE_SENSOR_CHANNEL_DLC); +} + +void App::loop() +{ + m_smpServer.process(millis()); + Speedometer::getInstance().process(); + + if (true == m_controlInterval.isTimeout()) + { + 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(); + + m_controlInterval.restart(); + } + + m_systemStateMachine.process(); + + /* Determine whether the robot can be remote controlled or not. */ + if (&RemoteCtrlState::getInstance() == m_systemStateMachine.getState()) + { + gIsRemoteCtrlActive = true; + } + else + { + gIsRemoteCtrlActive = false; + } + + /* Send periodically line sensor data. */ + if (true == m_sendLineSensorsDataInterval.isTimeout()) + { + sendLineSensorsData(); + + m_sendLineSensorsDataInterval.restart(); + } + + /* Send remote control command responses. */ + sendRemoteControlResponses(); +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +void App::sendRemoteControlResponses() +{ + RemoteCtrlState::RspId remoteControlRspId = RemoteCtrlState::getInstance().getCmdRsp(); + + /* Send only on change. */ + if (remoteControlRspId != m_lastRemoteControlRspId) + { + const CommandResponse* payload = reinterpret_cast(&remoteControlRspId); + + (void)m_smpServer.sendData(m_smpChannelIdRemoteCtrlRsp, reinterpret_cast(&payload), sizeof(payload)); + + m_lastRemoteControlRspId = remoteControlRspId; + } +} + +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_smpChannelIdLineSensors, reinterpret_cast(&payload), sizeof(payload)); +} + +/****************************************************************************** + * 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) +{ + (void)userData; + if ((nullptr != payload) && (sizeof(RemoteCtrlState::CmdId) == payloadSize)) + { + RemoteCtrlState::CmdId cmdId = *reinterpret_cast(payload); + + RemoteCtrlState::getInstance().execute(cmdId); + } +} + +/** + * Receives motor speeds over SerialMuxProt channel. + * + * @param[in] payload Motor speed left/right + * @param[in] payloadSize Size of twice motor speeds + * @param[in] userData User data + */ +static void App_motorSpeedsChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData) +{ + (void)userData; + if ((nullptr != payload) && (SPEED_SETPOINT_CHANNEL_DLC == payloadSize) && (true == gIsRemoteCtrlActive)) + { + const SpeedData* motorSpeedData = reinterpret_cast(payload); + DifferentialDrive::getInstance().setLinearSpeed(motorSpeedData->left, motorSpeedData->right); + } +} diff --git a/lib/APPRemoteControl/App.h b/lib/APPRemoteControl/App.h index 09f16d7b..174af166 100644 --- a/lib/APPRemoteControl/App.h +++ b/lib/APPRemoteControl/App.h @@ -1,150 +1,150 @@ -/* MIT License - * - * Copyright (c) 2023 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 RemoteControl application - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef APP_H -#define APP_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include -#include "SerialMuxChannels.h" -#include "RemoteCtrlState.h" - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The remote control application. */ -class App -{ -public: - /** - * Construct the remote control application. - */ - App() : - m_systemStateMachine(), - m_controlInterval(), - m_sendLineSensorsDataInterval(), - m_smpServer(Serial), - m_smpChannelIdRemoteCtrlRsp(0U), - m_smpChannelIdLineSensors(0U), - m_lastRemoteControlRspId(RemoteCtrlState::RSP_ID_OK) - { - } - - /** - * Destroy the remote control application. - */ - ~App() - { - } - - /** - * Setup the application. - */ - void setup(); - - /** - * Process the application periodically. - */ - void loop(); - -private: - /** Baudrate for Serial Communication */ - static const uint32_t SERIAL_BAUDRATE = 115200U; - - /** Differential drive control period in ms. */ - static const uint32_t DIFFERENTIAL_DRIVE_CONTROL_PERIOD = 5; - - /** Sending Data period in ms. */ - static const uint32_t SEND_LINE_SENSORS_DATA_PERIOD = 20; - - /** The system state machine. */ - StateMachine m_systemStateMachine; - - /** Timer used for differential drive control processing. */ - SimpleTimer m_controlInterval; - - /** Timer used for sending data periodically. */ - SimpleTimer m_sendLineSensorsDataInterval; - - /** - * SerialMuxProt Server Instance - * - * @tparam tMaxChannels set to MAX_CHANNELS, defined in SerialMuxChannels.h. - */ - SerialMuxProtServer m_smpServer; - - /** Channel id sending remote control command responses. */ - uint8_t m_smpChannelIdRemoteCtrlRsp; - - /** Channel id sending line sensors data. */ - uint8_t m_smpChannelIdLineSensors; - - /** Last remote control response id */ - RemoteCtrlState::RspId m_lastRemoteControlRspId; - - /* Not allowed. */ - App(const App& app); /**< Copy construction of an instance. */ - App& operator=(const App& app); /**< Assignment of an instance. */ - - /** - * Send remote control command responses on change. - */ - void sendRemoteControlResponses(); - - /** - * Send line sensors data via SerialMuxProt. - */ - void sendLineSensorsData() const; -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* APP_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 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 RemoteControl application + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef APP_H +#define APP_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include +#include "SerialMuxChannels.h" +#include "RemoteCtrlState.h" + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The remote control application. */ +class App +{ +public: + /** + * Construct the remote control application. + */ + App() : + m_systemStateMachine(), + m_controlInterval(), + m_sendLineSensorsDataInterval(), + m_smpServer(Serial), + m_smpChannelIdRemoteCtrlRsp(0U), + m_smpChannelIdLineSensors(0U), + m_lastRemoteControlRspId(RemoteCtrlState::RSP_ID_OK) + { + } + + /** + * Destroy the remote control application. + */ + ~App() + { + } + + /** + * Setup the application. + */ + void setup(); + + /** + * Process the application periodically. + */ + void loop(); + +private: + /** Baudrate for Serial Communication */ + static const uint32_t SERIAL_BAUDRATE = 115200U; + + /** Differential drive control period in ms. */ + static const uint32_t DIFFERENTIAL_DRIVE_CONTROL_PERIOD = 5; + + /** Sending Data period in ms. */ + static const uint32_t SEND_LINE_SENSORS_DATA_PERIOD = 20; + + /** The system state machine. */ + StateMachine m_systemStateMachine; + + /** Timer used for differential drive control processing. */ + SimpleTimer m_controlInterval; + + /** Timer used for sending data periodically. */ + SimpleTimer m_sendLineSensorsDataInterval; + + /** + * SerialMuxProt Server Instance + * + * @tparam tMaxChannels set to MAX_CHANNELS, defined in SerialMuxChannels.h. + */ + SerialMuxProtServer m_smpServer; + + /** Channel id sending remote control command responses. */ + uint8_t m_smpChannelIdRemoteCtrlRsp; + + /** Channel id sending line sensors data. */ + uint8_t m_smpChannelIdLineSensors; + + /** Last remote control response id */ + RemoteCtrlState::RspId m_lastRemoteControlRspId; + + /* Not allowed. */ + App(const App& app); /**< Copy construction of an instance. */ + App& operator=(const App& app); /**< Assignment of an instance. */ + + /** + * Send remote control command responses on change. + */ + void sendRemoteControlResponses(); + + /** + * Send line sensors data via SerialMuxProt. + */ + void sendLineSensorsData() const; +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* APP_H */ +/** @} */ diff --git a/lib/APPRemoteControl/ErrorState.cpp b/lib/APPRemoteControl/ErrorState.cpp index 111e7aff..2bbc056a 100644 --- a/lib/APPRemoteControl/ErrorState.cpp +++ b/lib/APPRemoteControl/ErrorState.cpp @@ -1,118 +1,118 @@ -/* MIT License - * - * Copyright (c) 2023 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 "MotorSpeedCalibrationState.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void ErrorState::entry() -{ - IDisplay& display = Board::getInstance().getDisplay(); - - display.clear(); - display.print("Error"); - display.gotoXY(0, 1); - display.print(m_errorMsg); -} - -void ErrorState::process(StateMachine& sm) -{ - IButton& buttonA = Board::getInstance().getButtonA(); - - /* Restart calibration? */ - if (true == buttonA.isPressed()) - { - buttonA.waitForRelease(); - sm.setState(&MotorSpeedCalibrationState::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 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 "MotorSpeedCalibrationState.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void ErrorState::entry() +{ + IDisplay& display = Board::getInstance().getDisplay(); + + display.clear(); + display.print("Error"); + display.gotoXY(0, 1); + display.print(m_errorMsg); +} + +void ErrorState::process(StateMachine& sm) +{ + IButton& buttonA = Board::getInstance().getButtonA(); + + /* Restart calibration? */ + if (true == buttonA.isPressed()) + { + buttonA.waitForRelease(); + sm.setState(&MotorSpeedCalibrationState::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/APPRemoteControl/ErrorState.h b/lib/APPRemoteControl/ErrorState.h index 4f9c592e..3ded9542 100644 --- a/lib/APPRemoteControl/ErrorState.h +++ b/lib/APPRemoteControl/ErrorState.h @@ -1,134 +1,134 @@ -/* MIT License - * - * Copyright (c) 2023 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 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 */ +/** @} */ diff --git a/lib/APPRemoteControl/LineSensorsCalibrationState.cpp b/lib/APPRemoteControl/LineSensorsCalibrationState.cpp index 39429055..65ff0e9d 100644 --- a/lib/APPRemoteControl/LineSensorsCalibrationState.cpp +++ b/lib/APPRemoteControl/LineSensorsCalibrationState.cpp @@ -1,209 +1,209 @@ -/* MIT License - * - * Copyright (c) 2023 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 "RemoteCtrlState.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(); - - display.clear(); - display.print("Calib"); - display.gotoXY(0, 1); - display.print("LineS"); - - /* Prepare calibration drive. */ - m_calibrationSpeed = diffDrive.getMaxMotorSpeed() / 3; - m_orientation = odometry.getOrientation(); - - /* 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, "Cal=", 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(&RemoteCtrlState::getInstance()); - } -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 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 "RemoteCtrlState.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(); + + display.clear(); + display.print("Calib"); + display.gotoXY(0, 1); + display.print("LineS"); + + /* Prepare calibration drive. */ + m_calibrationSpeed = diffDrive.getMaxMotorSpeed() / 3; + m_orientation = odometry.getOrientation(); + + /* 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, "Cal=", 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(&RemoteCtrlState::getInstance()); + } +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPRemoteControl/LineSensorsCalibrationState.h b/lib/APPRemoteControl/LineSensorsCalibrationState.h index 075577f1..3f695436 100644 --- a/lib/APPRemoteControl/LineSensorsCalibrationState.h +++ b/lib/APPRemoteControl/LineSensorsCalibrationState.h @@ -1,161 +1,161 @@ -/* MIT License - * - * Copyright (c) 2023 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 60°). - */ - static const int32_t CALIB_ANGLE = (FP_2PI() / 6); - - 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 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 60°). + */ + static const int32_t CALIB_ANGLE = (FP_2PI() / 6); + + 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/APPRemoteControl/MotorSpeedCalibrationState.cpp b/lib/APPRemoteControl/MotorSpeedCalibrationState.cpp index 874d0295..de2bacad 100644 --- a/lib/APPRemoteControl/MotorSpeedCalibrationState.cpp +++ b/lib/APPRemoteControl/MotorSpeedCalibrationState.cpp @@ -1,234 +1,234 @@ -/* MIT License - * - * Copyright (c) 2023 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 "RemoteCtrlState.h" -#include "ErrorState.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/** - * Logging source. - */ -LOG_TAG("MSCState"); - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void MotorSpeedCalibrationState::entry() -{ - IDisplay& display = Board::getInstance().getDisplay(); - - display.clear(); - display.print("Calib"); - display.gotoXY(0, 1); - display.print("MSpeed"); - - /* 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(); - - /* 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; - - /* 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("MS=0"); - sm.setState(&ErrorState::getInstance()); - } - else - { - LOG_INFO_VAL("Calibrated max. speed (steps/s): ", maxSpeed); - LOG_INFO_VAL("Calibrated max. speed (mm/s): ", maxSpeed / RobotConstants::ENCODER_STEPS_PER_MM); - - sm.setState(&RemoteCtrlState::getInstance()); - } -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 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 "RemoteCtrlState.h" +#include "ErrorState.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/** + * Logging source. + */ +LOG_TAG("MSCState"); + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void MotorSpeedCalibrationState::entry() +{ + IDisplay& display = Board::getInstance().getDisplay(); + + display.clear(); + display.print("Calib"); + display.gotoXY(0, 1); + display.print("MSpeed"); + + /* 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(); + + /* 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; + + /* 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("MS=0"); + sm.setState(&ErrorState::getInstance()); + } + else + { + LOG_INFO_VAL("Calibrated max. speed (steps/s): ", maxSpeed); + LOG_INFO_VAL("Calibrated max. speed (mm/s): ", maxSpeed / RobotConstants::ENCODER_STEPS_PER_MM); + + sm.setState(&RemoteCtrlState::getInstance()); + } +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPRemoteControl/MotorSpeedCalibrationState.h b/lib/APPRemoteControl/MotorSpeedCalibrationState.h index 4dbca55e..356d0204 100644 --- a/lib/APPRemoteControl/MotorSpeedCalibrationState.h +++ b/lib/APPRemoteControl/MotorSpeedCalibrationState.h @@ -1,165 +1,165 @@ -/* MIT License - * - * Copyright (c) 2023 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 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/APPRemoteControl/RemoteCtrlState.cpp b/lib/APPRemoteControl/RemoteCtrlState.cpp index b379a645..1b226b09 100644 --- a/lib/APPRemoteControl/RemoteCtrlState.cpp +++ b/lib/APPRemoteControl/RemoteCtrlState.cpp @@ -1,129 +1,129 @@ -/* MIT License - * - * Copyright (c) 2023 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 Remote control state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "RemoteCtrlState.h" -#include -#include -#include -#include "LineSensorsCalibrationState.h" -#include "MotorSpeedCalibrationState.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void RemoteCtrlState::entry() -{ - DifferentialDrive::getInstance().enable(); - - /* It is assumed that by entering this state, that a pending command will be complete. */ - finishCommand(RSP_ID_OK); -} - -void RemoteCtrlState::process(StateMachine& sm) -{ - switch(m_cmdId) - { - case CMD_ID_IDLE: - /* Nothing to do. */ - break; - - case CMD_ID_START_LINE_SENSOR_CALIB: - sm.setState(&LineSensorsCalibrationState::getInstance()); - break; - - case CMD_ID_START_MOTOR_SPEED_CALIB: - sm.setState(&MotorSpeedCalibrationState::getInstance()); - break; - - case CMD_ID_REINIT_BOARD: - /* Ensure that the motors are stopped, before re-initialize the board. */ - DifferentialDrive::getInstance().setLinearSpeed(0, 0); - - /* 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. - */ - Board::getInstance().init(); - - finishCommand(RSP_ID_OK); - break; - - default: - break; - } -} - -void RemoteCtrlState::exit() -{ - DifferentialDrive::getInstance().disable(); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ - - +/* MIT License + * + * Copyright (c) 2023 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 Remote control state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "RemoteCtrlState.h" +#include +#include +#include +#include "LineSensorsCalibrationState.h" +#include "MotorSpeedCalibrationState.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void RemoteCtrlState::entry() +{ + DifferentialDrive::getInstance().enable(); + + /* It is assumed that by entering this state, that a pending command will be complete. */ + finishCommand(RSP_ID_OK); +} + +void RemoteCtrlState::process(StateMachine& sm) +{ + switch(m_cmdId) + { + case CMD_ID_IDLE: + /* Nothing to do. */ + break; + + case CMD_ID_START_LINE_SENSOR_CALIB: + sm.setState(&LineSensorsCalibrationState::getInstance()); + break; + + case CMD_ID_START_MOTOR_SPEED_CALIB: + sm.setState(&MotorSpeedCalibrationState::getInstance()); + break; + + case CMD_ID_REINIT_BOARD: + /* Ensure that the motors are stopped, before re-initialize the board. */ + DifferentialDrive::getInstance().setLinearSpeed(0, 0); + + /* 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. + */ + Board::getInstance().init(); + + finishCommand(RSP_ID_OK); + break; + + default: + break; + } +} + +void RemoteCtrlState::exit() +{ + DifferentialDrive::getInstance().disable(); +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ + + diff --git a/lib/APPRemoteControl/RemoteCtrlState.h b/lib/APPRemoteControl/RemoteCtrlState.h index e45e1913..dedaeb7a 100644 --- a/lib/APPRemoteControl/RemoteCtrlState.h +++ b/lib/APPRemoteControl/RemoteCtrlState.h @@ -1,175 +1,175 @@ -/* MIT License - * - * Copyright (c) 2023 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 Remote control state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef REMOTE_CTRL_H -#define REMOTE_CTRL_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The system remote control state. */ -class RemoteCtrlState : public IState -{ -public: - /** Remote control commands. */ - typedef enum : uint8_t - { - CMD_ID_IDLE = 0, /**< Nothing to do. */ - CMD_ID_START_LINE_SENSOR_CALIB, /**< Start line sensor calibration. */ - CMD_ID_START_MOTOR_SPEED_CALIB, /**< Start motor speed calibration. */ - CMD_ID_REINIT_BOARD /**< Re-initialize the board. Required for webots simulation. */ - - } CmdId; - - /** 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; - - /** - * Get state instance. - * - * @return State instance. - */ - static RemoteCtrlState& getInstance() - { - static RemoteCtrlState 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; - - /** - * Execute command. - * If a command is pending, it won't be executed. - * - * @param[in] cmdId The id of the command which to execute. - */ - void execute(CmdId cmdId) - { - if (CMD_ID_IDLE == m_cmdId) - { - m_cmdId = cmdId; - m_rspId = RSP_ID_PENDING; - } - } - - /** - * Get command response of current command. - * - * @return Command response - */ - RspId getCmdRsp() const - { - return m_rspId; - } - -protected: -private: - CmdId m_cmdId; /**< Current pending command. */ - RspId m_rspId; /**< Current command response. */ - - /** - * Default constructor. - */ - RemoteCtrlState() : m_cmdId(CMD_ID_IDLE), m_rspId(RSP_ID_OK) - { - } - - /** - * Default destructor. - */ - ~RemoteCtrlState() - { - } - - RemoteCtrlState(const RemoteCtrlState& state); /**< Copy construction of an instance. */ - RemoteCtrlState& operator=(const RemoteCtrlState& state); /**< Assignment of an instance. */ - - /** - * Set command response and finish the command execution. - * - * @param[in] rsp Command response - */ - void finishCommand(RspId rsp) - { - m_rspId = rsp; - m_cmdId = CMD_ID_IDLE; - } -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* REMOTE_CTRL_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 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 Remote control state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef REMOTE_CTRL_H +#define REMOTE_CTRL_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The system remote control state. */ +class RemoteCtrlState : public IState +{ +public: + /** Remote control commands. */ + typedef enum : uint8_t + { + CMD_ID_IDLE = 0, /**< Nothing to do. */ + CMD_ID_START_LINE_SENSOR_CALIB, /**< Start line sensor calibration. */ + CMD_ID_START_MOTOR_SPEED_CALIB, /**< Start motor speed calibration. */ + CMD_ID_REINIT_BOARD /**< Re-initialize the board. Required for webots simulation. */ + + } CmdId; + + /** 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; + + /** + * Get state instance. + * + * @return State instance. + */ + static RemoteCtrlState& getInstance() + { + static RemoteCtrlState 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; + + /** + * Execute command. + * If a command is pending, it won't be executed. + * + * @param[in] cmdId The id of the command which to execute. + */ + void execute(CmdId cmdId) + { + if (CMD_ID_IDLE == m_cmdId) + { + m_cmdId = cmdId; + m_rspId = RSP_ID_PENDING; + } + } + + /** + * Get command response of current command. + * + * @return Command response + */ + RspId getCmdRsp() const + { + return m_rspId; + } + +protected: +private: + CmdId m_cmdId; /**< Current pending command. */ + RspId m_rspId; /**< Current command response. */ + + /** + * Default constructor. + */ + RemoteCtrlState() : m_cmdId(CMD_ID_IDLE), m_rspId(RSP_ID_OK) + { + } + + /** + * Default destructor. + */ + ~RemoteCtrlState() + { + } + + RemoteCtrlState(const RemoteCtrlState& state); /**< Copy construction of an instance. */ + RemoteCtrlState& operator=(const RemoteCtrlState& state); /**< Assignment of an instance. */ + + /** + * Set command response and finish the command execution. + * + * @param[in] rsp Command response + */ + void finishCommand(RspId rsp) + { + m_rspId = rsp; + m_cmdId = CMD_ID_IDLE; + } +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* REMOTE_CTRL_H */ +/** @} */ diff --git a/lib/APPRemoteControl/SerialMuxChannels.h b/lib/APPRemoteControl/SerialMuxChannels.h index 4380af6f..caeaddc1 100644 --- a/lib/APPRemoteControl/SerialMuxChannels.h +++ b/lib/APPRemoteControl/SerialMuxChannels.h @@ -1,105 +1,105 @@ -/* MIT License - * - * Copyright (c) 2023 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 - -/****************************************************************************** - * 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 Motor Speed Setpoints to. */ -#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)) - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** Struct of the "Command" channel payload. */ -typedef struct _Command -{ - uint8_t commandId; /**< Command ID */ -} __attribute__((packed)) Command; - -/** Struct of the "Command Response" channel payload. */ -typedef struct _CommandResponse -{ - uint8_t response; /**< Response to the command */ -} __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 "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 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 + +/****************************************************************************** + * 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 Motor Speed Setpoints to. */ +#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)) + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** Struct of the "Command" channel payload. */ +typedef struct _Command +{ + uint8_t commandId; /**< Command ID */ +} __attribute__((packed)) Command; + +/** Struct of the "Command Response" channel payload. */ +typedef struct _CommandResponse +{ + uint8_t response; /**< Response to the command */ +} __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 "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/APPRemoteControl/StartupState.cpp b/lib/APPRemoteControl/StartupState.cpp index d28652f1..b3441294 100644 --- a/lib/APPRemoteControl/StartupState.cpp +++ b/lib/APPRemoteControl/StartupState.cpp @@ -1,102 +1,102 @@ -/* MIT License - * - * Copyright (c) 2023 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 "RemoteCtrlState.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void StartupState::entry() -{ - IDisplay& display = Board::getInstance().getDisplay(); - - /* Initialize HAL */ - Board::getInstance().init(); - - display.clear(); - display.print("Remote"); - display.gotoXY(0, 1); - display.print("Ctrl"); - delay(APP_NAME_DURATION); -} - -void StartupState::process(StateMachine& sm) -{ - sm.setState(&RemoteCtrlState::getInstance()); -} - -void StartupState::exit() -{ - /* Nothing to do */ -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 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 "RemoteCtrlState.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void StartupState::entry() +{ + IDisplay& display = Board::getInstance().getDisplay(); + + /* Initialize HAL */ + Board::getInstance().init(); + + display.clear(); + display.print("Remote"); + display.gotoXY(0, 1); + display.print("Ctrl"); + delay(APP_NAME_DURATION); +} + +void StartupState::process(StateMachine& sm) +{ + sm.setState(&RemoteCtrlState::getInstance()); +} + +void StartupState::exit() +{ + /* Nothing to do */ +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPRemoteControl/StartupState.h b/lib/APPRemoteControl/StartupState.h index 09ad0d3c..c58233fb 100644 --- a/lib/APPRemoteControl/StartupState.h +++ b/lib/APPRemoteControl/StartupState.h @@ -1,123 +1,123 @@ -/* MIT License - * - * Copyright (c) 2023 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 - -/****************************************************************************** - * 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: - /** - * Duration in ms how long the application name shall be shown at startup. - */ - static const uint32_t APP_NAME_DURATION = 2000; - - /** - * Default constructor. - */ - StartupState() - { - } - - /** - * Default destructor. - */ - ~StartupState() - { - } - - /* Not allowed. */ - StartupState(const StartupState& state); /**< Copy construction of an instance. */ - StartupState& operator=(const StartupState& state); /**< Assignment of an instance. */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* STARTUP_STATE_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 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 + +/****************************************************************************** + * 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: + /** + * Duration in ms how long the application name shall be shown at startup. + */ + static const uint32_t APP_NAME_DURATION = 2000; + + /** + * Default constructor. + */ + StartupState() + { + } + + /** + * Default destructor. + */ + ~StartupState() + { + } + + /* Not allowed. */ + StartupState(const StartupState& state); /**< Copy construction of an instance. */ + StartupState& operator=(const StartupState& state); /**< Assignment of an instance. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* STARTUP_STATE_H */ +/** @} */ diff --git a/lib/APPRemoteControl/library.json b/lib/APPRemoteControl/library.json index 490e3052..efcce554 100644 --- a/lib/APPRemoteControl/library.json +++ b/lib/APPRemoteControl/library.json @@ -1,29 +1,29 @@ -{ - "name": "APPRemoteControl", - "version": "0.1.0", - "description": "RemoteControl application", - "authors": [ - { - "name": "Andreas Merkle", - "email": "web@blue-andi.de", - "url": "https://github.com/BlueAndi", - "maintainer": true - } - ], - "license": "MIT", - "dependencies": [ - { - "name": "Service" - }, - { - "name": "HALInterfaces" - }, - { - "owner": "gabryelreyes", - "name": "SerialMuxProt", - "version": "^2.0.0" - } - ], - "frameworks": "*", - "platforms": "*" +{ + "name": "APPRemoteControl", + "version": "0.1.0", + "description": "RemoteControl application", + "authors": [ + { + "name": "Andreas Merkle", + "email": "web@blue-andi.de", + "url": "https://github.com/BlueAndi", + "maintainer": true + } + ], + "license": "MIT", + "dependencies": [ + { + "name": "Service" + }, + { + "name": "HALInterfaces" + }, + { + "owner": "gabryelreyes", + "name": "SerialMuxProt", + "version": "^2.0.0" + } + ], + "frameworks": "*", + "platforms": "*" } \ No newline at end of file diff --git a/lib/APPSensorFusion/App.cpp b/lib/APPSensorFusion/App.cpp index ce7b0888..8f6f66ff 100644 --- a/lib/APPSensorFusion/App.cpp +++ b/lib/APPSensorFusion/App.cpp @@ -1,192 +1,192 @@ -/* MIT License - * - * Copyright (c) 2023 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 SensorFusion application - * @author Juliane Kerpe - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "App.h" -#include "StartupState.h" -#include -#include -#include -#include -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void App::setup() -{ - Serial.begin(SERIAL_BAUDRATE); - Board::getInstance().init(); - - m_systemStateMachine.setState(&StartupState::getInstance()); - m_controlInterval.start(DIFFERENTIAL_DRIVE_CONTROL_PERIOD); - - m_sendSensorsDataInterval.start(SEND_SENSORS_DATA_PERIOD); - - /* Providing Sensor data */ - m_smpChannelIdSensorData = m_smpServer.createChannel(SENSORDATA_CHANNEL_NAME, SENSORDATA_CHANNEL_DLC); - - /* Sending End Line Detection signal */ - m_smpChannelIdEndLine = m_smpServer.createChannel(ENDLINE_CHANNEL_NAME, ENDLINE_CHANNEL_DLC); -} - -void App::loop() -{ - m_smpServer.process(millis()); - Speedometer::getInstance().process(); - IIMU& imu = Board::getInstance().getIMU(); - - 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(); - - m_controlInterval.restart(); - } - - /* Send sensor data periodically if new data is available. */ - if ((true == m_sendSensorsDataInterval.isTimeout()) && (true == imu.accelerometerDataReady()) && - (true == imu.gyroDataReady()) && (true == imu.magnetometerDataReady())) - { - m_sendSensorsDataInterval.restart(); - sendSensorData(); - - /* Send End line detection signal if the application is currently in the Driving state. */ - if (&DrivingState::getInstance() == m_systemStateMachine.getState()) - { - sendEndLineDetectionSignal(); - } - } - m_systemStateMachine.process(); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -void App::sendSensorData() const -{ - SensorData payload; - IIMU& imu = Board::getInstance().getIMU(); - Odometry& odometry = Odometry::getInstance(); - int32_t positionOdometryX; - int32_t positionOdometryY; - - /* Get the current values from the Odometry. */ - odometry.getPosition(positionOdometryX, positionOdometryY); - - /* Read the accelerometer. */ - IMUData accelerationValues; - imu.readAccelerometer(); - imu.getAccelerationValues(&accelerationValues); - - /* Read the magnetometer. */ - IMUData magnetometerValues; - imu.readMagnetometer(); - imu.getMagnetometerValues(&magnetometerValues); - - /* Read the gyro. */ - IMUData turnRates; - imu.readGyro(); - imu.getTurnRates(&turnRates); - - /* Write the sensor data in the SensorData Struct. */ - payload.positionOdometryX = positionOdometryX; - payload.positionOdometryY = positionOdometryY; - payload.orientationOdometry = odometry.getOrientation(); - payload.accelerationX = accelerationValues.valueX; - payload.accelerationY = accelerationValues.valueY; - payload.magnetometerValueX = magnetometerValues.valueX; - payload.magnetometerValueY = magnetometerValues.valueY; - payload.turnRate = turnRates.valueZ; - - /* Send the sensor data via the SerialMuxProt. */ - (void)m_smpServer.sendData(m_smpChannelIdSensorData, reinterpret_cast(&payload), sizeof(payload)); -} - -void App::sendEndLineDetectionSignal() -{ - DrivingState::LineStatus lineStatus = DrivingState::getInstance().getLineStatus(); - - /* Send only if a new End Line has been detected. */ - if ((DrivingState::LINE_STATUS_FIND_END_LINE == m_lastLineDetectionStatus) && - (DrivingState::LINE_STATUS_END_LINE_DETECTED == lineStatus)) - { - EndLineFlag payload = {.isEndLineDetected = true}; - (void)m_smpServer.sendData(m_smpChannelIdEndLine, reinterpret_cast(&payload), sizeof(payload)); - } - - m_lastLineDetectionStatus = lineStatus; -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions +/* MIT License + * + * Copyright (c) 2023 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 SensorFusion application + * @author Juliane Kerpe + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "App.h" +#include "StartupState.h" +#include +#include +#include +#include +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void App::setup() +{ + Serial.begin(SERIAL_BAUDRATE); + Board::getInstance().init(); + + m_systemStateMachine.setState(&StartupState::getInstance()); + m_controlInterval.start(DIFFERENTIAL_DRIVE_CONTROL_PERIOD); + + m_sendSensorsDataInterval.start(SEND_SENSORS_DATA_PERIOD); + + /* Providing Sensor data */ + m_smpChannelIdSensorData = m_smpServer.createChannel(SENSORDATA_CHANNEL_NAME, SENSORDATA_CHANNEL_DLC); + + /* Sending End Line Detection signal */ + m_smpChannelIdEndLine = m_smpServer.createChannel(ENDLINE_CHANNEL_NAME, ENDLINE_CHANNEL_DLC); +} + +void App::loop() +{ + m_smpServer.process(millis()); + Speedometer::getInstance().process(); + IIMU& imu = Board::getInstance().getIMU(); + + 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(); + + m_controlInterval.restart(); + } + + /* Send sensor data periodically if new data is available. */ + if ((true == m_sendSensorsDataInterval.isTimeout()) && (true == imu.accelerometerDataReady()) && + (true == imu.gyroDataReady()) && (true == imu.magnetometerDataReady())) + { + m_sendSensorsDataInterval.restart(); + sendSensorData(); + + /* Send End line detection signal if the application is currently in the Driving state. */ + if (&DrivingState::getInstance() == m_systemStateMachine.getState()) + { + sendEndLineDetectionSignal(); + } + } + m_systemStateMachine.process(); +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +void App::sendSensorData() const +{ + SensorData payload; + IIMU& imu = Board::getInstance().getIMU(); + Odometry& odometry = Odometry::getInstance(); + int32_t positionOdometryX; + int32_t positionOdometryY; + + /* Get the current values from the Odometry. */ + odometry.getPosition(positionOdometryX, positionOdometryY); + + /* Read the accelerometer. */ + IMUData accelerationValues; + imu.readAccelerometer(); + imu.getAccelerationValues(&accelerationValues); + + /* Read the magnetometer. */ + IMUData magnetometerValues; + imu.readMagnetometer(); + imu.getMagnetometerValues(&magnetometerValues); + + /* Read the gyro. */ + IMUData turnRates; + imu.readGyro(); + imu.getTurnRates(&turnRates); + + /* Write the sensor data in the SensorData Struct. */ + payload.positionOdometryX = positionOdometryX; + payload.positionOdometryY = positionOdometryY; + payload.orientationOdometry = odometry.getOrientation(); + payload.accelerationX = accelerationValues.valueX; + payload.accelerationY = accelerationValues.valueY; + payload.magnetometerValueX = magnetometerValues.valueX; + payload.magnetometerValueY = magnetometerValues.valueY; + payload.turnRate = turnRates.valueZ; + + /* Send the sensor data via the SerialMuxProt. */ + (void)m_smpServer.sendData(m_smpChannelIdSensorData, reinterpret_cast(&payload), sizeof(payload)); +} + +void App::sendEndLineDetectionSignal() +{ + DrivingState::LineStatus lineStatus = DrivingState::getInstance().getLineStatus(); + + /* Send only if a new End Line has been detected. */ + if ((DrivingState::LINE_STATUS_FIND_END_LINE == m_lastLineDetectionStatus) && + (DrivingState::LINE_STATUS_END_LINE_DETECTED == lineStatus)) + { + EndLineFlag payload = {.isEndLineDetected = true}; + (void)m_smpServer.sendData(m_smpChannelIdEndLine, reinterpret_cast(&payload), sizeof(payload)); + } + + m_lastLineDetectionStatus = lineStatus; +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions *****************************************************************************/ \ No newline at end of file diff --git a/lib/APPSensorFusion/App.h b/lib/APPSensorFusion/App.h index 10fdaab7..0ad2881b 100644 --- a/lib/APPSensorFusion/App.h +++ b/lib/APPSensorFusion/App.h @@ -1,152 +1,152 @@ -/* MIT License - * - * Copyright (c) 2023 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 SensorFusion application - * @author Juliane Kerpe - * - * @addtogroup Application - * - * @{ - */ - -#ifndef APP_H -#define APP_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include -#include -#include "SerialMuxChannels.h" -#include "DrivingState.h" - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The SensorFusion application. */ -class App -{ -public: - /** - * Construct the SensorFusion application. - */ - App() : - m_smpChannelIdSensorData(0U), - m_smpChannelIdEndLine(0U), - m_systemStateMachine(), - m_controlInterval(), - m_sendSensorsDataInterval(), - m_lastLineDetectionStatus(DrivingState::LINE_STATUS_FIND_START_LINE), - m_smpServer(Serial) - { - } - - /** - * Destroy the SensorFusion application. - */ - ~App() - { - } - - /** - * Setup the application. - */ - void setup(); - - /** - * Process the application periodically. - */ - void loop(); - -private: - /** Sending Data period in ms. */ - static const uint32_t SEND_SENSORS_DATA_PERIOD = 20; - - /** 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; - - /** Channel id for sending sensor data used for sensor fusion. */ - uint8_t m_smpChannelIdSensorData; - - /** Channel id for sending End Line Detection signal. */ - uint8_t m_smpChannelIdEndLine; - - /** The system state machine. */ - StateMachine m_systemStateMachine; - - /** Timer used for differential drive control processing. */ - SimpleTimer m_controlInterval; - - /** Timer used for sending data periodically. */ - SimpleTimer m_sendSensorsDataInterval; - - /** End Line Status of the previous iteration */ - DrivingState::LineStatus m_lastLineDetectionStatus; - - /** - * SerialMuxProt Server Instance - * - * @tparam tMaxChannels set to MAX_CHANNELS, defined in SerialMuxChannels.h. - */ - SerialMuxProtServer m_smpServer; - - /** - * Send the Sensor data as a SensorData struct via SerialMuxProt. - */ - void sendSensorData() const; - - /** - * Send the End Line Detection Flag as a EndLineFlag struct via SerialMuxProt. - * The Signal will only be sent if the a new End Line has been detected. - */ - void sendEndLineDetectionSignal(); - - /* Not allowed. */ - App(const App& app); /**< Copy construction of an instance. */ - App& operator=(const App& app); /**< Assignment of an instance. */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* APP_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 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 SensorFusion application + * @author Juliane Kerpe + * + * @addtogroup Application + * + * @{ + */ + +#ifndef APP_H +#define APP_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include +#include +#include "SerialMuxChannels.h" +#include "DrivingState.h" + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The SensorFusion application. */ +class App +{ +public: + /** + * Construct the SensorFusion application. + */ + App() : + m_smpChannelIdSensorData(0U), + m_smpChannelIdEndLine(0U), + m_systemStateMachine(), + m_controlInterval(), + m_sendSensorsDataInterval(), + m_lastLineDetectionStatus(DrivingState::LINE_STATUS_FIND_START_LINE), + m_smpServer(Serial) + { + } + + /** + * Destroy the SensorFusion application. + */ + ~App() + { + } + + /** + * Setup the application. + */ + void setup(); + + /** + * Process the application periodically. + */ + void loop(); + +private: + /** Sending Data period in ms. */ + static const uint32_t SEND_SENSORS_DATA_PERIOD = 20; + + /** 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; + + /** Channel id for sending sensor data used for sensor fusion. */ + uint8_t m_smpChannelIdSensorData; + + /** Channel id for sending End Line Detection signal. */ + uint8_t m_smpChannelIdEndLine; + + /** The system state machine. */ + StateMachine m_systemStateMachine; + + /** Timer used for differential drive control processing. */ + SimpleTimer m_controlInterval; + + /** Timer used for sending data periodically. */ + SimpleTimer m_sendSensorsDataInterval; + + /** End Line Status of the previous iteration */ + DrivingState::LineStatus m_lastLineDetectionStatus; + + /** + * SerialMuxProt Server Instance + * + * @tparam tMaxChannels set to MAX_CHANNELS, defined in SerialMuxChannels.h. + */ + SerialMuxProtServer m_smpServer; + + /** + * Send the Sensor data as a SensorData struct via SerialMuxProt. + */ + void sendSensorData() const; + + /** + * Send the End Line Detection Flag as a EndLineFlag struct via SerialMuxProt. + * The Signal will only be sent if the a new End Line has been detected. + */ + void sendEndLineDetectionSignal(); + + /* 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/APPSensorFusion/DrivingState.cpp b/lib/APPSensorFusion/DrivingState.cpp index 84aebe48..a74738ea 100644 --- a/lib/APPSensorFusion/DrivingState.cpp +++ b/lib/APPSensorFusion/DrivingState.cpp @@ -1,364 +1,364 @@ -/* MIT License - * - * Copyright (c) 2023 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" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void DrivingState::entry() -{ - 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_FIND_START_LINE; - m_trackStatus = TRACK_STATUS_ON_TRACK; /* Assume that the robot is placed on track. */ - - /* 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(true); -} - -void DrivingState::process(StateMachine& sm) -{ - ILineSensors& lineSensors = Board::getInstance().getLineSensors(); - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - int16_t position = 0; - - /* Get the position of the line. */ - position = lineSensors.readLine(); - - switch (m_trackStatus) - { - case TRACK_STATUS_ON_TRACK: - processOnTrack(position, lineSensors.getSensorValues()); - break; - - case TRACK_STATUS_LOST: - processTrackLost(position, lineSensors.getSensorValues()); - break; - - case TRACK_STATUS_FINISHED: - /* Change to ready state. */ - sm.setState(&ReadyState::getInstance()); - break; - - default: - /* Fatal error */ - diffDrive.setLinearSpeed(0, 0); - Sound::playAlarm(); - sm.setState(&ReadyState::getInstance()); - break; - } - - /* Max. time for finishing the track over? */ - if ((TRACK_STATUS_FINISHED != m_trackStatus) && (true == m_observationTimer.isTimeout())) - { - m_trackStatus = TRACK_STATUS_FINISHED; - - /* Stop motors immediately. Don't move this to a later position, - * as this would extend the driven length. - */ - diffDrive.setLinearSpeed(0, 0); - - Sound::playAlarm(); - } -} - -void DrivingState::exit() -{ - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - - diffDrive.disable(); - m_observationTimer.stop(); - Board::getInstance().getYellowLed().enable(false); -} - -/* PROTECTED METHODES *****************************************************************************/ - -/* PRIVATE METHODES *******************************************************************************/ - -void DrivingState::processOnTrack(int16_t position, const uint16_t* lineSensorValues) -{ - if (nullptr == lineSensorValues) - { - return; - } - - /* Track lost just in this moment? */ - if (true == isTrackGapDetected(position)) - { - m_trackStatus = TRACK_STATUS_LOST; - - /* Set mileage to 0, to be able to measure the max. distance, till - * the track must be found again. - */ - Odometry::getInstance().clearMileage(); - - /* Show the operator that the track is lost visual. */ - Board::getInstance().getYellowLed().enable(true); - } - else - { - /* Detect start-/endline */ - if (true == isStartEndLineDetected(lineSensorValues)) - { - /* Start line detected? */ - if (LINE_STATUS_FIND_START_LINE == m_lineStatus) - { - m_lineStatus = LINE_STATUS_START_LINE_DETECTED; - - Sound::playBeep(); - } - /* End line detected */ - else if (LINE_STATUS_FIND_END_LINE == m_lineStatus) - { - m_lineStatus = LINE_STATUS_END_LINE_DETECTED; - /* Don't switch to the Ready State and do nothing. */ - } - else - { - ; - } - } - else if (LINE_STATUS_START_LINE_DETECTED == m_lineStatus) - { - m_lineStatus = LINE_STATUS_FIND_END_LINE; - } - else - { - /* Be able to switch back to LINE_STATUS_FIND_END_LINE */ - m_lineStatus = LINE_STATUS_FIND_END_LINE; - } - - if (TRACK_STATUS_FINISHED != m_trackStatus) - { - if (true == m_pidProcessTime.isTimeout()) - { - adaptDriving(position); - - m_pidProcessTime.start(PID_PROCESS_PERIOD); - } - } - } -} - -void DrivingState::processTrackLost(int16_t position, const uint16_t* lineSensorValues) -{ - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - - if (nullptr == lineSensorValues) - { - return; - } - - /* Back on track? */ - if (false == isTrackGapDetected(position)) - { - m_trackStatus = TRACK_STATUS_ON_TRACK; - m_pidCtrl.resync(); - - Board::getInstance().getYellowLed().enable(false); - } - /* Max. distance driven, but track still not found? */ - else if (MAX_DISTANCE < Odometry::getInstance().getMileageCenter()) - { - /* Stop motors immediately. Don't move this to a later position, - * as this would extend the driven length. - */ - diffDrive.setLinearSpeed(0, 0); - - Sound::playAlarm(); - m_trackStatus = TRACK_STATUS_FINISHED; - } - else - { - /* Drive straight on. */ - diffDrive.setLinearSpeed(m_topSpeed, m_topSpeed); - } -} - -bool DrivingState::isStartEndLineDetected(const uint16_t* lineSensorValues) -{ - bool isDetected = false; - uint16_t leftSensor = lineSensorValues[0]; - uint16_t middleSensor = (lineSensorValues[1] + lineSensorValues[2] + lineSensorValues[3]) / 3; - uint16_t rightSensor = lineSensorValues[4]; - const uint8_t DEBOUNCE_CNT = 3; - const uint16_t LINE_SENSOR_OFF_TRACK_VALUE = 200; - - /* Note, the start-/end line detection must be debounced. Otherwise - * especially in low speed use cases, the line may be in one cycle - * detected, in the next not and then detected again. This would lead - * to a start line detection and afterwards to a end line detection, - * which would be wrong. - * - * Note the three sensors in the middle are handled as one sensor to - * avoid detection problems with different kind of line widths. - */ - if ((LINE_SENSOR_OFF_TRACK_VALUE <= leftSensor) && (LINE_SENSOR_OFF_TRACK_VALUE <= middleSensor) && - (LINE_SENSOR_OFF_TRACK_VALUE <= rightSensor)) - { - if (DEBOUNCE_CNT > m_startEndLineDebounce) - { - ++m_startEndLineDebounce; - } - - if (DEBOUNCE_CNT <= m_startEndLineDebounce) - { - isDetected = true; - } - } - else - { - m_startEndLineDebounce = 0; - } - - return isDetected; -} - -bool DrivingState::isTrackGapDetected(int16_t position) const -{ - bool isDetected = false; - const ILineSensors& lineSensors = Board::getInstance().getLineSensors(); - - /* Position value after loosing the track and sensor 0 saw it as last. - * It depends on the Zumo32U4LineSensors::readLine() implementation. - */ - const int16_t POS_MIN = 0; - - /* Position value after loosing the track and sensor N saw it as last. - * It depends on the Zumo32U4LineSensors::readLine() implementation. - */ - const int16_t POS_MAX = (lineSensors.getNumLineSensors() - 1) * 1000; - - /* Note, no debouncing is done here. If necessary, it shall be done - * outside this method. - */ - if ((POS_MIN >= position) || (POS_MAX <= position)) - { - isDetected = true; - } - - return isDetected; -} - -void DrivingState::adaptDriving(int16_t position) -{ - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - const ILineSensors& lineSensors = Board::getInstance().getLineSensors(); - const int16_t MAX_MOTOR_SPEED = diffDrive.getMaxMotorSpeed(); - 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(lineSensors.getSensorValueMax() * 2, 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, 0, MAX_MOTOR_SPEED); - rightSpeed = constrain(rightSpeed, 0, MAX_MOTOR_SPEED); - - diffDrive.setLinearSpeed(leftSpeed, rightSpeed); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 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" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void DrivingState::entry() +{ + 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_FIND_START_LINE; + m_trackStatus = TRACK_STATUS_ON_TRACK; /* Assume that the robot is placed on track. */ + + /* 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(true); +} + +void DrivingState::process(StateMachine& sm) +{ + ILineSensors& lineSensors = Board::getInstance().getLineSensors(); + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + int16_t position = 0; + + /* Get the position of the line. */ + position = lineSensors.readLine(); + + switch (m_trackStatus) + { + case TRACK_STATUS_ON_TRACK: + processOnTrack(position, lineSensors.getSensorValues()); + break; + + case TRACK_STATUS_LOST: + processTrackLost(position, lineSensors.getSensorValues()); + break; + + case TRACK_STATUS_FINISHED: + /* Change to ready state. */ + sm.setState(&ReadyState::getInstance()); + break; + + default: + /* Fatal error */ + diffDrive.setLinearSpeed(0, 0); + Sound::playAlarm(); + sm.setState(&ReadyState::getInstance()); + break; + } + + /* Max. time for finishing the track over? */ + if ((TRACK_STATUS_FINISHED != m_trackStatus) && (true == m_observationTimer.isTimeout())) + { + m_trackStatus = TRACK_STATUS_FINISHED; + + /* Stop motors immediately. Don't move this to a later position, + * as this would extend the driven length. + */ + diffDrive.setLinearSpeed(0, 0); + + Sound::playAlarm(); + } +} + +void DrivingState::exit() +{ + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + + diffDrive.disable(); + m_observationTimer.stop(); + Board::getInstance().getYellowLed().enable(false); +} + +/* PROTECTED METHODES *****************************************************************************/ + +/* PRIVATE METHODES *******************************************************************************/ + +void DrivingState::processOnTrack(int16_t position, const uint16_t* lineSensorValues) +{ + if (nullptr == lineSensorValues) + { + return; + } + + /* Track lost just in this moment? */ + if (true == isTrackGapDetected(position)) + { + m_trackStatus = TRACK_STATUS_LOST; + + /* Set mileage to 0, to be able to measure the max. distance, till + * the track must be found again. + */ + Odometry::getInstance().clearMileage(); + + /* Show the operator that the track is lost visual. */ + Board::getInstance().getYellowLed().enable(true); + } + else + { + /* Detect start-/endline */ + if (true == isStartEndLineDetected(lineSensorValues)) + { + /* Start line detected? */ + if (LINE_STATUS_FIND_START_LINE == m_lineStatus) + { + m_lineStatus = LINE_STATUS_START_LINE_DETECTED; + + Sound::playBeep(); + } + /* End line detected */ + else if (LINE_STATUS_FIND_END_LINE == m_lineStatus) + { + m_lineStatus = LINE_STATUS_END_LINE_DETECTED; + /* Don't switch to the Ready State and do nothing. */ + } + else + { + ; + } + } + else if (LINE_STATUS_START_LINE_DETECTED == m_lineStatus) + { + m_lineStatus = LINE_STATUS_FIND_END_LINE; + } + else + { + /* Be able to switch back to LINE_STATUS_FIND_END_LINE */ + m_lineStatus = LINE_STATUS_FIND_END_LINE; + } + + if (TRACK_STATUS_FINISHED != m_trackStatus) + { + if (true == m_pidProcessTime.isTimeout()) + { + adaptDriving(position); + + m_pidProcessTime.start(PID_PROCESS_PERIOD); + } + } + } +} + +void DrivingState::processTrackLost(int16_t position, const uint16_t* lineSensorValues) +{ + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + + if (nullptr == lineSensorValues) + { + return; + } + + /* Back on track? */ + if (false == isTrackGapDetected(position)) + { + m_trackStatus = TRACK_STATUS_ON_TRACK; + m_pidCtrl.resync(); + + Board::getInstance().getYellowLed().enable(false); + } + /* Max. distance driven, but track still not found? */ + else if (MAX_DISTANCE < Odometry::getInstance().getMileageCenter()) + { + /* Stop motors immediately. Don't move this to a later position, + * as this would extend the driven length. + */ + diffDrive.setLinearSpeed(0, 0); + + Sound::playAlarm(); + m_trackStatus = TRACK_STATUS_FINISHED; + } + else + { + /* Drive straight on. */ + diffDrive.setLinearSpeed(m_topSpeed, m_topSpeed); + } +} + +bool DrivingState::isStartEndLineDetected(const uint16_t* lineSensorValues) +{ + bool isDetected = false; + uint16_t leftSensor = lineSensorValues[0]; + uint16_t middleSensor = (lineSensorValues[1] + lineSensorValues[2] + lineSensorValues[3]) / 3; + uint16_t rightSensor = lineSensorValues[4]; + const uint8_t DEBOUNCE_CNT = 3; + const uint16_t LINE_SENSOR_OFF_TRACK_VALUE = 200; + + /* Note, the start-/end line detection must be debounced. Otherwise + * especially in low speed use cases, the line may be in one cycle + * detected, in the next not and then detected again. This would lead + * to a start line detection and afterwards to a end line detection, + * which would be wrong. + * + * Note the three sensors in the middle are handled as one sensor to + * avoid detection problems with different kind of line widths. + */ + if ((LINE_SENSOR_OFF_TRACK_VALUE <= leftSensor) && (LINE_SENSOR_OFF_TRACK_VALUE <= middleSensor) && + (LINE_SENSOR_OFF_TRACK_VALUE <= rightSensor)) + { + if (DEBOUNCE_CNT > m_startEndLineDebounce) + { + ++m_startEndLineDebounce; + } + + if (DEBOUNCE_CNT <= m_startEndLineDebounce) + { + isDetected = true; + } + } + else + { + m_startEndLineDebounce = 0; + } + + return isDetected; +} + +bool DrivingState::isTrackGapDetected(int16_t position) const +{ + bool isDetected = false; + const ILineSensors& lineSensors = Board::getInstance().getLineSensors(); + + /* Position value after loosing the track and sensor 0 saw it as last. + * It depends on the Zumo32U4LineSensors::readLine() implementation. + */ + const int16_t POS_MIN = 0; + + /* Position value after loosing the track and sensor N saw it as last. + * It depends on the Zumo32U4LineSensors::readLine() implementation. + */ + const int16_t POS_MAX = (lineSensors.getNumLineSensors() - 1) * 1000; + + /* Note, no debouncing is done here. If necessary, it shall be done + * outside this method. + */ + if ((POS_MIN >= position) || (POS_MAX <= position)) + { + isDetected = true; + } + + return isDetected; +} + +void DrivingState::adaptDriving(int16_t position) +{ + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + const ILineSensors& lineSensors = Board::getInstance().getLineSensors(); + const int16_t MAX_MOTOR_SPEED = diffDrive.getMaxMotorSpeed(); + 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(lineSensors.getSensorValueMax() * 2, 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, 0, MAX_MOTOR_SPEED); + rightSpeed = constrain(rightSpeed, 0, MAX_MOTOR_SPEED); + + diffDrive.setLinearSpeed(leftSpeed, rightSpeed); +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPSensorFusion/DrivingState.h b/lib/APPSensorFusion/DrivingState.h index 205f006b..8a9c0f76 100644 --- a/lib/APPSensorFusion/DrivingState.h +++ b/lib/APPSensorFusion/DrivingState.h @@ -1,220 +1,220 @@ -/* MIT License - * - * Copyright (c) 2023 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; - - /** - * The line detection status. - */ - enum LineStatus - { - LINE_STATUS_FIND_START_LINE = 0, /**< Find the start line. */ - LINE_STATUS_START_LINE_DETECTED, /**< Start line detected. */ - LINE_STATUS_FIND_END_LINE, /**< Find the end line. */ - LINE_STATUS_END_LINE_DETECTED /**< End Line detected. */ - }; - - /** - * Indicates if there is currently an End line detected. - * - * @return current Line Status as a LineStatus enum - */ - DrivingState::LineStatus getLineStatus() - { - return m_lineStatus; - } - -protected: -private: - /** - * The track status. - */ - enum TrackStatus - { - TRACK_STATUS_ON_TRACK = 0, /**< Robot is on track */ - TRACK_STATUS_LOST, /**< Robot lost track */ - 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; - - SimpleTimer m_observationTimer; /**< Observation timer to observe the max. time per challenge. */ - 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. */ - uint8_t m_startEndLineDebounce; /**< Counter used for easys debouncing of the start-/end line detection. */ - - /** - * Default constructor. - */ - DrivingState() : - m_observationTimer(), - m_pidProcessTime(), - m_pidCtrl(), - m_topSpeed(0), - m_lineStatus(LINE_STATUS_FIND_START_LINE), - m_trackStatus(TRACK_STATUS_ON_TRACK), - m_startEndLineDebounce(0) - { - } - - /** - * Default destructor. - */ - ~DrivingState() - { - } - - /* Not allowed. */ - DrivingState(const DrivingState& state); /**< Copy construction of an instance. */ - DrivingState& operator=(const DrivingState& state); /**< Assignment of an instance. */ - - /** - * Control driving in case the robot is on track. - * - * @param[in] position Current position on track - * @param[in] lineSensorValues Value of each line sensor - */ - void processOnTrack(int16_t position, const uint16_t* lineSensorValues); - - /** - * Control driving in case the robot lost the track. - * It handles the track search algorithm. - * - * @param[in] position Current position on track - * @param[in] lineSensorValues Value of each line sensor - */ - void processTrackLost(int16_t position, const uint16_t* lineSensorValues); - - /** - * Is start-/endline detected? - * - * @param[in] lineSensorValues Line sensor values - * - * @return If a start-/endline is detected, it will return true otherwise false. - */ - bool isStartEndLineDetected(const uint16_t* lineSensorValues); - - /** - * Is a track gap detected? - * Note, it contains no debouncing inside. If necessary, it shall be - * done outside. - * - * @param[in] position Determined position in digits - * - * @return If a track gap is detected, it will return true otherwise false. - */ - bool isTrackGapDetected(int16_t position) const; - - /** - * Adapt driving by using a PID algorithm, depended on the position - * input. - * - * @param[in] position Position in digits - */ - void adaptDriving(int16_t position); -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* DRIVING_STATE_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 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; + + /** + * The line detection status. + */ + enum LineStatus + { + LINE_STATUS_FIND_START_LINE = 0, /**< Find the start line. */ + LINE_STATUS_START_LINE_DETECTED, /**< Start line detected. */ + LINE_STATUS_FIND_END_LINE, /**< Find the end line. */ + LINE_STATUS_END_LINE_DETECTED /**< End Line detected. */ + }; + + /** + * Indicates if there is currently an End line detected. + * + * @return current Line Status as a LineStatus enum + */ + DrivingState::LineStatus getLineStatus() + { + return m_lineStatus; + } + +protected: +private: + /** + * The track status. + */ + enum TrackStatus + { + TRACK_STATUS_ON_TRACK = 0, /**< Robot is on track */ + TRACK_STATUS_LOST, /**< Robot lost track */ + 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; + + SimpleTimer m_observationTimer; /**< Observation timer to observe the max. time per challenge. */ + 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. */ + uint8_t m_startEndLineDebounce; /**< Counter used for easys debouncing of the start-/end line detection. */ + + /** + * Default constructor. + */ + DrivingState() : + m_observationTimer(), + m_pidProcessTime(), + m_pidCtrl(), + m_topSpeed(0), + m_lineStatus(LINE_STATUS_FIND_START_LINE), + m_trackStatus(TRACK_STATUS_ON_TRACK), + m_startEndLineDebounce(0) + { + } + + /** + * Default destructor. + */ + ~DrivingState() + { + } + + /* Not allowed. */ + DrivingState(const DrivingState& state); /**< Copy construction of an instance. */ + DrivingState& operator=(const DrivingState& state); /**< Assignment of an instance. */ + + /** + * Control driving in case the robot is on track. + * + * @param[in] position Current position on track + * @param[in] lineSensorValues Value of each line sensor + */ + void processOnTrack(int16_t position, const uint16_t* lineSensorValues); + + /** + * Control driving in case the robot lost the track. + * It handles the track search algorithm. + * + * @param[in] position Current position on track + * @param[in] lineSensorValues Value of each line sensor + */ + void processTrackLost(int16_t position, const uint16_t* lineSensorValues); + + /** + * Is start-/endline detected? + * + * @param[in] lineSensorValues Line sensor values + * + * @return If a start-/endline is detected, it will return true otherwise false. + */ + bool isStartEndLineDetected(const uint16_t* lineSensorValues); + + /** + * Is a track gap detected? + * Note, it contains no debouncing inside. If necessary, it shall be + * done outside. + * + * @param[in] position Determined position in digits + * + * @return If a track gap is detected, it will return true otherwise false. + */ + bool isTrackGapDetected(int16_t position) const; + + /** + * Adapt driving by using a PID algorithm, depended on the position + * input. + * + * @param[in] position Position in digits + */ + void adaptDriving(int16_t position); +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* DRIVING_STATE_H */ +/** @} */ diff --git a/lib/APPSensorFusion/ErrorState.cpp b/lib/APPSensorFusion/ErrorState.cpp index 1fa09820..636c799d 100644 --- a/lib/APPSensorFusion/ErrorState.cpp +++ b/lib/APPSensorFusion/ErrorState.cpp @@ -1,113 +1,113 @@ -/* MIT License - * - * Copyright (c) 2023 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 "MotorSpeedCalibrationState.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void ErrorState::entry() -{ - /* Nothing to do. */ -} - -void ErrorState::process(StateMachine& sm) -{ - IButton& buttonA = Board::getInstance().getButtonA(); - - /* Restart calibration? */ - if (true == buttonA.isPressed()) - { - buttonA.waitForRelease(); - sm.setState(&MotorSpeedCalibrationState::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 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 "MotorSpeedCalibrationState.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void ErrorState::entry() +{ + /* Nothing to do. */ +} + +void ErrorState::process(StateMachine& sm) +{ + IButton& buttonA = Board::getInstance().getButtonA(); + + /* Restart calibration? */ + if (true == buttonA.isPressed()) + { + buttonA.waitForRelease(); + sm.setState(&MotorSpeedCalibrationState::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/APPSensorFusion/ErrorState.h b/lib/APPSensorFusion/ErrorState.h index 4f9c592e..3ded9542 100644 --- a/lib/APPSensorFusion/ErrorState.h +++ b/lib/APPSensorFusion/ErrorState.h @@ -1,134 +1,134 @@ -/* MIT License - * - * Copyright (c) 2023 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 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 */ +/** @} */ diff --git a/lib/APPSensorFusion/LineSensorsCalibrationState.cpp b/lib/APPSensorFusion/LineSensorsCalibrationState.cpp index dcb6455c..ae551727 100644 --- a/lib/APPSensorFusion/LineSensorsCalibrationState.cpp +++ b/lib/APPSensorFusion/LineSensorsCalibrationState.cpp @@ -1,203 +1,203 @@ -/* MIT License - * - * Copyright (c) 2023 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() -{ - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - Odometry& odometry = Odometry::getInstance(); - - /* Prepare calibration drive. */ - m_calibrationSpeed = diffDrive.getMaxMotorSpeed() / 3; - m_orientation = odometry.getOrientation(); - - /* 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, "Cal=", 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 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() +{ + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + Odometry& odometry = Odometry::getInstance(); + + /* Prepare calibration drive. */ + m_calibrationSpeed = diffDrive.getMaxMotorSpeed() / 3; + m_orientation = odometry.getOrientation(); + + /* 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, "Cal=", 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/APPSensorFusion/LineSensorsCalibrationState.h b/lib/APPSensorFusion/LineSensorsCalibrationState.h index 075577f1..3f695436 100644 --- a/lib/APPSensorFusion/LineSensorsCalibrationState.h +++ b/lib/APPSensorFusion/LineSensorsCalibrationState.h @@ -1,161 +1,161 @@ -/* MIT License - * - * Copyright (c) 2023 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 60°). - */ - static const int32_t CALIB_ANGLE = (FP_2PI() / 6); - - 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 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 60°). + */ + static const int32_t CALIB_ANGLE = (FP_2PI() / 6); + + 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/APPSensorFusion/MotorSpeedCalibrationState.cpp b/lib/APPSensorFusion/MotorSpeedCalibrationState.cpp index da991ca2..3aeb2ac3 100644 --- a/lib/APPSensorFusion/MotorSpeedCalibrationState.cpp +++ b/lib/APPSensorFusion/MotorSpeedCalibrationState.cpp @@ -1,227 +1,227 @@ -/* MIT License - * - * Copyright (c) 2023 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" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/** - * Logging source. - */ -LOG_TAG("MSCState"); - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void MotorSpeedCalibrationState::entry() -{ - /* 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(); - - /* 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; - - /* 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("MS=0"); - sm.setState(&ErrorState::getInstance()); - } - else - { - LOG_INFO_VAL("Calibrated max. speed (steps/s): ", maxSpeed); - LOG_INFO_VAL("Calibrated max. speed (mm/s): ", maxSpeed / RobotConstants::ENCODER_STEPS_PER_MM); - - sm.setState(&LineSensorsCalibrationState::getInstance()); - } -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 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" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/** + * Logging source. + */ +LOG_TAG("MSCState"); + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void MotorSpeedCalibrationState::entry() +{ + /* 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(); + + /* 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; + + /* 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("MS=0"); + sm.setState(&ErrorState::getInstance()); + } + else + { + LOG_INFO_VAL("Calibrated max. speed (steps/s): ", maxSpeed); + LOG_INFO_VAL("Calibrated max. speed (mm/s): ", maxSpeed / RobotConstants::ENCODER_STEPS_PER_MM); + + sm.setState(&LineSensorsCalibrationState::getInstance()); + } +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPSensorFusion/MotorSpeedCalibrationState.h b/lib/APPSensorFusion/MotorSpeedCalibrationState.h index 4dbca55e..356d0204 100644 --- a/lib/APPSensorFusion/MotorSpeedCalibrationState.h +++ b/lib/APPSensorFusion/MotorSpeedCalibrationState.h @@ -1,165 +1,165 @@ -/* MIT License - * - * Copyright (c) 2023 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 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/APPSensorFusion/ParameterSets.cpp b/lib/APPSensorFusion/ParameterSets.cpp index e3539b00..49042585 100644 --- a/lib/APPSensorFusion/ParameterSets.cpp +++ b/lib/APPSensorFusion/ParameterSets.cpp @@ -1,139 +1,139 @@ -/* MIT License - * - * Copyright (c) 2023 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] = { - "PID Slow", /* Name */ - 1920, /* Top speed in steps/s */ - 3, /* Kp Numerator */ - 2, /* Kp Denominator */ - 1, /* Ki Numerator */ - 60, /* Ki Denominator */ - 4, /* Kd Numerator */ - 1 /* Kd Denominator */ - }; - - m_parSets[1] = { - "PID Fast", /* Name */ - 2400, /* Top speed in steps/s */ - 3, /* Kp Numerator */ - 2, /* Kp Denominator */ - 1, /* Ki Numerator */ - 40, /* Ki Denominator */ - 40, /* Kd Numerator */ - 1 /* Kd Denominator */ - }; - - m_parSets[2] = { - "PD Fast", /* Name */ - 2400, /* Top speed in steps/s */ - 3, /* Kp Numerator */ - 1, /* Kp Denominator */ - 0, /* Ki Numerator */ - 1, /* Ki Denominator */ - 40, /* Kd Numerator */ - 1 /* Kd Denominator */ - }; -} - -ParameterSets::~ParameterSets() -{ -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 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] = { + "PID Slow", /* Name */ + 1920, /* Top speed in steps/s */ + 3, /* Kp Numerator */ + 2, /* Kp Denominator */ + 1, /* Ki Numerator */ + 60, /* Ki Denominator */ + 4, /* Kd Numerator */ + 1 /* Kd Denominator */ + }; + + m_parSets[1] = { + "PID Fast", /* Name */ + 2400, /* Top speed in steps/s */ + 3, /* Kp Numerator */ + 2, /* Kp Denominator */ + 1, /* Ki Numerator */ + 40, /* Ki Denominator */ + 40, /* Kd Numerator */ + 1 /* Kd Denominator */ + }; + + m_parSets[2] = { + "PD Fast", /* Name */ + 2400, /* Top speed in steps/s */ + 3, /* Kp Numerator */ + 1, /* Kp Denominator */ + 0, /* Ki Numerator */ + 1, /* Ki Denominator */ + 40, /* Kd Numerator */ + 1 /* Kd Denominator */ + }; +} + +ParameterSets::~ParameterSets() +{ +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPSensorFusion/ParameterSets.h b/lib/APPSensorFusion/ParameterSets.h index b3684da4..28b3bd3c 100644 --- a/lib/APPSensorFusion/ParameterSets.h +++ b/lib/APPSensorFusion/ParameterSets.h @@ -1,145 +1,145 @@ -/* MIT License - * - * Copyright (c) 2023 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 = 3; - -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(); - - /* Not allowed. */ - ParameterSets(const ParameterSets& set); /**< Copy construction of an instance. */ - ParameterSets& operator=(const ParameterSets& set); /**< Assignment of an instance. */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* PARAMETER_SET_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 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 = 3; + +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(); + + /* Not allowed. */ + ParameterSets(const ParameterSets& set); /**< Copy construction of an instance. */ + ParameterSets& operator=(const ParameterSets& set); /**< Assignment of an instance. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* PARAMETER_SET_H */ +/** @} */ diff --git a/lib/APPSensorFusion/ReadyState.cpp b/lib/APPSensorFusion/ReadyState.cpp index de72782a..0630cf64 100644 --- a/lib/APPSensorFusion/ReadyState.cpp +++ b/lib/APPSensorFusion/ReadyState.cpp @@ -1,150 +1,150 @@ -/* MIT License - * - * Copyright (c) 2023 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() -{ - const int32_t SENSOR_VALUE_OUT_PERIOD = 1000; /* ms */ - - /* 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 == buttonA.isPressed()) - { - buttonA.waitForRelease(); - sm.setState(&ReleaseTrackState::getInstance()); - } - /* Shall the line sensor values be printed out on console? */ - else 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(); - } -} - -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 - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 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() +{ + const int32_t SENSOR_VALUE_OUT_PERIOD = 1000; /* ms */ + + /* 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 == buttonA.isPressed()) + { + buttonA.waitForRelease(); + sm.setState(&ReleaseTrackState::getInstance()); + } + /* Shall the line sensor values be printed out on console? */ + else 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(); + } +} + +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/APPSensorFusion/ReadyState.h b/lib/APPSensorFusion/ReadyState.h index 1553f1b2..25efd942 100644 --- a/lib/APPSensorFusion/ReadyState.h +++ b/lib/APPSensorFusion/ReadyState.h @@ -1,129 +1,129 @@ -/* MIT License - * - * Copyright (c) 2023 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. */ - - /** - * Default constructor. - */ - ReadyState() : m_timer(), m_isLapTimeAvailable(false), m_lapTime(0) - { - } - - /** - * 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 */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 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. */ + + /** + * Default constructor. + */ + ReadyState() : m_timer(), m_isLapTimeAvailable(false), m_lapTime(0) + { + } + + /** + * 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/APPSensorFusion/ReleaseTrackState.cpp b/lib/APPSensorFusion/ReleaseTrackState.cpp index 8c7206e7..e6d2e15d 100644 --- a/lib/APPSensorFusion/ReleaseTrackState.cpp +++ b/lib/APPSensorFusion/ReleaseTrackState.cpp @@ -1,121 +1,121 @@ -/* MIT License - * - * Copyright (c) 2023 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" - -/****************************************************************************** - * 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 == buttonA.isPressed()) - { - /* Choose next parameter set (round-robin) */ - ParameterSets::getInstance().next(); - showParSet(); - - buttonA.waitForRelease(); - 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 -{ - /* Nothing to do. */ -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 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" + +/****************************************************************************** + * 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 == buttonA.isPressed()) + { + /* Choose next parameter set (round-robin) */ + ParameterSets::getInstance().next(); + showParSet(); + + buttonA.waitForRelease(); + 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 +{ + /* Nothing to do. */ +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPSensorFusion/ReleaseTrackState.h b/lib/APPSensorFusion/ReleaseTrackState.h index 56f2a636..3c408768 100644 --- a/lib/APPSensorFusion/ReleaseTrackState.h +++ b/lib/APPSensorFusion/ReleaseTrackState.h @@ -1,129 +1,129 @@ -/* MIT License - * - * Copyright (c) 2023 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 */ - - /** - * Default constructor. - */ - ReleaseTrackState() - { - } - - /** - * Default destructor. - */ - ~ReleaseTrackState() - { - } - - /* Not allowed. */ - ReleaseTrackState(const ReleaseTrackState& state); /**< Copy construction of an instance. */ - ReleaseTrackState& operator=(const ReleaseTrackState& state); /**< Assignment of an instance. */ - - /** - * Show choosen parameter set on LCD. - */ - void showParSet() const; -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* RELEASE_TRACK_STATE_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 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 */ + + /** + * Default constructor. + */ + ReleaseTrackState() + { + } + + /** + * Default destructor. + */ + ~ReleaseTrackState() + { + } + + /* Not allowed. */ + ReleaseTrackState(const ReleaseTrackState& state); /**< Copy construction of an instance. */ + ReleaseTrackState& operator=(const ReleaseTrackState& state); /**< Assignment of an instance. */ + + /** + * Show choosen parameter set on LCD. + */ + void showParSet() const; +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* RELEASE_TRACK_STATE_H */ +/** @} */ diff --git a/lib/APPSensorFusion/SerialMuxChannels.h b/lib/APPSensorFusion/SerialMuxChannels.h index 43fca858..4c122602 100644 --- a/lib/APPSensorFusion/SerialMuxChannels.h +++ b/lib/APPSensorFusion/SerialMuxChannels.h @@ -1,119 +1,119 @@ -/* MIT License - * - * Copyright (c) 2023 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 - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/** Name of Channel to send Sensor Data to. */ -#define SENSORDATA_CHANNEL_NAME "SENSOR_DATA" - -/** DLC of Sensordata Channel */ -#define SENSORDATA_CHANNEL_DLC (sizeof(SensorData)) - -/** Name of Channel to End Line Detected Signal. */ -#define ENDLINE_CHANNEL_NAME "END_LINE" - -/** DLC of End Line Channel */ -#define ENDLINE_CHANNEL_DLC (sizeof(EndLineFlag)) - -/** Maximum number of SerialMuxProt Channels. */ -#define MAX_CHANNELS (10U) - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** Struct of the Sensor Data channel payload. */ -typedef struct _SensorData -{ - /** Position in x direction in mm calculated by odometry. */ - int32_t positionOdometryX; - - /** Position in y direction in mm calculated by odometry. */ - int32_t positionOdometryY; - - /** Orientation in mrad calculated by odometry. */ - int32_t orientationOdometry; - - /** Acceleration in x direction as a raw sensor value in digits. - * It can be converted into a physical acceleration value in mm/s^2 via the - * multiplication with a sensitivity factor in mm/s^2/digit. - */ - int16_t accelerationX; - - /** Acceleration in y direction as a raw sensor value in digits. - * It can be converted into a physical acceleration value in mm/s^2 via the - * multiplication with a sensitivity factor in mm/s^2/digit. - */ - int16_t accelerationY; - - /** Magnetometer value in x direction as a raw sensor value in digits. - * It does not require conversion into a physical magnetometer value since only the - * ratio with the value in y direction is important. - */ - int16_t magnetometerValueX; - - /** Magnetometer value in y direction as a raw sensor value in digits. - * It does not require conversion into a physical magnetometer value since only the - * ratio with the value in x direction is important. - */ - int16_t magnetometerValueY; - - /** Gyro value around z axis as a raw sensor value in digits. - * It can be converted into a physical turn rate in mrad/s via the multiplication - * with a sensitivity factor in mrad/s/digit. - */ - int16_t turnRate; -} __attribute__((packed)) SensorData; - -/** Struct of the End Line Detection payload. */ -typedef struct _EndLineFlag -{ - /** Indicates if the End Line has been detected. */ - bool isEndLineDetected; -} __attribute__((packed)) EndLineFlag; - -/****************************************************************************** - * Functions - *****************************************************************************/ - +/* MIT License + * + * Copyright (c) 2023 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 + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/** Name of Channel to send Sensor Data to. */ +#define SENSORDATA_CHANNEL_NAME "SENSOR_DATA" + +/** DLC of Sensordata Channel */ +#define SENSORDATA_CHANNEL_DLC (sizeof(SensorData)) + +/** Name of Channel to End Line Detected Signal. */ +#define ENDLINE_CHANNEL_NAME "END_LINE" + +/** DLC of End Line Channel */ +#define ENDLINE_CHANNEL_DLC (sizeof(EndLineFlag)) + +/** Maximum number of SerialMuxProt Channels. */ +#define MAX_CHANNELS (10U) + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** Struct of the Sensor Data channel payload. */ +typedef struct _SensorData +{ + /** Position in x direction in mm calculated by odometry. */ + int32_t positionOdometryX; + + /** Position in y direction in mm calculated by odometry. */ + int32_t positionOdometryY; + + /** Orientation in mrad calculated by odometry. */ + int32_t orientationOdometry; + + /** Acceleration in x direction as a raw sensor value in digits. + * It can be converted into a physical acceleration value in mm/s^2 via the + * multiplication with a sensitivity factor in mm/s^2/digit. + */ + int16_t accelerationX; + + /** Acceleration in y direction as a raw sensor value in digits. + * It can be converted into a physical acceleration value in mm/s^2 via the + * multiplication with a sensitivity factor in mm/s^2/digit. + */ + int16_t accelerationY; + + /** Magnetometer value in x direction as a raw sensor value in digits. + * It does not require conversion into a physical magnetometer value since only the + * ratio with the value in y direction is important. + */ + int16_t magnetometerValueX; + + /** Magnetometer value in y direction as a raw sensor value in digits. + * It does not require conversion into a physical magnetometer value since only the + * ratio with the value in x direction is important. + */ + int16_t magnetometerValueY; + + /** Gyro value around z axis as a raw sensor value in digits. + * It can be converted into a physical turn rate in mrad/s via the multiplication + * with a sensitivity factor in mrad/s/digit. + */ + int16_t turnRate; +} __attribute__((packed)) SensorData; + +/** Struct of the End Line Detection payload. */ +typedef struct _EndLineFlag +{ + /** Indicates if the End Line has been detected. */ + bool isEndLineDetected; +} __attribute__((packed)) EndLineFlag; + +/****************************************************************************** + * Functions + *****************************************************************************/ + #endif /* SERIAL_MUX_CHANNELS_H_ */ \ No newline at end of file diff --git a/lib/APPSensorFusion/StartupState.cpp b/lib/APPSensorFusion/StartupState.cpp index 25ee2a57..204804e2 100644 --- a/lib/APPSensorFusion/StartupState.cpp +++ b/lib/APPSensorFusion/StartupState.cpp @@ -1,101 +1,101 @@ -/* MIT License - * - * Copyright (c) 2023 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 "StartupState.h" -#include -#include -#include "MotorSpeedCalibrationState.h" -#include "Sound.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void StartupState::entry() -{ - /* Initialize HAL */ - Board::getInstance().init(); -} - -void StartupState::process(StateMachine& sm) -{ - IButton& buttonA = Board::getInstance().getButtonA(); - - if (true == buttonA.isPressed()) - { - buttonA.waitForRelease(); - sm.setState(&MotorSpeedCalibrationState::getInstance()); - } -} - -void StartupState::exit() -{ - /* Nothing to do */ -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 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 "StartupState.h" +#include +#include +#include "MotorSpeedCalibrationState.h" +#include "Sound.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void StartupState::entry() +{ + /* Initialize HAL */ + Board::getInstance().init(); +} + +void StartupState::process(StateMachine& sm) +{ + IButton& buttonA = Board::getInstance().getButtonA(); + + if (true == buttonA.isPressed()) + { + buttonA.waitForRelease(); + sm.setState(&MotorSpeedCalibrationState::getInstance()); + } +} + +void StartupState::exit() +{ + /* Nothing to do */ +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPSensorFusion/StartupState.h b/lib/APPSensorFusion/StartupState.h index 589eb4f3..8f3d8b5a 100644 --- a/lib/APPSensorFusion/StartupState.h +++ b/lib/APPSensorFusion/StartupState.h @@ -1,123 +1,123 @@ -/* MIT License - * - * Copyright (c) 2023 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 - -/****************************************************************************** - * 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: - /** - * Duration in ms how long the team id or team name shall be shown at startup. - */ - static const uint32_t TEAM_NAME_DURATION = 2000; - - /** - * Default constructor. - */ - StartupState() - { - } - - /** - * Default destructor. - */ - ~StartupState() - { - } - - /* Not allowed. */ - StartupState(const StartupState& state); /**< Copy construction of an instance. */ - StartupState& operator=(const StartupState& state); /**< Assignment of an instance. */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* STARTUP_STATE_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 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 + +/****************************************************************************** + * 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: + /** + * Duration in ms how long the team id or team name shall be shown at startup. + */ + static const uint32_t TEAM_NAME_DURATION = 2000; + + /** + * Default constructor. + */ + StartupState() + { + } + + /** + * Default destructor. + */ + ~StartupState() + { + } + + /* Not allowed. */ + StartupState(const StartupState& state); /**< Copy construction of an instance. */ + StartupState& operator=(const StartupState& state); /**< Assignment of an instance. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* STARTUP_STATE_H */ +/** @} */ diff --git a/lib/APPSensorFusion/library.json b/lib/APPSensorFusion/library.json index 41865f20..be98d623 100644 --- a/lib/APPSensorFusion/library.json +++ b/lib/APPSensorFusion/library.json @@ -1,29 +1,29 @@ -{ - "name": "APPSensorFusion", - "version": "0.1.0", - "description": "Sensor Fusion application", - "authors": [ - { - "name": "Juliane Kerpe", - "email": "juliane.kerpe@web.de", - "url": "https://github.com/jkerpe", - "maintainer": true - } - ], - "license": "MIT", - "dependencies": [ - { - "name": "Service" - }, - { - "name": "HALInterfaces" - }, - { - "owner": "gabryelreyes", - "name": "SerialMuxProt", - "version": "^2.0.0" - } - ], - "frameworks": "*", - "platforms": "*" -} +{ + "name": "APPSensorFusion", + "version": "0.1.0", + "description": "Sensor Fusion application", + "authors": [ + { + "name": "Juliane Kerpe", + "email": "juliane.kerpe@web.de", + "url": "https://github.com/jkerpe", + "maintainer": true + } + ], + "license": "MIT", + "dependencies": [ + { + "name": "Service" + }, + { + "name": "HALInterfaces" + }, + { + "owner": "gabryelreyes", + "name": "SerialMuxProt", + "version": "^2.0.0" + } + ], + "frameworks": "*", + "platforms": "*" +} diff --git a/lib/HALConvoyLeaderSim/Board.cpp b/lib/HALConvoyLeaderSim/Board.cpp index d4a1fd84..abcfb210 100644 --- a/lib/HALConvoyLeaderSim/Board.cpp +++ b/lib/HALConvoyLeaderSim/Board.cpp @@ -1,143 +1,143 @@ -/* MIT License - * - * Copyright (c) 2019 - 2023 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 - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/* Name of the speaker in the robot simulation. */ -const char* Board::SPEAKER_NAME = "robot_speaker"; - -/* Name of the display in the robot simulation. */ -const char* Board::DISPLAY_NAME = "robot_display"; - -/* Name of the left motor in the robot simulation. */ -const char* Board::LEFT_MOTOR_NAME = "motor_left"; - -/* Name of the right motor in the robot simulation. */ -const char* Board::RIGHT_MOTOR_NAME = "motor_right"; - -/* Name of the infrared emitter 0 in the robot simulation. */ -const char* Board::EMITTER_0_NAME = "emitter_l"; - -/* Name of the infrared emitter 1 in the robot simulation. */ -const char* Board::EMITTER_1_NAME = "emitter_lm"; - -/* Name of the infrared emitter 2 in the robot simulation. */ -const char* Board::EMITTER_2_NAME = "emitter_m"; - -/* Name of the infrared emitter 3 in the robot simulation. */ -const char* Board::EMITTER_3_NAME = "emitter_rm"; - -/* Name of the infrared emitter 4 in the robot simulation. */ -const char* Board::EMITTER_4_NAME = "emitter_r"; - -/** Name of the position sensor of the left motor in the robot simulation. */ -const char* Board::POS_SENSOR_LEFT_NAME = "position_sensor_left"; - -/** Name of the position sensor of the right motor in the robot simulation. */ -const char* Board::POS_SENSOR_RIGHT_NAME = "position_sensor_right"; - -/* Name of the light sensor 0 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_0_NAME = "lightsensor_l"; - -/* Name of the light sensor 1 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_1_NAME = "lightsensor_lm"; - -/* Name of the light sensor 2 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_2_NAME = "lightsensor_m"; - -/* Name of the light sensor 3 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_3_NAME = "lightsensor_rm"; - -/* Name of the light sensor 4 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_4_NAME = "lightsensor_r"; - -/* Name of the red LED in the robot simulation. */ -const char* Board::LED_RED_NAME = "led_red"; - -/* Name of the yellow LED in the robot simulation. */ -const char* Board::LED_YELLOW_NAME = "led_yellow"; - -/* Name of the green LED in the robot simulation. */ -const char* Board::LED_GREEN_NAME = "led_green"; - -/* Name of the front left proximity sensor in the robot simulation. */ -const char* Board::PROXIMITY_SENSOR_FRONT_LEFT_NAME = "proxim_sensor_fl"; - -/* Name of the front right proximity sensor in the robot simulation. */ -const char* Board::PROXIMITY_SENSOR_FRONT_RIGHT_NAME = "proxim_sensor_fr"; - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -void Board::init() -{ - m_encoders.init(); - m_keyboard.init(); - m_lineSensors.init(); - m_motors.init(); - m_proximitySensors.initFrontSensor(); -} - -/****************************************************************************** - * Local Functions +/* MIT License + * + * Copyright (c) 2019 - 2023 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 + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/* Name of the speaker in the robot simulation. */ +const char* Board::SPEAKER_NAME = "robot_speaker"; + +/* Name of the display in the robot simulation. */ +const char* Board::DISPLAY_NAME = "robot_display"; + +/* Name of the left motor in the robot simulation. */ +const char* Board::LEFT_MOTOR_NAME = "motor_left"; + +/* Name of the right motor in the robot simulation. */ +const char* Board::RIGHT_MOTOR_NAME = "motor_right"; + +/* Name of the infrared emitter 0 in the robot simulation. */ +const char* Board::EMITTER_0_NAME = "emitter_l"; + +/* Name of the infrared emitter 1 in the robot simulation. */ +const char* Board::EMITTER_1_NAME = "emitter_lm"; + +/* Name of the infrared emitter 2 in the robot simulation. */ +const char* Board::EMITTER_2_NAME = "emitter_m"; + +/* Name of the infrared emitter 3 in the robot simulation. */ +const char* Board::EMITTER_3_NAME = "emitter_rm"; + +/* Name of the infrared emitter 4 in the robot simulation. */ +const char* Board::EMITTER_4_NAME = "emitter_r"; + +/** Name of the position sensor of the left motor in the robot simulation. */ +const char* Board::POS_SENSOR_LEFT_NAME = "position_sensor_left"; + +/** Name of the position sensor of the right motor in the robot simulation. */ +const char* Board::POS_SENSOR_RIGHT_NAME = "position_sensor_right"; + +/* Name of the light sensor 0 in the robot simulation. */ +const char* Board::LIGHT_SENSOR_0_NAME = "lightsensor_l"; + +/* Name of the light sensor 1 in the robot simulation. */ +const char* Board::LIGHT_SENSOR_1_NAME = "lightsensor_lm"; + +/* Name of the light sensor 2 in the robot simulation. */ +const char* Board::LIGHT_SENSOR_2_NAME = "lightsensor_m"; + +/* Name of the light sensor 3 in the robot simulation. */ +const char* Board::LIGHT_SENSOR_3_NAME = "lightsensor_rm"; + +/* Name of the light sensor 4 in the robot simulation. */ +const char* Board::LIGHT_SENSOR_4_NAME = "lightsensor_r"; + +/* Name of the red LED in the robot simulation. */ +const char* Board::LED_RED_NAME = "led_red"; + +/* Name of the yellow LED in the robot simulation. */ +const char* Board::LED_YELLOW_NAME = "led_yellow"; + +/* Name of the green LED in the robot simulation. */ +const char* Board::LED_GREEN_NAME = "led_green"; + +/* Name of the front left proximity sensor in the robot simulation. */ +const char* Board::PROXIMITY_SENSOR_FRONT_LEFT_NAME = "proxim_sensor_fl"; + +/* Name of the front right proximity sensor in the robot simulation. */ +const char* Board::PROXIMITY_SENSOR_FRONT_RIGHT_NAME = "proxim_sensor_fr"; + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +void Board::init() +{ + m_encoders.init(); + m_keyboard.init(); + m_lineSensors.init(); + m_motors.init(); + m_proximitySensors.initFrontSensor(); +} + +/****************************************************************************** + * Local Functions *****************************************************************************/ \ No newline at end of file diff --git a/lib/HALConvoyLeaderSim/Board.h b/lib/HALConvoyLeaderSim/Board.h index 33c20b6e..6ee59544 100644 --- a/lib/HALConvoyLeaderSim/Board.h +++ b/lib/HALConvoyLeaderSim/Board.h @@ -1,398 +1,398 @@ -/* MIT License - * - * Copyright (c) 2023 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 - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** - * The concrete simulation robot board. - */ -class Board -{ -public: - /** - * Get board instance. - * - * @return Board instance - */ - static Board& getInstance() - { - static Board instance; /* idiom */ - - return instance; - } - - /** - * Initialize the hardware. - */ - void init(); - - /** - * Get button A driver. - * - * @return Button A driver. - */ - IButton& getButtonA() - { - return m_buttonA; - } - - /** - * Get button B driver. - * - * @return Button B driver. - */ - IButton& getButtonB() - { - return m_buttonB; - } - - /** - * Get button C driver. - * - * @return Button C driver. - */ - IButton& getButtonC() - { - return m_buttonC; - } - - /** - * Get buzzer driver. - * - * @return Buzzer driver. - */ - IBuzzer& getBuzzer() - { - return m_buzzer; - } - - /** - * Get LCD driver. - * - * @return LCD driver. - */ - IDisplay& getDisplay() - { - return m_display; - } - - /** - * Get encoders. - * - * @return Encoders driver. - */ - IEncoders& getEncoders() - { - return m_encoders; - } - - /** - * Get line sensors driver. - * - * @return Line sensor driver. - */ - ILineSensors& getLineSensors() - { - return m_lineSensors; - } - - /** - * Get motor driver. - * - * @return Motor driver. - */ - IMotors& getMotors() - { - return m_motors; - } - - /** - * Get red LED driver. - * - * @return Red LED driver. - */ - ILed& getRedLed() - { - return m_ledRed; - } - - /** - * Get yellow LED driver. - * - * @return Yellow LED driver. - */ - ILed& getYellowLed() - { - return m_ledYellow; - } - - /** - * Get green LED driver. - * - * @return Green LED driver. - */ - ILed& getGreenLed() - { - return m_ledGreen; - } - - /** - * Get proximity sensors driver. - * - * @return Proximity sensors driver - */ - IProximitySensors& getProximitySensors() - { - return m_proximitySensors; - } - -protected: -private: - /** Name of the speaker in the robot simulation. */ - static const char* SPEAKER_NAME; - - /** Name of the display in the robot simulation. */ - static const char* DISPLAY_NAME; - - /** Name of the left motor in the robot simulation. */ - static const char* LEFT_MOTOR_NAME; - - /** Name of the right motor in the robot simulation. */ - static const char* RIGHT_MOTOR_NAME; - - /** Name of the infrared emitter 0 in the robot simulation. */ - static const char* EMITTER_0_NAME; - - /** Name of the infrared emitter 1 in the robot simulation. */ - static const char* EMITTER_1_NAME; - - /** Name of the infrared emitter 2 in the robot simulation. */ - static const char* EMITTER_2_NAME; - - /** Name of the infrared emitter 3 in the robot simulation. */ - static const char* EMITTER_3_NAME; - - /** Name of the infrared emitter 4 in the robot simulation. */ - static const char* EMITTER_4_NAME; - - /** Name of the position sensor of the left motor in the robot simulation. */ - static const char* POS_SENSOR_LEFT_NAME; - - /** Name of the position sensor of the right motor in the robot simulation. */ - static const char* POS_SENSOR_RIGHT_NAME; - - /** Name of the light sensor 0 in the robot simulation. */ - static const char* LIGHT_SENSOR_0_NAME; - - /** Name of the light sensor 1 in the robot simulation. */ - static const char* LIGHT_SENSOR_1_NAME; - - /** Name of the light sensor 2 in the robot simulation. */ - static const char* LIGHT_SENSOR_2_NAME; - - /** Name of the light sensor 3 in the robot simulation. */ - static const char* LIGHT_SENSOR_3_NAME; - - /** Name of the light sensor 4 in the robot simulation. */ - static const char* LIGHT_SENSOR_4_NAME; - - /** Name of the red LED in the robot simulation. */ - static const char* LED_RED_NAME; - - /** Name of the yellow LED in the robot simulation. */ - static const char* LED_YELLOW_NAME; - - /** Name of the green LED in the robot simulation. */ - static const char* LED_GREEN_NAME; - - /** Name of the front proximity sensor in the robot simulation. */ - static const char* PROXIMITY_SENSOR_FRONT_LEFT_NAME; - - /** Name of the front right proximity sensor in the robot simulation. */ - static const char* PROXIMITY_SENSOR_FRONT_RIGHT_NAME; - - /** 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; - - /** Yellow LED driver */ - LedYellow m_ledYellow; - - /** Green LED driver */ - LedGreen m_ledGreen; - - /** Proximity sensors */ - ProximitySensors m_proximitySensors; - - /** - * Constructs the concrete board. - */ - Board() : - m_robot(), - 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(SPEAKER_NAME)), - m_display(m_robot.getDisplay(DISPLAY_NAME)), - m_encoders(m_simTime, m_robot.getPositionSensor(POS_SENSOR_LEFT_NAME), - m_robot.getPositionSensor(POS_SENSOR_RIGHT_NAME)), - m_lineSensors(m_simTime, m_robot.getEmitter(EMITTER_0_NAME), m_robot.getEmitter(EMITTER_1_NAME), - m_robot.getEmitter(EMITTER_2_NAME), m_robot.getEmitter(EMITTER_3_NAME), - m_robot.getEmitter(EMITTER_4_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_0_NAME), - m_robot.getDistanceSensor(LIGHT_SENSOR_1_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_2_NAME), - m_robot.getDistanceSensor(LIGHT_SENSOR_3_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_4_NAME)), - m_motors(m_robot.getMotor(LEFT_MOTOR_NAME), m_robot.getMotor(RIGHT_MOTOR_NAME)), - m_ledRed(m_robot.getLED(LED_RED_NAME)), - m_ledYellow(m_robot.getLED(LED_YELLOW_NAME)), - m_ledGreen(m_robot.getLED(LED_GREEN_NAME)), - m_proximitySensors(m_simTime, m_robot.getDistanceSensor(PROXIMITY_SENSOR_FRONT_LEFT_NAME), - m_robot.getDistanceSensor(PROXIMITY_SENSOR_FRONT_RIGHT_NAME)) - { - } - - /** - * 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; - } - - /** - * 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 */ +/* MIT License + * + * Copyright (c) 2023 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 + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** + * The concrete simulation robot board. + */ +class Board +{ +public: + /** + * Get board instance. + * + * @return Board instance + */ + static Board& getInstance() + { + static Board instance; /* idiom */ + + return instance; + } + + /** + * Initialize the hardware. + */ + void init(); + + /** + * Get button A driver. + * + * @return Button A driver. + */ + IButton& getButtonA() + { + return m_buttonA; + } + + /** + * Get button B driver. + * + * @return Button B driver. + */ + IButton& getButtonB() + { + return m_buttonB; + } + + /** + * Get button C driver. + * + * @return Button C driver. + */ + IButton& getButtonC() + { + return m_buttonC; + } + + /** + * Get buzzer driver. + * + * @return Buzzer driver. + */ + IBuzzer& getBuzzer() + { + return m_buzzer; + } + + /** + * Get LCD driver. + * + * @return LCD driver. + */ + IDisplay& getDisplay() + { + return m_display; + } + + /** + * Get encoders. + * + * @return Encoders driver. + */ + IEncoders& getEncoders() + { + return m_encoders; + } + + /** + * Get line sensors driver. + * + * @return Line sensor driver. + */ + ILineSensors& getLineSensors() + { + return m_lineSensors; + } + + /** + * Get motor driver. + * + * @return Motor driver. + */ + IMotors& getMotors() + { + return m_motors; + } + + /** + * Get red LED driver. + * + * @return Red LED driver. + */ + ILed& getRedLed() + { + return m_ledRed; + } + + /** + * Get yellow LED driver. + * + * @return Yellow LED driver. + */ + ILed& getYellowLed() + { + return m_ledYellow; + } + + /** + * Get green LED driver. + * + * @return Green LED driver. + */ + ILed& getGreenLed() + { + return m_ledGreen; + } + + /** + * Get proximity sensors driver. + * + * @return Proximity sensors driver + */ + IProximitySensors& getProximitySensors() + { + return m_proximitySensors; + } + +protected: +private: + /** Name of the speaker in the robot simulation. */ + static const char* SPEAKER_NAME; + + /** Name of the display in the robot simulation. */ + static const char* DISPLAY_NAME; + + /** Name of the left motor in the robot simulation. */ + static const char* LEFT_MOTOR_NAME; + + /** Name of the right motor in the robot simulation. */ + static const char* RIGHT_MOTOR_NAME; + + /** Name of the infrared emitter 0 in the robot simulation. */ + static const char* EMITTER_0_NAME; + + /** Name of the infrared emitter 1 in the robot simulation. */ + static const char* EMITTER_1_NAME; + + /** Name of the infrared emitter 2 in the robot simulation. */ + static const char* EMITTER_2_NAME; + + /** Name of the infrared emitter 3 in the robot simulation. */ + static const char* EMITTER_3_NAME; + + /** Name of the infrared emitter 4 in the robot simulation. */ + static const char* EMITTER_4_NAME; + + /** Name of the position sensor of the left motor in the robot simulation. */ + static const char* POS_SENSOR_LEFT_NAME; + + /** Name of the position sensor of the right motor in the robot simulation. */ + static const char* POS_SENSOR_RIGHT_NAME; + + /** Name of the light sensor 0 in the robot simulation. */ + static const char* LIGHT_SENSOR_0_NAME; + + /** Name of the light sensor 1 in the robot simulation. */ + static const char* LIGHT_SENSOR_1_NAME; + + /** Name of the light sensor 2 in the robot simulation. */ + static const char* LIGHT_SENSOR_2_NAME; + + /** Name of the light sensor 3 in the robot simulation. */ + static const char* LIGHT_SENSOR_3_NAME; + + /** Name of the light sensor 4 in the robot simulation. */ + static const char* LIGHT_SENSOR_4_NAME; + + /** Name of the red LED in the robot simulation. */ + static const char* LED_RED_NAME; + + /** Name of the yellow LED in the robot simulation. */ + static const char* LED_YELLOW_NAME; + + /** Name of the green LED in the robot simulation. */ + static const char* LED_GREEN_NAME; + + /** Name of the front proximity sensor in the robot simulation. */ + static const char* PROXIMITY_SENSOR_FRONT_LEFT_NAME; + + /** Name of the front right proximity sensor in the robot simulation. */ + static const char* PROXIMITY_SENSOR_FRONT_RIGHT_NAME; + + /** 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; + + /** Yellow LED driver */ + LedYellow m_ledYellow; + + /** Green LED driver */ + LedGreen m_ledGreen; + + /** Proximity sensors */ + ProximitySensors m_proximitySensors; + + /** + * Constructs the concrete board. + */ + Board() : + m_robot(), + 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(SPEAKER_NAME)), + m_display(m_robot.getDisplay(DISPLAY_NAME)), + m_encoders(m_simTime, m_robot.getPositionSensor(POS_SENSOR_LEFT_NAME), + m_robot.getPositionSensor(POS_SENSOR_RIGHT_NAME)), + m_lineSensors(m_simTime, m_robot.getEmitter(EMITTER_0_NAME), m_robot.getEmitter(EMITTER_1_NAME), + m_robot.getEmitter(EMITTER_2_NAME), m_robot.getEmitter(EMITTER_3_NAME), + m_robot.getEmitter(EMITTER_4_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_0_NAME), + m_robot.getDistanceSensor(LIGHT_SENSOR_1_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_2_NAME), + m_robot.getDistanceSensor(LIGHT_SENSOR_3_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_4_NAME)), + m_motors(m_robot.getMotor(LEFT_MOTOR_NAME), m_robot.getMotor(RIGHT_MOTOR_NAME)), + m_ledRed(m_robot.getLED(LED_RED_NAME)), + m_ledYellow(m_robot.getLED(LED_YELLOW_NAME)), + m_ledGreen(m_robot.getLED(LED_GREEN_NAME)), + m_proximitySensors(m_simTime, m_robot.getDistanceSensor(PROXIMITY_SENSOR_FRONT_LEFT_NAME), + m_robot.getDistanceSensor(PROXIMITY_SENSOR_FRONT_RIGHT_NAME)) + { + } + + /** + * 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; + } + + /** + * 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 */ /** @} */ \ No newline at end of file diff --git a/lib/HALConvoyLeaderSim/library.json b/lib/HALConvoyLeaderSim/library.json index cf950c12..e3386ecf 100644 --- a/lib/HALConvoyLeaderSim/library.json +++ b/lib/HALConvoyLeaderSim/library.json @@ -1,21 +1,21 @@ -{ - "name": "HALConvoyLeaderSim", - "version": "0.1.0", - "description": "...", - "authors": [{ - "name": "Andreas Merkle", - "email": "web@blue-andi.de", - "url": "https://github.com/BlueAndi", - "maintainer": true - }], - "license": "MIT", - "dependencies": [{ - "name": "HALInterfaces" - }, { - "name": "HALSim" - }, { - "name": "Webots" - }], - "frameworks": "*", - "platforms": "*" +{ + "name": "HALConvoyLeaderSim", + "version": "0.1.0", + "description": "...", + "authors": [{ + "name": "Andreas Merkle", + "email": "web@blue-andi.de", + "url": "https://github.com/BlueAndi", + "maintainer": true + }], + "license": "MIT", + "dependencies": [{ + "name": "HALInterfaces" + }, { + "name": "HALSim" + }, { + "name": "Webots" + }], + "frameworks": "*", + "platforms": "*" } \ No newline at end of file diff --git a/lib/HALConvoyLeaderTarget/Board.cpp b/lib/HALConvoyLeaderTarget/Board.cpp index e900fbf9..d4f6bdce 100644 --- a/lib/HALConvoyLeaderTarget/Board.cpp +++ b/lib/HALConvoyLeaderTarget/Board.cpp @@ -1,83 +1,83 @@ -/* MIT License - * - * Copyright (c) 2019 - 2023 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 physical robot board realization. - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -void Board::init() -{ - m_encoders.init(); - m_lineSensors.init(); - m_motors.init(); - m_proximitySensors.initFrontSensor(); -} - -/****************************************************************************** - * Local Functions +/* MIT License + * + * Copyright (c) 2019 - 2023 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 physical robot board realization. + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +void Board::init() +{ + m_encoders.init(); + m_lineSensors.init(); + m_motors.init(); + m_proximitySensors.initFrontSensor(); +} + +/****************************************************************************** + * Local Functions *****************************************************************************/ \ No newline at end of file diff --git a/lib/HALConvoyLeaderTarget/Board.h b/lib/HALConvoyLeaderTarget/Board.h index 15c457af..b258cf31 100644 --- a/lib/HALConvoyLeaderTarget/Board.h +++ b/lib/HALConvoyLeaderTarget/Board.h @@ -1,284 +1,284 @@ -/* MIT License - * - * Copyright (c) 2023 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 physical robot board realization. - * @author Andreas Merkle - * - * @addtogroup HALTarget - * - * @{ - */ -#ifndef BOARD_H -#define BOARD_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** - * The concrete physical board. - */ -class Board -{ -public: - - /** - * Get board instance. - * - * @return Board instance - */ - static Board& getInstance() - { - static Board instance; /* idiom */ - - return instance; - } - - /** - * Initialize the hardware. - */ - void init(); - - /** - * Get button A driver. - * - * @return Button A driver. - */ - IButton& getButtonA() - { - return m_buttonA; - } - - /** - * Get button B driver. - * - * @return Button B driver. - */ - IButton& getButtonB() - { - return m_buttonB; - } - - /** - * Get button C driver. - * - * @return Button C driver. - */ - IButton& getButtonC() - { - return m_buttonC; - } - - /** - * Get buzzer driver. - * - * @return Buzzer driver. - */ - IBuzzer& getBuzzer() - { - return m_buzzer; - } - - /** - * Get LCD driver. - * - * @return LCD driver. - */ - IDisplay& getDisplay() - { - return m_display; - } - - /** - * Get encoders. - * - * @return Encoders driver. - */ - IEncoders& getEncoders() - { - return m_encoders; - } - - /** - * Get line sensors driver. - * - * @return Line sensor driver. - */ - ILineSensors& getLineSensors() - { - return m_lineSensors; - } - - /** - * Get motor driver. - * - * @return Motor driver. - */ - IMotors& getMotors() - { - return m_motors; - } - - /** - * Get red LED driver. - * - * @return Red LED driver. - */ - ILed& getRedLed() - { - return m_ledRed; - } - - /** - * Get yellow LED driver. - * - * @return Yellow LED driver. - */ - ILed& getYellowLed() - { - return m_ledYellow; - } - - /** - * Get green LED driver. - * - * @return Green LED driver. - */ - ILed& getGreenLed() - { - return m_ledGreen; - } - - /** - * Get proximity sensors driver. - * - * @return Proximity sensors driver - */ - IProximitySensors& getProximitySensors() - { - return m_proximitySensors; - } - -protected: - -private: - - /** 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; - - /** Yellow LED driver */ - LedYellow m_ledYellow; - - /** Green LED driver */ - LedGreen m_ledGreen; - - /** Proximity sensors */ - ProximitySensors m_proximitySensors; - - /** - * Constructs the concrete board. - */ - Board() : - m_buttonA(), - m_buttonB(), - m_buttonC(), - m_buzzer(), - m_display(), - m_encoders(), - m_lineSensors(), - m_motors(), - m_ledRed(), - m_ledYellow(), - m_ledGreen(), - m_proximitySensors() - { - } - - /** - * Destroys the concrete board. - */ - ~Board() - { - } - -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* BOARD_H */ +/* MIT License + * + * Copyright (c) 2023 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 physical robot board realization. + * @author Andreas Merkle + * + * @addtogroup HALTarget + * + * @{ + */ +#ifndef BOARD_H +#define BOARD_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** + * The concrete physical board. + */ +class Board +{ +public: + + /** + * Get board instance. + * + * @return Board instance + */ + static Board& getInstance() + { + static Board instance; /* idiom */ + + return instance; + } + + /** + * Initialize the hardware. + */ + void init(); + + /** + * Get button A driver. + * + * @return Button A driver. + */ + IButton& getButtonA() + { + return m_buttonA; + } + + /** + * Get button B driver. + * + * @return Button B driver. + */ + IButton& getButtonB() + { + return m_buttonB; + } + + /** + * Get button C driver. + * + * @return Button C driver. + */ + IButton& getButtonC() + { + return m_buttonC; + } + + /** + * Get buzzer driver. + * + * @return Buzzer driver. + */ + IBuzzer& getBuzzer() + { + return m_buzzer; + } + + /** + * Get LCD driver. + * + * @return LCD driver. + */ + IDisplay& getDisplay() + { + return m_display; + } + + /** + * Get encoders. + * + * @return Encoders driver. + */ + IEncoders& getEncoders() + { + return m_encoders; + } + + /** + * Get line sensors driver. + * + * @return Line sensor driver. + */ + ILineSensors& getLineSensors() + { + return m_lineSensors; + } + + /** + * Get motor driver. + * + * @return Motor driver. + */ + IMotors& getMotors() + { + return m_motors; + } + + /** + * Get red LED driver. + * + * @return Red LED driver. + */ + ILed& getRedLed() + { + return m_ledRed; + } + + /** + * Get yellow LED driver. + * + * @return Yellow LED driver. + */ + ILed& getYellowLed() + { + return m_ledYellow; + } + + /** + * Get green LED driver. + * + * @return Green LED driver. + */ + ILed& getGreenLed() + { + return m_ledGreen; + } + + /** + * Get proximity sensors driver. + * + * @return Proximity sensors driver + */ + IProximitySensors& getProximitySensors() + { + return m_proximitySensors; + } + +protected: + +private: + + /** 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; + + /** Yellow LED driver */ + LedYellow m_ledYellow; + + /** Green LED driver */ + LedGreen m_ledGreen; + + /** Proximity sensors */ + ProximitySensors m_proximitySensors; + + /** + * Constructs the concrete board. + */ + Board() : + m_buttonA(), + m_buttonB(), + m_buttonC(), + m_buzzer(), + m_display(), + m_encoders(), + m_lineSensors(), + m_motors(), + m_ledRed(), + m_ledYellow(), + m_ledGreen(), + m_proximitySensors() + { + } + + /** + * Destroys the concrete board. + */ + ~Board() + { + } + +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* BOARD_H */ /** @} */ \ No newline at end of file diff --git a/lib/HALConvoyLeaderTarget/library.json b/lib/HALConvoyLeaderTarget/library.json index 78ecc094..5177e08f 100644 --- a/lib/HALConvoyLeaderTarget/library.json +++ b/lib/HALConvoyLeaderTarget/library.json @@ -1,23 +1,23 @@ -{ - "name": "HALConvoyLeaderTarget", - "version": "0.1.0", - "description": "...", - "authors": [{ - "name": "Andreas Merkle", - "email": "web@blue-andi.de", - "url": "https://github.com/BlueAndi", - "maintainer": true - }], - "license": "MIT", - "dependencies": [{ - "name": "HALInterfaces" - }, { - "name": "HALTarget" - }, { - "owner": "pololu", - "name": "Zumo32U4", - "version": "^2.0.1" - }], - "frameworks": "arduino", - "platforms": "*" +{ + "name": "HALConvoyLeaderTarget", + "version": "0.1.0", + "description": "...", + "authors": [{ + "name": "Andreas Merkle", + "email": "web@blue-andi.de", + "url": "https://github.com/BlueAndi", + "maintainer": true + }], + "license": "MIT", + "dependencies": [{ + "name": "HALInterfaces" + }, { + "name": "HALTarget" + }, { + "owner": "pololu", + "name": "Zumo32U4", + "version": "^2.0.1" + }], + "frameworks": "arduino", + "platforms": "*" } \ No newline at end of file diff --git a/lib/HALInterfaces/IIMU.h b/lib/HALInterfaces/IIMU.h index 7c685645..e32fc8b4 100644 --- a/lib/HALInterfaces/IIMU.h +++ b/lib/HALInterfaces/IIMU.h @@ -1,178 +1,178 @@ -/* MIT License - * - * Copyright (c) 2023 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 Abstract IMU (Inertial Measurement Unit) interface, depicts Zumo32U4IMU library - * @author Juliane Kerpe - * - * @addtogroup HALInterfaces - * - * @{ - */ -#ifndef IIMU_H -#define IIMU_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** Struct of the raw and not yet converted IMU Data (accelerometer, gyro or magnetometer values) - * in digits in x, y and z direction. */ -typedef struct _IMUData -{ - int16_t valueX; /**< Raw sensor value in x direction in digits */ - int16_t valueY; /**< Raw sensor value in y direction in digits */ - int16_t valueZ; /**< Raw sensor value in z direction in digits */ -} IMUData; - -/** The abstract IMU interface. - * IMU stands for Inertial Measurement Unit. - */ -class IIMU -{ -public: - /** - * Destroys the interface. - */ - virtual ~IIMU() - { - } - - /** - * Initializes the inertial sensors and detects their type. - * - * @return True if the sensor type was detected succesfully; false otherwise. - */ - virtual bool init() = 0; - - /** - * Enables all of the inertial sensors with a default configuration. - */ - virtual void enableDefault() = 0; - - /** - * Configures the sensors with settings optimized for turn sensing. - */ - virtual void configureForTurnSensing() = 0; - - /** - * Takes a reading from the accelerometer and makes the measurements available in a. - */ - virtual void readAccelerometer() = 0; - - /** - * Takes a reading from the gyro and makes the measurements available in g. - */ - virtual void readGyro() = 0; - - /** - * Takes a reading from the magnetometer and makes the measurements available in m. - */ - virtual void readMagnetometer() = 0; - - /** - * Indicates whether the accelerometer has new measurement data ready. - * - * @return True if there is new accelerometer data available; false otherwise. - */ - virtual bool accelerometerDataReady() = 0; - - /** - * Indicates whether the gyro has new measurement data ready. - * - * @return True if there is new gyro data available; false otherwise. - */ - virtual bool gyroDataReady() = 0; - - /** - * Indicates whether the magnetometer has new measurement data ready. - * - * @return True if there is new magnetometer data available; false otherwise. - */ - virtual bool magnetometerDataReady() = 0; - - /** - * Get last raw Accelerometer values as an IMUData struct containing values in x, y and z in digits. - * - * @param[in] accelerationValues Pointer to IMUData struct where the raw acceleration values in digits in - * x, y and z direction will be written into. The values can be converted into physical values in mm/s^2 via the - * multiplication with a sensitivity factor in mm/s^2/digit. - */ - virtual const void getAccelerationValues(IMUData* accelerationValues) = 0; - - /** - * Get last raw Gyro values as an IMUData struct containing values in x, y and z in digits. - * - * @param[in] turnRates Pointer to IMUData struct where the raw turn Rates in digits in x, y and z - * direction will be written into. The values can be converted into physical values in mrad/s via the multiplication - * with a sensitivity factor in mrad/s/digit. - */ - virtual const void getTurnRates(IMUData* turnRates) = 0; - - /** - * Get last raw Magnetometer values as an IMUData struct containing values in x, y and z in digits. - * - * @param[in] magnetometerValues Pointer to IMUData struct where the raw magnetometer values in digits in - * x, y and z direction will be written into. The values can be converted into physical values in mgauss via the - * multiplication with a sensitivity factor in mgauss/digit. - */ - virtual const void getMagnetometerValues(IMUData* magnetometerValues) = 0; - - /** - * Calibrate the IMU. - */ - virtual void calibrate() = 0; - -protected: - /** - * Constructs the interface. - */ - IIMU() - { - } - -private: -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* IIMU_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 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 Abstract IMU (Inertial Measurement Unit) interface, depicts Zumo32U4IMU library + * @author Juliane Kerpe + * + * @addtogroup HALInterfaces + * + * @{ + */ +#ifndef IIMU_H +#define IIMU_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** Struct of the raw and not yet converted IMU Data (accelerometer, gyro or magnetometer values) + * in digits in x, y and z direction. */ +typedef struct _IMUData +{ + int16_t valueX; /**< Raw sensor value in x direction in digits */ + int16_t valueY; /**< Raw sensor value in y direction in digits */ + int16_t valueZ; /**< Raw sensor value in z direction in digits */ +} IMUData; + +/** The abstract IMU interface. + * IMU stands for Inertial Measurement Unit. + */ +class IIMU +{ +public: + /** + * Destroys the interface. + */ + virtual ~IIMU() + { + } + + /** + * Initializes the inertial sensors and detects their type. + * + * @return True if the sensor type was detected succesfully; false otherwise. + */ + virtual bool init() = 0; + + /** + * Enables all of the inertial sensors with a default configuration. + */ + virtual void enableDefault() = 0; + + /** + * Configures the sensors with settings optimized for turn sensing. + */ + virtual void configureForTurnSensing() = 0; + + /** + * Takes a reading from the accelerometer and makes the measurements available in a. + */ + virtual void readAccelerometer() = 0; + + /** + * Takes a reading from the gyro and makes the measurements available in g. + */ + virtual void readGyro() = 0; + + /** + * Takes a reading from the magnetometer and makes the measurements available in m. + */ + virtual void readMagnetometer() = 0; + + /** + * Indicates whether the accelerometer has new measurement data ready. + * + * @return True if there is new accelerometer data available; false otherwise. + */ + virtual bool accelerometerDataReady() = 0; + + /** + * Indicates whether the gyro has new measurement data ready. + * + * @return True if there is new gyro data available; false otherwise. + */ + virtual bool gyroDataReady() = 0; + + /** + * Indicates whether the magnetometer has new measurement data ready. + * + * @return True if there is new magnetometer data available; false otherwise. + */ + virtual bool magnetometerDataReady() = 0; + + /** + * Get last raw Accelerometer values as an IMUData struct containing values in x, y and z in digits. + * + * @param[in] accelerationValues Pointer to IMUData struct where the raw acceleration values in digits in + * x, y and z direction will be written into. The values can be converted into physical values in mm/s^2 via the + * multiplication with a sensitivity factor in mm/s^2/digit. + */ + virtual const void getAccelerationValues(IMUData* accelerationValues) = 0; + + /** + * Get last raw Gyro values as an IMUData struct containing values in x, y and z in digits. + * + * @param[in] turnRates Pointer to IMUData struct where the raw turn Rates in digits in x, y and z + * direction will be written into. The values can be converted into physical values in mrad/s via the multiplication + * with a sensitivity factor in mrad/s/digit. + */ + virtual const void getTurnRates(IMUData* turnRates) = 0; + + /** + * Get last raw Magnetometer values as an IMUData struct containing values in x, y and z in digits. + * + * @param[in] magnetometerValues Pointer to IMUData struct where the raw magnetometer values in digits in + * x, y and z direction will be written into. The values can be converted into physical values in mgauss via the + * multiplication with a sensitivity factor in mgauss/digit. + */ + virtual const void getMagnetometerValues(IMUData* magnetometerValues) = 0; + + /** + * Calibrate the IMU. + */ + virtual void calibrate() = 0; + +protected: + /** + * Constructs the interface. + */ + IIMU() + { + } + +private: +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* IIMU_H */ +/** @} */ diff --git a/lib/HALLineFollowerSim/Board.cpp b/lib/HALLineFollowerSim/Board.cpp index d4a1fd84..abcfb210 100644 --- a/lib/HALLineFollowerSim/Board.cpp +++ b/lib/HALLineFollowerSim/Board.cpp @@ -1,143 +1,143 @@ -/* MIT License - * - * Copyright (c) 2019 - 2023 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 - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/* Name of the speaker in the robot simulation. */ -const char* Board::SPEAKER_NAME = "robot_speaker"; - -/* Name of the display in the robot simulation. */ -const char* Board::DISPLAY_NAME = "robot_display"; - -/* Name of the left motor in the robot simulation. */ -const char* Board::LEFT_MOTOR_NAME = "motor_left"; - -/* Name of the right motor in the robot simulation. */ -const char* Board::RIGHT_MOTOR_NAME = "motor_right"; - -/* Name of the infrared emitter 0 in the robot simulation. */ -const char* Board::EMITTER_0_NAME = "emitter_l"; - -/* Name of the infrared emitter 1 in the robot simulation. */ -const char* Board::EMITTER_1_NAME = "emitter_lm"; - -/* Name of the infrared emitter 2 in the robot simulation. */ -const char* Board::EMITTER_2_NAME = "emitter_m"; - -/* Name of the infrared emitter 3 in the robot simulation. */ -const char* Board::EMITTER_3_NAME = "emitter_rm"; - -/* Name of the infrared emitter 4 in the robot simulation. */ -const char* Board::EMITTER_4_NAME = "emitter_r"; - -/** Name of the position sensor of the left motor in the robot simulation. */ -const char* Board::POS_SENSOR_LEFT_NAME = "position_sensor_left"; - -/** Name of the position sensor of the right motor in the robot simulation. */ -const char* Board::POS_SENSOR_RIGHT_NAME = "position_sensor_right"; - -/* Name of the light sensor 0 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_0_NAME = "lightsensor_l"; - -/* Name of the light sensor 1 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_1_NAME = "lightsensor_lm"; - -/* Name of the light sensor 2 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_2_NAME = "lightsensor_m"; - -/* Name of the light sensor 3 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_3_NAME = "lightsensor_rm"; - -/* Name of the light sensor 4 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_4_NAME = "lightsensor_r"; - -/* Name of the red LED in the robot simulation. */ -const char* Board::LED_RED_NAME = "led_red"; - -/* Name of the yellow LED in the robot simulation. */ -const char* Board::LED_YELLOW_NAME = "led_yellow"; - -/* Name of the green LED in the robot simulation. */ -const char* Board::LED_GREEN_NAME = "led_green"; - -/* Name of the front left proximity sensor in the robot simulation. */ -const char* Board::PROXIMITY_SENSOR_FRONT_LEFT_NAME = "proxim_sensor_fl"; - -/* Name of the front right proximity sensor in the robot simulation. */ -const char* Board::PROXIMITY_SENSOR_FRONT_RIGHT_NAME = "proxim_sensor_fr"; - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -void Board::init() -{ - m_encoders.init(); - m_keyboard.init(); - m_lineSensors.init(); - m_motors.init(); - m_proximitySensors.initFrontSensor(); -} - -/****************************************************************************** - * Local Functions +/* MIT License + * + * Copyright (c) 2019 - 2023 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 + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/* Name of the speaker in the robot simulation. */ +const char* Board::SPEAKER_NAME = "robot_speaker"; + +/* Name of the display in the robot simulation. */ +const char* Board::DISPLAY_NAME = "robot_display"; + +/* Name of the left motor in the robot simulation. */ +const char* Board::LEFT_MOTOR_NAME = "motor_left"; + +/* Name of the right motor in the robot simulation. */ +const char* Board::RIGHT_MOTOR_NAME = "motor_right"; + +/* Name of the infrared emitter 0 in the robot simulation. */ +const char* Board::EMITTER_0_NAME = "emitter_l"; + +/* Name of the infrared emitter 1 in the robot simulation. */ +const char* Board::EMITTER_1_NAME = "emitter_lm"; + +/* Name of the infrared emitter 2 in the robot simulation. */ +const char* Board::EMITTER_2_NAME = "emitter_m"; + +/* Name of the infrared emitter 3 in the robot simulation. */ +const char* Board::EMITTER_3_NAME = "emitter_rm"; + +/* Name of the infrared emitter 4 in the robot simulation. */ +const char* Board::EMITTER_4_NAME = "emitter_r"; + +/** Name of the position sensor of the left motor in the robot simulation. */ +const char* Board::POS_SENSOR_LEFT_NAME = "position_sensor_left"; + +/** Name of the position sensor of the right motor in the robot simulation. */ +const char* Board::POS_SENSOR_RIGHT_NAME = "position_sensor_right"; + +/* Name of the light sensor 0 in the robot simulation. */ +const char* Board::LIGHT_SENSOR_0_NAME = "lightsensor_l"; + +/* Name of the light sensor 1 in the robot simulation. */ +const char* Board::LIGHT_SENSOR_1_NAME = "lightsensor_lm"; + +/* Name of the light sensor 2 in the robot simulation. */ +const char* Board::LIGHT_SENSOR_2_NAME = "lightsensor_m"; + +/* Name of the light sensor 3 in the robot simulation. */ +const char* Board::LIGHT_SENSOR_3_NAME = "lightsensor_rm"; + +/* Name of the light sensor 4 in the robot simulation. */ +const char* Board::LIGHT_SENSOR_4_NAME = "lightsensor_r"; + +/* Name of the red LED in the robot simulation. */ +const char* Board::LED_RED_NAME = "led_red"; + +/* Name of the yellow LED in the robot simulation. */ +const char* Board::LED_YELLOW_NAME = "led_yellow"; + +/* Name of the green LED in the robot simulation. */ +const char* Board::LED_GREEN_NAME = "led_green"; + +/* Name of the front left proximity sensor in the robot simulation. */ +const char* Board::PROXIMITY_SENSOR_FRONT_LEFT_NAME = "proxim_sensor_fl"; + +/* Name of the front right proximity sensor in the robot simulation. */ +const char* Board::PROXIMITY_SENSOR_FRONT_RIGHT_NAME = "proxim_sensor_fr"; + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +void Board::init() +{ + m_encoders.init(); + m_keyboard.init(); + m_lineSensors.init(); + m_motors.init(); + m_proximitySensors.initFrontSensor(); +} + +/****************************************************************************** + * Local Functions *****************************************************************************/ \ No newline at end of file diff --git a/lib/HALLineFollowerSim/Board.h b/lib/HALLineFollowerSim/Board.h index 33c20b6e..6ee59544 100644 --- a/lib/HALLineFollowerSim/Board.h +++ b/lib/HALLineFollowerSim/Board.h @@ -1,398 +1,398 @@ -/* MIT License - * - * Copyright (c) 2023 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 - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** - * The concrete simulation robot board. - */ -class Board -{ -public: - /** - * Get board instance. - * - * @return Board instance - */ - static Board& getInstance() - { - static Board instance; /* idiom */ - - return instance; - } - - /** - * Initialize the hardware. - */ - void init(); - - /** - * Get button A driver. - * - * @return Button A driver. - */ - IButton& getButtonA() - { - return m_buttonA; - } - - /** - * Get button B driver. - * - * @return Button B driver. - */ - IButton& getButtonB() - { - return m_buttonB; - } - - /** - * Get button C driver. - * - * @return Button C driver. - */ - IButton& getButtonC() - { - return m_buttonC; - } - - /** - * Get buzzer driver. - * - * @return Buzzer driver. - */ - IBuzzer& getBuzzer() - { - return m_buzzer; - } - - /** - * Get LCD driver. - * - * @return LCD driver. - */ - IDisplay& getDisplay() - { - return m_display; - } - - /** - * Get encoders. - * - * @return Encoders driver. - */ - IEncoders& getEncoders() - { - return m_encoders; - } - - /** - * Get line sensors driver. - * - * @return Line sensor driver. - */ - ILineSensors& getLineSensors() - { - return m_lineSensors; - } - - /** - * Get motor driver. - * - * @return Motor driver. - */ - IMotors& getMotors() - { - return m_motors; - } - - /** - * Get red LED driver. - * - * @return Red LED driver. - */ - ILed& getRedLed() - { - return m_ledRed; - } - - /** - * Get yellow LED driver. - * - * @return Yellow LED driver. - */ - ILed& getYellowLed() - { - return m_ledYellow; - } - - /** - * Get green LED driver. - * - * @return Green LED driver. - */ - ILed& getGreenLed() - { - return m_ledGreen; - } - - /** - * Get proximity sensors driver. - * - * @return Proximity sensors driver - */ - IProximitySensors& getProximitySensors() - { - return m_proximitySensors; - } - -protected: -private: - /** Name of the speaker in the robot simulation. */ - static const char* SPEAKER_NAME; - - /** Name of the display in the robot simulation. */ - static const char* DISPLAY_NAME; - - /** Name of the left motor in the robot simulation. */ - static const char* LEFT_MOTOR_NAME; - - /** Name of the right motor in the robot simulation. */ - static const char* RIGHT_MOTOR_NAME; - - /** Name of the infrared emitter 0 in the robot simulation. */ - static const char* EMITTER_0_NAME; - - /** Name of the infrared emitter 1 in the robot simulation. */ - static const char* EMITTER_1_NAME; - - /** Name of the infrared emitter 2 in the robot simulation. */ - static const char* EMITTER_2_NAME; - - /** Name of the infrared emitter 3 in the robot simulation. */ - static const char* EMITTER_3_NAME; - - /** Name of the infrared emitter 4 in the robot simulation. */ - static const char* EMITTER_4_NAME; - - /** Name of the position sensor of the left motor in the robot simulation. */ - static const char* POS_SENSOR_LEFT_NAME; - - /** Name of the position sensor of the right motor in the robot simulation. */ - static const char* POS_SENSOR_RIGHT_NAME; - - /** Name of the light sensor 0 in the robot simulation. */ - static const char* LIGHT_SENSOR_0_NAME; - - /** Name of the light sensor 1 in the robot simulation. */ - static const char* LIGHT_SENSOR_1_NAME; - - /** Name of the light sensor 2 in the robot simulation. */ - static const char* LIGHT_SENSOR_2_NAME; - - /** Name of the light sensor 3 in the robot simulation. */ - static const char* LIGHT_SENSOR_3_NAME; - - /** Name of the light sensor 4 in the robot simulation. */ - static const char* LIGHT_SENSOR_4_NAME; - - /** Name of the red LED in the robot simulation. */ - static const char* LED_RED_NAME; - - /** Name of the yellow LED in the robot simulation. */ - static const char* LED_YELLOW_NAME; - - /** Name of the green LED in the robot simulation. */ - static const char* LED_GREEN_NAME; - - /** Name of the front proximity sensor in the robot simulation. */ - static const char* PROXIMITY_SENSOR_FRONT_LEFT_NAME; - - /** Name of the front right proximity sensor in the robot simulation. */ - static const char* PROXIMITY_SENSOR_FRONT_RIGHT_NAME; - - /** 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; - - /** Yellow LED driver */ - LedYellow m_ledYellow; - - /** Green LED driver */ - LedGreen m_ledGreen; - - /** Proximity sensors */ - ProximitySensors m_proximitySensors; - - /** - * Constructs the concrete board. - */ - Board() : - m_robot(), - 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(SPEAKER_NAME)), - m_display(m_robot.getDisplay(DISPLAY_NAME)), - m_encoders(m_simTime, m_robot.getPositionSensor(POS_SENSOR_LEFT_NAME), - m_robot.getPositionSensor(POS_SENSOR_RIGHT_NAME)), - m_lineSensors(m_simTime, m_robot.getEmitter(EMITTER_0_NAME), m_robot.getEmitter(EMITTER_1_NAME), - m_robot.getEmitter(EMITTER_2_NAME), m_robot.getEmitter(EMITTER_3_NAME), - m_robot.getEmitter(EMITTER_4_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_0_NAME), - m_robot.getDistanceSensor(LIGHT_SENSOR_1_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_2_NAME), - m_robot.getDistanceSensor(LIGHT_SENSOR_3_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_4_NAME)), - m_motors(m_robot.getMotor(LEFT_MOTOR_NAME), m_robot.getMotor(RIGHT_MOTOR_NAME)), - m_ledRed(m_robot.getLED(LED_RED_NAME)), - m_ledYellow(m_robot.getLED(LED_YELLOW_NAME)), - m_ledGreen(m_robot.getLED(LED_GREEN_NAME)), - m_proximitySensors(m_simTime, m_robot.getDistanceSensor(PROXIMITY_SENSOR_FRONT_LEFT_NAME), - m_robot.getDistanceSensor(PROXIMITY_SENSOR_FRONT_RIGHT_NAME)) - { - } - - /** - * 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; - } - - /** - * 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 */ +/* MIT License + * + * Copyright (c) 2023 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 + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** + * The concrete simulation robot board. + */ +class Board +{ +public: + /** + * Get board instance. + * + * @return Board instance + */ + static Board& getInstance() + { + static Board instance; /* idiom */ + + return instance; + } + + /** + * Initialize the hardware. + */ + void init(); + + /** + * Get button A driver. + * + * @return Button A driver. + */ + IButton& getButtonA() + { + return m_buttonA; + } + + /** + * Get button B driver. + * + * @return Button B driver. + */ + IButton& getButtonB() + { + return m_buttonB; + } + + /** + * Get button C driver. + * + * @return Button C driver. + */ + IButton& getButtonC() + { + return m_buttonC; + } + + /** + * Get buzzer driver. + * + * @return Buzzer driver. + */ + IBuzzer& getBuzzer() + { + return m_buzzer; + } + + /** + * Get LCD driver. + * + * @return LCD driver. + */ + IDisplay& getDisplay() + { + return m_display; + } + + /** + * Get encoders. + * + * @return Encoders driver. + */ + IEncoders& getEncoders() + { + return m_encoders; + } + + /** + * Get line sensors driver. + * + * @return Line sensor driver. + */ + ILineSensors& getLineSensors() + { + return m_lineSensors; + } + + /** + * Get motor driver. + * + * @return Motor driver. + */ + IMotors& getMotors() + { + return m_motors; + } + + /** + * Get red LED driver. + * + * @return Red LED driver. + */ + ILed& getRedLed() + { + return m_ledRed; + } + + /** + * Get yellow LED driver. + * + * @return Yellow LED driver. + */ + ILed& getYellowLed() + { + return m_ledYellow; + } + + /** + * Get green LED driver. + * + * @return Green LED driver. + */ + ILed& getGreenLed() + { + return m_ledGreen; + } + + /** + * Get proximity sensors driver. + * + * @return Proximity sensors driver + */ + IProximitySensors& getProximitySensors() + { + return m_proximitySensors; + } + +protected: +private: + /** Name of the speaker in the robot simulation. */ + static const char* SPEAKER_NAME; + + /** Name of the display in the robot simulation. */ + static const char* DISPLAY_NAME; + + /** Name of the left motor in the robot simulation. */ + static const char* LEFT_MOTOR_NAME; + + /** Name of the right motor in the robot simulation. */ + static const char* RIGHT_MOTOR_NAME; + + /** Name of the infrared emitter 0 in the robot simulation. */ + static const char* EMITTER_0_NAME; + + /** Name of the infrared emitter 1 in the robot simulation. */ + static const char* EMITTER_1_NAME; + + /** Name of the infrared emitter 2 in the robot simulation. */ + static const char* EMITTER_2_NAME; + + /** Name of the infrared emitter 3 in the robot simulation. */ + static const char* EMITTER_3_NAME; + + /** Name of the infrared emitter 4 in the robot simulation. */ + static const char* EMITTER_4_NAME; + + /** Name of the position sensor of the left motor in the robot simulation. */ + static const char* POS_SENSOR_LEFT_NAME; + + /** Name of the position sensor of the right motor in the robot simulation. */ + static const char* POS_SENSOR_RIGHT_NAME; + + /** Name of the light sensor 0 in the robot simulation. */ + static const char* LIGHT_SENSOR_0_NAME; + + /** Name of the light sensor 1 in the robot simulation. */ + static const char* LIGHT_SENSOR_1_NAME; + + /** Name of the light sensor 2 in the robot simulation. */ + static const char* LIGHT_SENSOR_2_NAME; + + /** Name of the light sensor 3 in the robot simulation. */ + static const char* LIGHT_SENSOR_3_NAME; + + /** Name of the light sensor 4 in the robot simulation. */ + static const char* LIGHT_SENSOR_4_NAME; + + /** Name of the red LED in the robot simulation. */ + static const char* LED_RED_NAME; + + /** Name of the yellow LED in the robot simulation. */ + static const char* LED_YELLOW_NAME; + + /** Name of the green LED in the robot simulation. */ + static const char* LED_GREEN_NAME; + + /** Name of the front proximity sensor in the robot simulation. */ + static const char* PROXIMITY_SENSOR_FRONT_LEFT_NAME; + + /** Name of the front right proximity sensor in the robot simulation. */ + static const char* PROXIMITY_SENSOR_FRONT_RIGHT_NAME; + + /** 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; + + /** Yellow LED driver */ + LedYellow m_ledYellow; + + /** Green LED driver */ + LedGreen m_ledGreen; + + /** Proximity sensors */ + ProximitySensors m_proximitySensors; + + /** + * Constructs the concrete board. + */ + Board() : + m_robot(), + 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(SPEAKER_NAME)), + m_display(m_robot.getDisplay(DISPLAY_NAME)), + m_encoders(m_simTime, m_robot.getPositionSensor(POS_SENSOR_LEFT_NAME), + m_robot.getPositionSensor(POS_SENSOR_RIGHT_NAME)), + m_lineSensors(m_simTime, m_robot.getEmitter(EMITTER_0_NAME), m_robot.getEmitter(EMITTER_1_NAME), + m_robot.getEmitter(EMITTER_2_NAME), m_robot.getEmitter(EMITTER_3_NAME), + m_robot.getEmitter(EMITTER_4_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_0_NAME), + m_robot.getDistanceSensor(LIGHT_SENSOR_1_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_2_NAME), + m_robot.getDistanceSensor(LIGHT_SENSOR_3_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_4_NAME)), + m_motors(m_robot.getMotor(LEFT_MOTOR_NAME), m_robot.getMotor(RIGHT_MOTOR_NAME)), + m_ledRed(m_robot.getLED(LED_RED_NAME)), + m_ledYellow(m_robot.getLED(LED_YELLOW_NAME)), + m_ledGreen(m_robot.getLED(LED_GREEN_NAME)), + m_proximitySensors(m_simTime, m_robot.getDistanceSensor(PROXIMITY_SENSOR_FRONT_LEFT_NAME), + m_robot.getDistanceSensor(PROXIMITY_SENSOR_FRONT_RIGHT_NAME)) + { + } + + /** + * 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; + } + + /** + * 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 */ /** @} */ \ No newline at end of file diff --git a/lib/HALLineFollowerSim/library.json b/lib/HALLineFollowerSim/library.json index fd925552..1db98687 100644 --- a/lib/HALLineFollowerSim/library.json +++ b/lib/HALLineFollowerSim/library.json @@ -1,21 +1,21 @@ -{ - "name": "HALLineFollowerSim", - "version": "0.1.0", - "description": "...", - "authors": [{ - "name": "Andreas Merkle", - "email": "web@blue-andi.de", - "url": "https://github.com/BlueAndi", - "maintainer": true - }], - "license": "MIT", - "dependencies": [{ - "name": "HALInterfaces" - }, { - "name": "HALSim" - }, { - "name": "Webots" - }], - "frameworks": "*", - "platforms": "*" +{ + "name": "HALLineFollowerSim", + "version": "0.1.0", + "description": "...", + "authors": [{ + "name": "Andreas Merkle", + "email": "web@blue-andi.de", + "url": "https://github.com/BlueAndi", + "maintainer": true + }], + "license": "MIT", + "dependencies": [{ + "name": "HALInterfaces" + }, { + "name": "HALSim" + }, { + "name": "Webots" + }], + "frameworks": "*", + "platforms": "*" } \ No newline at end of file diff --git a/lib/HALLineFollowerTarget/Board.cpp b/lib/HALLineFollowerTarget/Board.cpp index e900fbf9..d4f6bdce 100644 --- a/lib/HALLineFollowerTarget/Board.cpp +++ b/lib/HALLineFollowerTarget/Board.cpp @@ -1,83 +1,83 @@ -/* MIT License - * - * Copyright (c) 2019 - 2023 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 physical robot board realization. - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -void Board::init() -{ - m_encoders.init(); - m_lineSensors.init(); - m_motors.init(); - m_proximitySensors.initFrontSensor(); -} - -/****************************************************************************** - * Local Functions +/* MIT License + * + * Copyright (c) 2019 - 2023 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 physical robot board realization. + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +void Board::init() +{ + m_encoders.init(); + m_lineSensors.init(); + m_motors.init(); + m_proximitySensors.initFrontSensor(); +} + +/****************************************************************************** + * Local Functions *****************************************************************************/ \ No newline at end of file diff --git a/lib/HALLineFollowerTarget/Board.h b/lib/HALLineFollowerTarget/Board.h index 15c457af..b258cf31 100644 --- a/lib/HALLineFollowerTarget/Board.h +++ b/lib/HALLineFollowerTarget/Board.h @@ -1,284 +1,284 @@ -/* MIT License - * - * Copyright (c) 2023 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 physical robot board realization. - * @author Andreas Merkle - * - * @addtogroup HALTarget - * - * @{ - */ -#ifndef BOARD_H -#define BOARD_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** - * The concrete physical board. - */ -class Board -{ -public: - - /** - * Get board instance. - * - * @return Board instance - */ - static Board& getInstance() - { - static Board instance; /* idiom */ - - return instance; - } - - /** - * Initialize the hardware. - */ - void init(); - - /** - * Get button A driver. - * - * @return Button A driver. - */ - IButton& getButtonA() - { - return m_buttonA; - } - - /** - * Get button B driver. - * - * @return Button B driver. - */ - IButton& getButtonB() - { - return m_buttonB; - } - - /** - * Get button C driver. - * - * @return Button C driver. - */ - IButton& getButtonC() - { - return m_buttonC; - } - - /** - * Get buzzer driver. - * - * @return Buzzer driver. - */ - IBuzzer& getBuzzer() - { - return m_buzzer; - } - - /** - * Get LCD driver. - * - * @return LCD driver. - */ - IDisplay& getDisplay() - { - return m_display; - } - - /** - * Get encoders. - * - * @return Encoders driver. - */ - IEncoders& getEncoders() - { - return m_encoders; - } - - /** - * Get line sensors driver. - * - * @return Line sensor driver. - */ - ILineSensors& getLineSensors() - { - return m_lineSensors; - } - - /** - * Get motor driver. - * - * @return Motor driver. - */ - IMotors& getMotors() - { - return m_motors; - } - - /** - * Get red LED driver. - * - * @return Red LED driver. - */ - ILed& getRedLed() - { - return m_ledRed; - } - - /** - * Get yellow LED driver. - * - * @return Yellow LED driver. - */ - ILed& getYellowLed() - { - return m_ledYellow; - } - - /** - * Get green LED driver. - * - * @return Green LED driver. - */ - ILed& getGreenLed() - { - return m_ledGreen; - } - - /** - * Get proximity sensors driver. - * - * @return Proximity sensors driver - */ - IProximitySensors& getProximitySensors() - { - return m_proximitySensors; - } - -protected: - -private: - - /** 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; - - /** Yellow LED driver */ - LedYellow m_ledYellow; - - /** Green LED driver */ - LedGreen m_ledGreen; - - /** Proximity sensors */ - ProximitySensors m_proximitySensors; - - /** - * Constructs the concrete board. - */ - Board() : - m_buttonA(), - m_buttonB(), - m_buttonC(), - m_buzzer(), - m_display(), - m_encoders(), - m_lineSensors(), - m_motors(), - m_ledRed(), - m_ledYellow(), - m_ledGreen(), - m_proximitySensors() - { - } - - /** - * Destroys the concrete board. - */ - ~Board() - { - } - -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* BOARD_H */ +/* MIT License + * + * Copyright (c) 2023 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 physical robot board realization. + * @author Andreas Merkle + * + * @addtogroup HALTarget + * + * @{ + */ +#ifndef BOARD_H +#define BOARD_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** + * The concrete physical board. + */ +class Board +{ +public: + + /** + * Get board instance. + * + * @return Board instance + */ + static Board& getInstance() + { + static Board instance; /* idiom */ + + return instance; + } + + /** + * Initialize the hardware. + */ + void init(); + + /** + * Get button A driver. + * + * @return Button A driver. + */ + IButton& getButtonA() + { + return m_buttonA; + } + + /** + * Get button B driver. + * + * @return Button B driver. + */ + IButton& getButtonB() + { + return m_buttonB; + } + + /** + * Get button C driver. + * + * @return Button C driver. + */ + IButton& getButtonC() + { + return m_buttonC; + } + + /** + * Get buzzer driver. + * + * @return Buzzer driver. + */ + IBuzzer& getBuzzer() + { + return m_buzzer; + } + + /** + * Get LCD driver. + * + * @return LCD driver. + */ + IDisplay& getDisplay() + { + return m_display; + } + + /** + * Get encoders. + * + * @return Encoders driver. + */ + IEncoders& getEncoders() + { + return m_encoders; + } + + /** + * Get line sensors driver. + * + * @return Line sensor driver. + */ + ILineSensors& getLineSensors() + { + return m_lineSensors; + } + + /** + * Get motor driver. + * + * @return Motor driver. + */ + IMotors& getMotors() + { + return m_motors; + } + + /** + * Get red LED driver. + * + * @return Red LED driver. + */ + ILed& getRedLed() + { + return m_ledRed; + } + + /** + * Get yellow LED driver. + * + * @return Yellow LED driver. + */ + ILed& getYellowLed() + { + return m_ledYellow; + } + + /** + * Get green LED driver. + * + * @return Green LED driver. + */ + ILed& getGreenLed() + { + return m_ledGreen; + } + + /** + * Get proximity sensors driver. + * + * @return Proximity sensors driver + */ + IProximitySensors& getProximitySensors() + { + return m_proximitySensors; + } + +protected: + +private: + + /** 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; + + /** Yellow LED driver */ + LedYellow m_ledYellow; + + /** Green LED driver */ + LedGreen m_ledGreen; + + /** Proximity sensors */ + ProximitySensors m_proximitySensors; + + /** + * Constructs the concrete board. + */ + Board() : + m_buttonA(), + m_buttonB(), + m_buttonC(), + m_buzzer(), + m_display(), + m_encoders(), + m_lineSensors(), + m_motors(), + m_ledRed(), + m_ledYellow(), + m_ledGreen(), + m_proximitySensors() + { + } + + /** + * Destroys the concrete board. + */ + ~Board() + { + } + +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* BOARD_H */ /** @} */ \ No newline at end of file diff --git a/lib/HALLineFollowerTarget/library.json b/lib/HALLineFollowerTarget/library.json index bf6ef9c3..00083a5d 100644 --- a/lib/HALLineFollowerTarget/library.json +++ b/lib/HALLineFollowerTarget/library.json @@ -1,23 +1,23 @@ -{ - "name": "HALLineFollowerTarget", - "version": "0.1.0", - "description": "...", - "authors": [{ - "name": "Andreas Merkle", - "email": "web@blue-andi.de", - "url": "https://github.com/BlueAndi", - "maintainer": true - }], - "license": "MIT", - "dependencies": [{ - "name": "HALInterfaces" - }, { - "name": "HALTarget" - }, { - "owner": "pololu", - "name": "Zumo32U4", - "version": "^2.0.1" - }], - "frameworks": "arduino", - "platforms": "*" +{ + "name": "HALLineFollowerTarget", + "version": "0.1.0", + "description": "...", + "authors": [{ + "name": "Andreas Merkle", + "email": "web@blue-andi.de", + "url": "https://github.com/BlueAndi", + "maintainer": true + }], + "license": "MIT", + "dependencies": [{ + "name": "HALInterfaces" + }, { + "name": "HALTarget" + }, { + "owner": "pololu", + "name": "Zumo32U4", + "version": "^2.0.1" + }], + "frameworks": "arduino", + "platforms": "*" } \ No newline at end of file diff --git a/lib/HALRemoteControlSim/Board.cpp b/lib/HALRemoteControlSim/Board.cpp index d4a1fd84..abcfb210 100644 --- a/lib/HALRemoteControlSim/Board.cpp +++ b/lib/HALRemoteControlSim/Board.cpp @@ -1,143 +1,143 @@ -/* MIT License - * - * Copyright (c) 2019 - 2023 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 - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/* Name of the speaker in the robot simulation. */ -const char* Board::SPEAKER_NAME = "robot_speaker"; - -/* Name of the display in the robot simulation. */ -const char* Board::DISPLAY_NAME = "robot_display"; - -/* Name of the left motor in the robot simulation. */ -const char* Board::LEFT_MOTOR_NAME = "motor_left"; - -/* Name of the right motor in the robot simulation. */ -const char* Board::RIGHT_MOTOR_NAME = "motor_right"; - -/* Name of the infrared emitter 0 in the robot simulation. */ -const char* Board::EMITTER_0_NAME = "emitter_l"; - -/* Name of the infrared emitter 1 in the robot simulation. */ -const char* Board::EMITTER_1_NAME = "emitter_lm"; - -/* Name of the infrared emitter 2 in the robot simulation. */ -const char* Board::EMITTER_2_NAME = "emitter_m"; - -/* Name of the infrared emitter 3 in the robot simulation. */ -const char* Board::EMITTER_3_NAME = "emitter_rm"; - -/* Name of the infrared emitter 4 in the robot simulation. */ -const char* Board::EMITTER_4_NAME = "emitter_r"; - -/** Name of the position sensor of the left motor in the robot simulation. */ -const char* Board::POS_SENSOR_LEFT_NAME = "position_sensor_left"; - -/** Name of the position sensor of the right motor in the robot simulation. */ -const char* Board::POS_SENSOR_RIGHT_NAME = "position_sensor_right"; - -/* Name of the light sensor 0 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_0_NAME = "lightsensor_l"; - -/* Name of the light sensor 1 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_1_NAME = "lightsensor_lm"; - -/* Name of the light sensor 2 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_2_NAME = "lightsensor_m"; - -/* Name of the light sensor 3 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_3_NAME = "lightsensor_rm"; - -/* Name of the light sensor 4 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_4_NAME = "lightsensor_r"; - -/* Name of the red LED in the robot simulation. */ -const char* Board::LED_RED_NAME = "led_red"; - -/* Name of the yellow LED in the robot simulation. */ -const char* Board::LED_YELLOW_NAME = "led_yellow"; - -/* Name of the green LED in the robot simulation. */ -const char* Board::LED_GREEN_NAME = "led_green"; - -/* Name of the front left proximity sensor in the robot simulation. */ -const char* Board::PROXIMITY_SENSOR_FRONT_LEFT_NAME = "proxim_sensor_fl"; - -/* Name of the front right proximity sensor in the robot simulation. */ -const char* Board::PROXIMITY_SENSOR_FRONT_RIGHT_NAME = "proxim_sensor_fr"; - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -void Board::init() -{ - m_encoders.init(); - m_keyboard.init(); - m_lineSensors.init(); - m_motors.init(); - m_proximitySensors.initFrontSensor(); -} - -/****************************************************************************** - * Local Functions +/* MIT License + * + * Copyright (c) 2019 - 2023 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 + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/* Name of the speaker in the robot simulation. */ +const char* Board::SPEAKER_NAME = "robot_speaker"; + +/* Name of the display in the robot simulation. */ +const char* Board::DISPLAY_NAME = "robot_display"; + +/* Name of the left motor in the robot simulation. */ +const char* Board::LEFT_MOTOR_NAME = "motor_left"; + +/* Name of the right motor in the robot simulation. */ +const char* Board::RIGHT_MOTOR_NAME = "motor_right"; + +/* Name of the infrared emitter 0 in the robot simulation. */ +const char* Board::EMITTER_0_NAME = "emitter_l"; + +/* Name of the infrared emitter 1 in the robot simulation. */ +const char* Board::EMITTER_1_NAME = "emitter_lm"; + +/* Name of the infrared emitter 2 in the robot simulation. */ +const char* Board::EMITTER_2_NAME = "emitter_m"; + +/* Name of the infrared emitter 3 in the robot simulation. */ +const char* Board::EMITTER_3_NAME = "emitter_rm"; + +/* Name of the infrared emitter 4 in the robot simulation. */ +const char* Board::EMITTER_4_NAME = "emitter_r"; + +/** Name of the position sensor of the left motor in the robot simulation. */ +const char* Board::POS_SENSOR_LEFT_NAME = "position_sensor_left"; + +/** Name of the position sensor of the right motor in the robot simulation. */ +const char* Board::POS_SENSOR_RIGHT_NAME = "position_sensor_right"; + +/* Name of the light sensor 0 in the robot simulation. */ +const char* Board::LIGHT_SENSOR_0_NAME = "lightsensor_l"; + +/* Name of the light sensor 1 in the robot simulation. */ +const char* Board::LIGHT_SENSOR_1_NAME = "lightsensor_lm"; + +/* Name of the light sensor 2 in the robot simulation. */ +const char* Board::LIGHT_SENSOR_2_NAME = "lightsensor_m"; + +/* Name of the light sensor 3 in the robot simulation. */ +const char* Board::LIGHT_SENSOR_3_NAME = "lightsensor_rm"; + +/* Name of the light sensor 4 in the robot simulation. */ +const char* Board::LIGHT_SENSOR_4_NAME = "lightsensor_r"; + +/* Name of the red LED in the robot simulation. */ +const char* Board::LED_RED_NAME = "led_red"; + +/* Name of the yellow LED in the robot simulation. */ +const char* Board::LED_YELLOW_NAME = "led_yellow"; + +/* Name of the green LED in the robot simulation. */ +const char* Board::LED_GREEN_NAME = "led_green"; + +/* Name of the front left proximity sensor in the robot simulation. */ +const char* Board::PROXIMITY_SENSOR_FRONT_LEFT_NAME = "proxim_sensor_fl"; + +/* Name of the front right proximity sensor in the robot simulation. */ +const char* Board::PROXIMITY_SENSOR_FRONT_RIGHT_NAME = "proxim_sensor_fr"; + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +void Board::init() +{ + m_encoders.init(); + m_keyboard.init(); + m_lineSensors.init(); + m_motors.init(); + m_proximitySensors.initFrontSensor(); +} + +/****************************************************************************** + * Local Functions *****************************************************************************/ \ No newline at end of file diff --git a/lib/HALRemoteControlSim/Board.h b/lib/HALRemoteControlSim/Board.h index 33c20b6e..6ee59544 100644 --- a/lib/HALRemoteControlSim/Board.h +++ b/lib/HALRemoteControlSim/Board.h @@ -1,398 +1,398 @@ -/* MIT License - * - * Copyright (c) 2023 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 - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** - * The concrete simulation robot board. - */ -class Board -{ -public: - /** - * Get board instance. - * - * @return Board instance - */ - static Board& getInstance() - { - static Board instance; /* idiom */ - - return instance; - } - - /** - * Initialize the hardware. - */ - void init(); - - /** - * Get button A driver. - * - * @return Button A driver. - */ - IButton& getButtonA() - { - return m_buttonA; - } - - /** - * Get button B driver. - * - * @return Button B driver. - */ - IButton& getButtonB() - { - return m_buttonB; - } - - /** - * Get button C driver. - * - * @return Button C driver. - */ - IButton& getButtonC() - { - return m_buttonC; - } - - /** - * Get buzzer driver. - * - * @return Buzzer driver. - */ - IBuzzer& getBuzzer() - { - return m_buzzer; - } - - /** - * Get LCD driver. - * - * @return LCD driver. - */ - IDisplay& getDisplay() - { - return m_display; - } - - /** - * Get encoders. - * - * @return Encoders driver. - */ - IEncoders& getEncoders() - { - return m_encoders; - } - - /** - * Get line sensors driver. - * - * @return Line sensor driver. - */ - ILineSensors& getLineSensors() - { - return m_lineSensors; - } - - /** - * Get motor driver. - * - * @return Motor driver. - */ - IMotors& getMotors() - { - return m_motors; - } - - /** - * Get red LED driver. - * - * @return Red LED driver. - */ - ILed& getRedLed() - { - return m_ledRed; - } - - /** - * Get yellow LED driver. - * - * @return Yellow LED driver. - */ - ILed& getYellowLed() - { - return m_ledYellow; - } - - /** - * Get green LED driver. - * - * @return Green LED driver. - */ - ILed& getGreenLed() - { - return m_ledGreen; - } - - /** - * Get proximity sensors driver. - * - * @return Proximity sensors driver - */ - IProximitySensors& getProximitySensors() - { - return m_proximitySensors; - } - -protected: -private: - /** Name of the speaker in the robot simulation. */ - static const char* SPEAKER_NAME; - - /** Name of the display in the robot simulation. */ - static const char* DISPLAY_NAME; - - /** Name of the left motor in the robot simulation. */ - static const char* LEFT_MOTOR_NAME; - - /** Name of the right motor in the robot simulation. */ - static const char* RIGHT_MOTOR_NAME; - - /** Name of the infrared emitter 0 in the robot simulation. */ - static const char* EMITTER_0_NAME; - - /** Name of the infrared emitter 1 in the robot simulation. */ - static const char* EMITTER_1_NAME; - - /** Name of the infrared emitter 2 in the robot simulation. */ - static const char* EMITTER_2_NAME; - - /** Name of the infrared emitter 3 in the robot simulation. */ - static const char* EMITTER_3_NAME; - - /** Name of the infrared emitter 4 in the robot simulation. */ - static const char* EMITTER_4_NAME; - - /** Name of the position sensor of the left motor in the robot simulation. */ - static const char* POS_SENSOR_LEFT_NAME; - - /** Name of the position sensor of the right motor in the robot simulation. */ - static const char* POS_SENSOR_RIGHT_NAME; - - /** Name of the light sensor 0 in the robot simulation. */ - static const char* LIGHT_SENSOR_0_NAME; - - /** Name of the light sensor 1 in the robot simulation. */ - static const char* LIGHT_SENSOR_1_NAME; - - /** Name of the light sensor 2 in the robot simulation. */ - static const char* LIGHT_SENSOR_2_NAME; - - /** Name of the light sensor 3 in the robot simulation. */ - static const char* LIGHT_SENSOR_3_NAME; - - /** Name of the light sensor 4 in the robot simulation. */ - static const char* LIGHT_SENSOR_4_NAME; - - /** Name of the red LED in the robot simulation. */ - static const char* LED_RED_NAME; - - /** Name of the yellow LED in the robot simulation. */ - static const char* LED_YELLOW_NAME; - - /** Name of the green LED in the robot simulation. */ - static const char* LED_GREEN_NAME; - - /** Name of the front proximity sensor in the robot simulation. */ - static const char* PROXIMITY_SENSOR_FRONT_LEFT_NAME; - - /** Name of the front right proximity sensor in the robot simulation. */ - static const char* PROXIMITY_SENSOR_FRONT_RIGHT_NAME; - - /** 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; - - /** Yellow LED driver */ - LedYellow m_ledYellow; - - /** Green LED driver */ - LedGreen m_ledGreen; - - /** Proximity sensors */ - ProximitySensors m_proximitySensors; - - /** - * Constructs the concrete board. - */ - Board() : - m_robot(), - 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(SPEAKER_NAME)), - m_display(m_robot.getDisplay(DISPLAY_NAME)), - m_encoders(m_simTime, m_robot.getPositionSensor(POS_SENSOR_LEFT_NAME), - m_robot.getPositionSensor(POS_SENSOR_RIGHT_NAME)), - m_lineSensors(m_simTime, m_robot.getEmitter(EMITTER_0_NAME), m_robot.getEmitter(EMITTER_1_NAME), - m_robot.getEmitter(EMITTER_2_NAME), m_robot.getEmitter(EMITTER_3_NAME), - m_robot.getEmitter(EMITTER_4_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_0_NAME), - m_robot.getDistanceSensor(LIGHT_SENSOR_1_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_2_NAME), - m_robot.getDistanceSensor(LIGHT_SENSOR_3_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_4_NAME)), - m_motors(m_robot.getMotor(LEFT_MOTOR_NAME), m_robot.getMotor(RIGHT_MOTOR_NAME)), - m_ledRed(m_robot.getLED(LED_RED_NAME)), - m_ledYellow(m_robot.getLED(LED_YELLOW_NAME)), - m_ledGreen(m_robot.getLED(LED_GREEN_NAME)), - m_proximitySensors(m_simTime, m_robot.getDistanceSensor(PROXIMITY_SENSOR_FRONT_LEFT_NAME), - m_robot.getDistanceSensor(PROXIMITY_SENSOR_FRONT_RIGHT_NAME)) - { - } - - /** - * 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; - } - - /** - * 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 */ +/* MIT License + * + * Copyright (c) 2023 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 + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** + * The concrete simulation robot board. + */ +class Board +{ +public: + /** + * Get board instance. + * + * @return Board instance + */ + static Board& getInstance() + { + static Board instance; /* idiom */ + + return instance; + } + + /** + * Initialize the hardware. + */ + void init(); + + /** + * Get button A driver. + * + * @return Button A driver. + */ + IButton& getButtonA() + { + return m_buttonA; + } + + /** + * Get button B driver. + * + * @return Button B driver. + */ + IButton& getButtonB() + { + return m_buttonB; + } + + /** + * Get button C driver. + * + * @return Button C driver. + */ + IButton& getButtonC() + { + return m_buttonC; + } + + /** + * Get buzzer driver. + * + * @return Buzzer driver. + */ + IBuzzer& getBuzzer() + { + return m_buzzer; + } + + /** + * Get LCD driver. + * + * @return LCD driver. + */ + IDisplay& getDisplay() + { + return m_display; + } + + /** + * Get encoders. + * + * @return Encoders driver. + */ + IEncoders& getEncoders() + { + return m_encoders; + } + + /** + * Get line sensors driver. + * + * @return Line sensor driver. + */ + ILineSensors& getLineSensors() + { + return m_lineSensors; + } + + /** + * Get motor driver. + * + * @return Motor driver. + */ + IMotors& getMotors() + { + return m_motors; + } + + /** + * Get red LED driver. + * + * @return Red LED driver. + */ + ILed& getRedLed() + { + return m_ledRed; + } + + /** + * Get yellow LED driver. + * + * @return Yellow LED driver. + */ + ILed& getYellowLed() + { + return m_ledYellow; + } + + /** + * Get green LED driver. + * + * @return Green LED driver. + */ + ILed& getGreenLed() + { + return m_ledGreen; + } + + /** + * Get proximity sensors driver. + * + * @return Proximity sensors driver + */ + IProximitySensors& getProximitySensors() + { + return m_proximitySensors; + } + +protected: +private: + /** Name of the speaker in the robot simulation. */ + static const char* SPEAKER_NAME; + + /** Name of the display in the robot simulation. */ + static const char* DISPLAY_NAME; + + /** Name of the left motor in the robot simulation. */ + static const char* LEFT_MOTOR_NAME; + + /** Name of the right motor in the robot simulation. */ + static const char* RIGHT_MOTOR_NAME; + + /** Name of the infrared emitter 0 in the robot simulation. */ + static const char* EMITTER_0_NAME; + + /** Name of the infrared emitter 1 in the robot simulation. */ + static const char* EMITTER_1_NAME; + + /** Name of the infrared emitter 2 in the robot simulation. */ + static const char* EMITTER_2_NAME; + + /** Name of the infrared emitter 3 in the robot simulation. */ + static const char* EMITTER_3_NAME; + + /** Name of the infrared emitter 4 in the robot simulation. */ + static const char* EMITTER_4_NAME; + + /** Name of the position sensor of the left motor in the robot simulation. */ + static const char* POS_SENSOR_LEFT_NAME; + + /** Name of the position sensor of the right motor in the robot simulation. */ + static const char* POS_SENSOR_RIGHT_NAME; + + /** Name of the light sensor 0 in the robot simulation. */ + static const char* LIGHT_SENSOR_0_NAME; + + /** Name of the light sensor 1 in the robot simulation. */ + static const char* LIGHT_SENSOR_1_NAME; + + /** Name of the light sensor 2 in the robot simulation. */ + static const char* LIGHT_SENSOR_2_NAME; + + /** Name of the light sensor 3 in the robot simulation. */ + static const char* LIGHT_SENSOR_3_NAME; + + /** Name of the light sensor 4 in the robot simulation. */ + static const char* LIGHT_SENSOR_4_NAME; + + /** Name of the red LED in the robot simulation. */ + static const char* LED_RED_NAME; + + /** Name of the yellow LED in the robot simulation. */ + static const char* LED_YELLOW_NAME; + + /** Name of the green LED in the robot simulation. */ + static const char* LED_GREEN_NAME; + + /** Name of the front proximity sensor in the robot simulation. */ + static const char* PROXIMITY_SENSOR_FRONT_LEFT_NAME; + + /** Name of the front right proximity sensor in the robot simulation. */ + static const char* PROXIMITY_SENSOR_FRONT_RIGHT_NAME; + + /** 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; + + /** Yellow LED driver */ + LedYellow m_ledYellow; + + /** Green LED driver */ + LedGreen m_ledGreen; + + /** Proximity sensors */ + ProximitySensors m_proximitySensors; + + /** + * Constructs the concrete board. + */ + Board() : + m_robot(), + 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(SPEAKER_NAME)), + m_display(m_robot.getDisplay(DISPLAY_NAME)), + m_encoders(m_simTime, m_robot.getPositionSensor(POS_SENSOR_LEFT_NAME), + m_robot.getPositionSensor(POS_SENSOR_RIGHT_NAME)), + m_lineSensors(m_simTime, m_robot.getEmitter(EMITTER_0_NAME), m_robot.getEmitter(EMITTER_1_NAME), + m_robot.getEmitter(EMITTER_2_NAME), m_robot.getEmitter(EMITTER_3_NAME), + m_robot.getEmitter(EMITTER_4_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_0_NAME), + m_robot.getDistanceSensor(LIGHT_SENSOR_1_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_2_NAME), + m_robot.getDistanceSensor(LIGHT_SENSOR_3_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_4_NAME)), + m_motors(m_robot.getMotor(LEFT_MOTOR_NAME), m_robot.getMotor(RIGHT_MOTOR_NAME)), + m_ledRed(m_robot.getLED(LED_RED_NAME)), + m_ledYellow(m_robot.getLED(LED_YELLOW_NAME)), + m_ledGreen(m_robot.getLED(LED_GREEN_NAME)), + m_proximitySensors(m_simTime, m_robot.getDistanceSensor(PROXIMITY_SENSOR_FRONT_LEFT_NAME), + m_robot.getDistanceSensor(PROXIMITY_SENSOR_FRONT_RIGHT_NAME)) + { + } + + /** + * 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; + } + + /** + * 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 */ /** @} */ \ No newline at end of file diff --git a/lib/HALRemoteControlSim/library.json b/lib/HALRemoteControlSim/library.json index d3159427..29af2800 100644 --- a/lib/HALRemoteControlSim/library.json +++ b/lib/HALRemoteControlSim/library.json @@ -1,21 +1,21 @@ -{ - "name": "HALRemoteControlSim", - "version": "0.1.0", - "description": "...", - "authors": [{ - "name": "Andreas Merkle", - "email": "web@blue-andi.de", - "url": "https://github.com/BlueAndi", - "maintainer": true - }], - "license": "MIT", - "dependencies": [{ - "name": "HALInterfaces" - }, { - "name": "HALSim" - }, { - "name": "Webots" - }], - "frameworks": "*", - "platforms": "*" +{ + "name": "HALRemoteControlSim", + "version": "0.1.0", + "description": "...", + "authors": [{ + "name": "Andreas Merkle", + "email": "web@blue-andi.de", + "url": "https://github.com/BlueAndi", + "maintainer": true + }], + "license": "MIT", + "dependencies": [{ + "name": "HALInterfaces" + }, { + "name": "HALSim" + }, { + "name": "Webots" + }], + "frameworks": "*", + "platforms": "*" } \ No newline at end of file diff --git a/lib/HALRemoteControlTarget/Board.cpp b/lib/HALRemoteControlTarget/Board.cpp index e900fbf9..d4f6bdce 100644 --- a/lib/HALRemoteControlTarget/Board.cpp +++ b/lib/HALRemoteControlTarget/Board.cpp @@ -1,83 +1,83 @@ -/* MIT License - * - * Copyright (c) 2019 - 2023 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 physical robot board realization. - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -void Board::init() -{ - m_encoders.init(); - m_lineSensors.init(); - m_motors.init(); - m_proximitySensors.initFrontSensor(); -} - -/****************************************************************************** - * Local Functions +/* MIT License + * + * Copyright (c) 2019 - 2023 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 physical robot board realization. + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +void Board::init() +{ + m_encoders.init(); + m_lineSensors.init(); + m_motors.init(); + m_proximitySensors.initFrontSensor(); +} + +/****************************************************************************** + * Local Functions *****************************************************************************/ \ No newline at end of file diff --git a/lib/HALRemoteControlTarget/Board.h b/lib/HALRemoteControlTarget/Board.h index 15c457af..b258cf31 100644 --- a/lib/HALRemoteControlTarget/Board.h +++ b/lib/HALRemoteControlTarget/Board.h @@ -1,284 +1,284 @@ -/* MIT License - * - * Copyright (c) 2023 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 physical robot board realization. - * @author Andreas Merkle - * - * @addtogroup HALTarget - * - * @{ - */ -#ifndef BOARD_H -#define BOARD_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** - * The concrete physical board. - */ -class Board -{ -public: - - /** - * Get board instance. - * - * @return Board instance - */ - static Board& getInstance() - { - static Board instance; /* idiom */ - - return instance; - } - - /** - * Initialize the hardware. - */ - void init(); - - /** - * Get button A driver. - * - * @return Button A driver. - */ - IButton& getButtonA() - { - return m_buttonA; - } - - /** - * Get button B driver. - * - * @return Button B driver. - */ - IButton& getButtonB() - { - return m_buttonB; - } - - /** - * Get button C driver. - * - * @return Button C driver. - */ - IButton& getButtonC() - { - return m_buttonC; - } - - /** - * Get buzzer driver. - * - * @return Buzzer driver. - */ - IBuzzer& getBuzzer() - { - return m_buzzer; - } - - /** - * Get LCD driver. - * - * @return LCD driver. - */ - IDisplay& getDisplay() - { - return m_display; - } - - /** - * Get encoders. - * - * @return Encoders driver. - */ - IEncoders& getEncoders() - { - return m_encoders; - } - - /** - * Get line sensors driver. - * - * @return Line sensor driver. - */ - ILineSensors& getLineSensors() - { - return m_lineSensors; - } - - /** - * Get motor driver. - * - * @return Motor driver. - */ - IMotors& getMotors() - { - return m_motors; - } - - /** - * Get red LED driver. - * - * @return Red LED driver. - */ - ILed& getRedLed() - { - return m_ledRed; - } - - /** - * Get yellow LED driver. - * - * @return Yellow LED driver. - */ - ILed& getYellowLed() - { - return m_ledYellow; - } - - /** - * Get green LED driver. - * - * @return Green LED driver. - */ - ILed& getGreenLed() - { - return m_ledGreen; - } - - /** - * Get proximity sensors driver. - * - * @return Proximity sensors driver - */ - IProximitySensors& getProximitySensors() - { - return m_proximitySensors; - } - -protected: - -private: - - /** 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; - - /** Yellow LED driver */ - LedYellow m_ledYellow; - - /** Green LED driver */ - LedGreen m_ledGreen; - - /** Proximity sensors */ - ProximitySensors m_proximitySensors; - - /** - * Constructs the concrete board. - */ - Board() : - m_buttonA(), - m_buttonB(), - m_buttonC(), - m_buzzer(), - m_display(), - m_encoders(), - m_lineSensors(), - m_motors(), - m_ledRed(), - m_ledYellow(), - m_ledGreen(), - m_proximitySensors() - { - } - - /** - * Destroys the concrete board. - */ - ~Board() - { - } - -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* BOARD_H */ +/* MIT License + * + * Copyright (c) 2023 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 physical robot board realization. + * @author Andreas Merkle + * + * @addtogroup HALTarget + * + * @{ + */ +#ifndef BOARD_H +#define BOARD_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** + * The concrete physical board. + */ +class Board +{ +public: + + /** + * Get board instance. + * + * @return Board instance + */ + static Board& getInstance() + { + static Board instance; /* idiom */ + + return instance; + } + + /** + * Initialize the hardware. + */ + void init(); + + /** + * Get button A driver. + * + * @return Button A driver. + */ + IButton& getButtonA() + { + return m_buttonA; + } + + /** + * Get button B driver. + * + * @return Button B driver. + */ + IButton& getButtonB() + { + return m_buttonB; + } + + /** + * Get button C driver. + * + * @return Button C driver. + */ + IButton& getButtonC() + { + return m_buttonC; + } + + /** + * Get buzzer driver. + * + * @return Buzzer driver. + */ + IBuzzer& getBuzzer() + { + return m_buzzer; + } + + /** + * Get LCD driver. + * + * @return LCD driver. + */ + IDisplay& getDisplay() + { + return m_display; + } + + /** + * Get encoders. + * + * @return Encoders driver. + */ + IEncoders& getEncoders() + { + return m_encoders; + } + + /** + * Get line sensors driver. + * + * @return Line sensor driver. + */ + ILineSensors& getLineSensors() + { + return m_lineSensors; + } + + /** + * Get motor driver. + * + * @return Motor driver. + */ + IMotors& getMotors() + { + return m_motors; + } + + /** + * Get red LED driver. + * + * @return Red LED driver. + */ + ILed& getRedLed() + { + return m_ledRed; + } + + /** + * Get yellow LED driver. + * + * @return Yellow LED driver. + */ + ILed& getYellowLed() + { + return m_ledYellow; + } + + /** + * Get green LED driver. + * + * @return Green LED driver. + */ + ILed& getGreenLed() + { + return m_ledGreen; + } + + /** + * Get proximity sensors driver. + * + * @return Proximity sensors driver + */ + IProximitySensors& getProximitySensors() + { + return m_proximitySensors; + } + +protected: + +private: + + /** 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; + + /** Yellow LED driver */ + LedYellow m_ledYellow; + + /** Green LED driver */ + LedGreen m_ledGreen; + + /** Proximity sensors */ + ProximitySensors m_proximitySensors; + + /** + * Constructs the concrete board. + */ + Board() : + m_buttonA(), + m_buttonB(), + m_buttonC(), + m_buzzer(), + m_display(), + m_encoders(), + m_lineSensors(), + m_motors(), + m_ledRed(), + m_ledYellow(), + m_ledGreen(), + m_proximitySensors() + { + } + + /** + * Destroys the concrete board. + */ + ~Board() + { + } + +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* BOARD_H */ /** @} */ \ No newline at end of file diff --git a/lib/HALRemoteControlTarget/library.json b/lib/HALRemoteControlTarget/library.json index 7ebd7947..62907680 100644 --- a/lib/HALRemoteControlTarget/library.json +++ b/lib/HALRemoteControlTarget/library.json @@ -1,23 +1,23 @@ -{ - "name": "HALRemoteControlTarget", - "version": "0.1.0", - "description": "...", - "authors": [{ - "name": "Andreas Merkle", - "email": "web@blue-andi.de", - "url": "https://github.com/BlueAndi", - "maintainer": true - }], - "license": "MIT", - "dependencies": [{ - "name": "HALInterfaces" - }, { - "name": "HALTarget" - }, { - "owner": "pololu", - "name": "Zumo32U4", - "version": "^2.0.1" - }], - "frameworks": "arduino", - "platforms": "*" +{ + "name": "HALRemoteControlTarget", + "version": "0.1.0", + "description": "...", + "authors": [{ + "name": "Andreas Merkle", + "email": "web@blue-andi.de", + "url": "https://github.com/BlueAndi", + "maintainer": true + }], + "license": "MIT", + "dependencies": [{ + "name": "HALInterfaces" + }, { + "name": "HALTarget" + }, { + "owner": "pololu", + "name": "Zumo32U4", + "version": "^2.0.1" + }], + "frameworks": "arduino", + "platforms": "*" } \ No newline at end of file diff --git a/lib/HALSensorFusionSim/Board.cpp b/lib/HALSensorFusionSim/Board.cpp index 65586f1a..65235e20 100644 --- a/lib/HALSensorFusionSim/Board.cpp +++ b/lib/HALSensorFusionSim/Board.cpp @@ -1,144 +1,144 @@ -/* MIT License - * - * Copyright (c) 2019 - 2023 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 - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/* Name of the speaker in the robot simulation. */ -const char* Board::SPEAKER_NAME = "robot_speaker"; - -/* Name of the display in the robot simulation. */ -const char* Board::DISPLAY_NAME = "robot_display"; - -/* Name of the left motor in the robot simulation. */ -const char* Board::LEFT_MOTOR_NAME = "motor_left"; - -/* Name of the right motor in the robot simulation. */ -const char* Board::RIGHT_MOTOR_NAME = "motor_right"; - -/* Name of the infrared emitter 0 in the robot simulation. */ -const char* Board::EMITTER_0_NAME = "emitter_l"; - -/* Name of the infrared emitter 1 in the robot simulation. */ -const char* Board::EMITTER_1_NAME = "emitter_lm"; - -/* Name of the infrared emitter 2 in the robot simulation. */ -const char* Board::EMITTER_2_NAME = "emitter_m"; - -/* Name of the infrared emitter 3 in the robot simulation. */ -const char* Board::EMITTER_3_NAME = "emitter_rm"; - -/* Name of the infrared emitter 4 in the robot simulation. */ -const char* Board::EMITTER_4_NAME = "emitter_r"; - -/** Name of the position sensor of the left motor in the robot simulation. */ -const char* Board::POS_SENSOR_LEFT_NAME = "position_sensor_left"; - -/** Name of the position sensor of the right motor in the robot simulation. */ -const char* Board::POS_SENSOR_RIGHT_NAME = "position_sensor_right"; - -/* Name of the light sensor 0 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_0_NAME = "lightsensor_l"; - -/* Name of the light sensor 1 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_1_NAME = "lightsensor_lm"; - -/* Name of the light sensor 2 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_2_NAME = "lightsensor_m"; - -/* Name of the light sensor 3 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_3_NAME = "lightsensor_rm"; - -/* Name of the light sensor 4 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_4_NAME = "lightsensor_r"; - -/* Name of the yellow LED in the robot simulation. */ -const char* Board::LED_YELLOW_NAME = "led_yellow"; -# -/* Name of the accelerometer in the robot simulation. */ -const char* Board::ACCELEROMETER_NAME = "accelerometer"; - -/* Name of the gyro in the robot simulation. */ -const char* Board::GYRO_NAME = "gyro"; - -/* Name of the magnetometer in the robot simulation. */ -const char* Board::MAGNETOMETER_NAME = "magnetometer"; - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -void Board::init() -{ - m_encoders.init(); - m_keyboard.init(); - m_lineSensors.init(); - m_motors.init(); - /* TODO: TD084 React if IMU initialization fails */ - (void)m_imu.init(); - m_imu.enableDefault(); - m_imu.configureForTurnSensing(); - m_imu.calibrate(); -} - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2019 - 2023 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 + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/* Name of the speaker in the robot simulation. */ +const char* Board::SPEAKER_NAME = "robot_speaker"; + +/* Name of the display in the robot simulation. */ +const char* Board::DISPLAY_NAME = "robot_display"; + +/* Name of the left motor in the robot simulation. */ +const char* Board::LEFT_MOTOR_NAME = "motor_left"; + +/* Name of the right motor in the robot simulation. */ +const char* Board::RIGHT_MOTOR_NAME = "motor_right"; + +/* Name of the infrared emitter 0 in the robot simulation. */ +const char* Board::EMITTER_0_NAME = "emitter_l"; + +/* Name of the infrared emitter 1 in the robot simulation. */ +const char* Board::EMITTER_1_NAME = "emitter_lm"; + +/* Name of the infrared emitter 2 in the robot simulation. */ +const char* Board::EMITTER_2_NAME = "emitter_m"; + +/* Name of the infrared emitter 3 in the robot simulation. */ +const char* Board::EMITTER_3_NAME = "emitter_rm"; + +/* Name of the infrared emitter 4 in the robot simulation. */ +const char* Board::EMITTER_4_NAME = "emitter_r"; + +/** Name of the position sensor of the left motor in the robot simulation. */ +const char* Board::POS_SENSOR_LEFT_NAME = "position_sensor_left"; + +/** Name of the position sensor of the right motor in the robot simulation. */ +const char* Board::POS_SENSOR_RIGHT_NAME = "position_sensor_right"; + +/* Name of the light sensor 0 in the robot simulation. */ +const char* Board::LIGHT_SENSOR_0_NAME = "lightsensor_l"; + +/* Name of the light sensor 1 in the robot simulation. */ +const char* Board::LIGHT_SENSOR_1_NAME = "lightsensor_lm"; + +/* Name of the light sensor 2 in the robot simulation. */ +const char* Board::LIGHT_SENSOR_2_NAME = "lightsensor_m"; + +/* Name of the light sensor 3 in the robot simulation. */ +const char* Board::LIGHT_SENSOR_3_NAME = "lightsensor_rm"; + +/* Name of the light sensor 4 in the robot simulation. */ +const char* Board::LIGHT_SENSOR_4_NAME = "lightsensor_r"; + +/* Name of the yellow LED in the robot simulation. */ +const char* Board::LED_YELLOW_NAME = "led_yellow"; +# +/* Name of the accelerometer in the robot simulation. */ +const char* Board::ACCELEROMETER_NAME = "accelerometer"; + +/* Name of the gyro in the robot simulation. */ +const char* Board::GYRO_NAME = "gyro"; + +/* Name of the magnetometer in the robot simulation. */ +const char* Board::MAGNETOMETER_NAME = "magnetometer"; + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +void Board::init() +{ + m_encoders.init(); + m_keyboard.init(); + m_lineSensors.init(); + m_motors.init(); + /* TODO: TD084 React if IMU initialization fails */ + (void)m_imu.init(); + m_imu.enableDefault(); + m_imu.configureForTurnSensing(); + m_imu.calibrate(); +} + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALSensorFusionSim/Board.h b/lib/HALSensorFusionSim/Board.h index 1ce9a9e0..ed5249bd 100644 --- a/lib/HALSensorFusionSim/Board.h +++ b/lib/HALSensorFusionSim/Board.h @@ -1,320 +1,320 @@ -/* MIT License - * - * Copyright (c) 2023 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 - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** - * The concrete simulation robot board. - */ -class Board -{ -public: - /** - * Get board instance. - * - * @return Board instance - */ - static Board& getInstance() - { - static Board instance; /* idiom */ - - return instance; - } - - /** - * Initialize the hardware. - */ - void init(); - - /** - * Get button A driver. - * - * @return Button A driver. - */ - IButton& getButtonA() - { - return m_buttonA; - } - - /** - * Get buzzer driver. - * - * @return Buzzer driver. - */ - IBuzzer& getBuzzer() - { - return m_buzzer; - } - - /** - * Get encoders. - * - * @return Encoders driver. - */ - IEncoders& getEncoders() - { - return m_encoders; - } - - /** - * Get line sensors driver. - * - * @return Line sensor driver. - */ - ILineSensors& getLineSensors() - { - return m_lineSensors; - } - - /** - * Get motor driver. - * - * @return Motor driver. - */ - IMotors& getMotors() - { - return m_motors; - } - - /** - * Get yellow LED driver. - * - * @return Yellow LED driver. - */ - ILed& getYellowLed() - { - return m_ledYellow; - } - - /** - * Get IMU (Inertial Measurement Unit) driver. - * - * @return IMU driver - */ - IIMU& getIMU() - { - return m_imu; - } - -protected: -private: - /** Name of the speaker in the robot simulation. */ - static const char* SPEAKER_NAME; - - /** Name of the display in the robot simulation. */ - static const char* DISPLAY_NAME; - - /** Name of the left motor in the robot simulation. */ - static const char* LEFT_MOTOR_NAME; - - /** Name of the right motor in the robot simulation. */ - static const char* RIGHT_MOTOR_NAME; - - /** Name of the infrared emitter 0 in the robot simulation. */ - static const char* EMITTER_0_NAME; - - /** Name of the infrared emitter 1 in the robot simulation. */ - static const char* EMITTER_1_NAME; - - /** Name of the infrared emitter 2 in the robot simulation. */ - static const char* EMITTER_2_NAME; - - /** Name of the infrared emitter 3 in the robot simulation. */ - static const char* EMITTER_3_NAME; - - /** Name of the infrared emitter 4 in the robot simulation. */ - static const char* EMITTER_4_NAME; - - /** Name of the position sensor of the left motor in the robot simulation. */ - static const char* POS_SENSOR_LEFT_NAME; - - /** Name of the position sensor of the right motor in the robot simulation. */ - static const char* POS_SENSOR_RIGHT_NAME; - - /** Name of the light sensor 0 in the robot simulation. */ - static const char* LIGHT_SENSOR_0_NAME; - - /** Name of the light sensor 1 in the robot simulation. */ - static const char* LIGHT_SENSOR_1_NAME; - - /** Name of the light sensor 2 in the robot simulation. */ - static const char* LIGHT_SENSOR_2_NAME; - - /** Name of the light sensor 3 in the robot simulation. */ - static const char* LIGHT_SENSOR_3_NAME; - - /** Name of the light sensor 4 in the robot simulation. */ - static const char* LIGHT_SENSOR_4_NAME; - - /** Name of the yellow LED in the robot simulation. */ - static const char* LED_YELLOW_NAME; - - /** Name of the accelerometer in the robot simulation. */ - static const char* ACCELEROMETER_NAME; - - /** Name of the gyro in the robot simulation. */ - static const char* GYRO_NAME; - - /** Name of the Magnetometer in the robot simulation. */ - static const char* MAGNETOMETER_NAME; - - /** 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; - - /** Buzzer driver */ - Buzzer m_buzzer; - - /** Encoders driver */ - Encoders m_encoders; - - /** Line sensors driver */ - LineSensors m_lineSensors; - - /** Motors driver */ - Motors m_motors; - - /** Red LED driver */ - LedYellow m_ledYellow; - - /** IMU driver */ - IMU m_imu; - - /** - * Constructs the concrete board. - */ - Board() : - m_robot(), - m_simTime(m_robot), - m_keyboard(m_simTime, m_robot.getKeyboard()), - m_buttonA(m_keyboard), - m_buzzer(m_robot.getSpeaker(SPEAKER_NAME)), - m_encoders(m_simTime, m_robot.getPositionSensor(POS_SENSOR_LEFT_NAME), - m_robot.getPositionSensor(POS_SENSOR_RIGHT_NAME)), - m_lineSensors(m_simTime, m_robot.getEmitter(EMITTER_0_NAME), m_robot.getEmitter(EMITTER_1_NAME), - m_robot.getEmitter(EMITTER_2_NAME), m_robot.getEmitter(EMITTER_3_NAME), - m_robot.getEmitter(EMITTER_4_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_0_NAME), - m_robot.getDistanceSensor(LIGHT_SENSOR_1_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_2_NAME), - m_robot.getDistanceSensor(LIGHT_SENSOR_3_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_4_NAME)), - m_motors(m_robot.getMotor(LEFT_MOTOR_NAME), m_robot.getMotor(RIGHT_MOTOR_NAME)), - m_ledYellow(m_robot.getLED(LED_YELLOW_NAME)), - m_imu(m_simTime, m_robot.getAccelerometer(ACCELEROMETER_NAME), m_robot.getGyro(GYRO_NAME), - m_robot.getCompass(MAGNETOMETER_NAME)) - { - } - - /** - * 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; - } - - /** - * 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 */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 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 + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** + * The concrete simulation robot board. + */ +class Board +{ +public: + /** + * Get board instance. + * + * @return Board instance + */ + static Board& getInstance() + { + static Board instance; /* idiom */ + + return instance; + } + + /** + * Initialize the hardware. + */ + void init(); + + /** + * Get button A driver. + * + * @return Button A driver. + */ + IButton& getButtonA() + { + return m_buttonA; + } + + /** + * Get buzzer driver. + * + * @return Buzzer driver. + */ + IBuzzer& getBuzzer() + { + return m_buzzer; + } + + /** + * Get encoders. + * + * @return Encoders driver. + */ + IEncoders& getEncoders() + { + return m_encoders; + } + + /** + * Get line sensors driver. + * + * @return Line sensor driver. + */ + ILineSensors& getLineSensors() + { + return m_lineSensors; + } + + /** + * Get motor driver. + * + * @return Motor driver. + */ + IMotors& getMotors() + { + return m_motors; + } + + /** + * Get yellow LED driver. + * + * @return Yellow LED driver. + */ + ILed& getYellowLed() + { + return m_ledYellow; + } + + /** + * Get IMU (Inertial Measurement Unit) driver. + * + * @return IMU driver + */ + IIMU& getIMU() + { + return m_imu; + } + +protected: +private: + /** Name of the speaker in the robot simulation. */ + static const char* SPEAKER_NAME; + + /** Name of the display in the robot simulation. */ + static const char* DISPLAY_NAME; + + /** Name of the left motor in the robot simulation. */ + static const char* LEFT_MOTOR_NAME; + + /** Name of the right motor in the robot simulation. */ + static const char* RIGHT_MOTOR_NAME; + + /** Name of the infrared emitter 0 in the robot simulation. */ + static const char* EMITTER_0_NAME; + + /** Name of the infrared emitter 1 in the robot simulation. */ + static const char* EMITTER_1_NAME; + + /** Name of the infrared emitter 2 in the robot simulation. */ + static const char* EMITTER_2_NAME; + + /** Name of the infrared emitter 3 in the robot simulation. */ + static const char* EMITTER_3_NAME; + + /** Name of the infrared emitter 4 in the robot simulation. */ + static const char* EMITTER_4_NAME; + + /** Name of the position sensor of the left motor in the robot simulation. */ + static const char* POS_SENSOR_LEFT_NAME; + + /** Name of the position sensor of the right motor in the robot simulation. */ + static const char* POS_SENSOR_RIGHT_NAME; + + /** Name of the light sensor 0 in the robot simulation. */ + static const char* LIGHT_SENSOR_0_NAME; + + /** Name of the light sensor 1 in the robot simulation. */ + static const char* LIGHT_SENSOR_1_NAME; + + /** Name of the light sensor 2 in the robot simulation. */ + static const char* LIGHT_SENSOR_2_NAME; + + /** Name of the light sensor 3 in the robot simulation. */ + static const char* LIGHT_SENSOR_3_NAME; + + /** Name of the light sensor 4 in the robot simulation. */ + static const char* LIGHT_SENSOR_4_NAME; + + /** Name of the yellow LED in the robot simulation. */ + static const char* LED_YELLOW_NAME; + + /** Name of the accelerometer in the robot simulation. */ + static const char* ACCELEROMETER_NAME; + + /** Name of the gyro in the robot simulation. */ + static const char* GYRO_NAME; + + /** Name of the Magnetometer in the robot simulation. */ + static const char* MAGNETOMETER_NAME; + + /** 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; + + /** Buzzer driver */ + Buzzer m_buzzer; + + /** Encoders driver */ + Encoders m_encoders; + + /** Line sensors driver */ + LineSensors m_lineSensors; + + /** Motors driver */ + Motors m_motors; + + /** Red LED driver */ + LedYellow m_ledYellow; + + /** IMU driver */ + IMU m_imu; + + /** + * Constructs the concrete board. + */ + Board() : + m_robot(), + m_simTime(m_robot), + m_keyboard(m_simTime, m_robot.getKeyboard()), + m_buttonA(m_keyboard), + m_buzzer(m_robot.getSpeaker(SPEAKER_NAME)), + m_encoders(m_simTime, m_robot.getPositionSensor(POS_SENSOR_LEFT_NAME), + m_robot.getPositionSensor(POS_SENSOR_RIGHT_NAME)), + m_lineSensors(m_simTime, m_robot.getEmitter(EMITTER_0_NAME), m_robot.getEmitter(EMITTER_1_NAME), + m_robot.getEmitter(EMITTER_2_NAME), m_robot.getEmitter(EMITTER_3_NAME), + m_robot.getEmitter(EMITTER_4_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_0_NAME), + m_robot.getDistanceSensor(LIGHT_SENSOR_1_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_2_NAME), + m_robot.getDistanceSensor(LIGHT_SENSOR_3_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_4_NAME)), + m_motors(m_robot.getMotor(LEFT_MOTOR_NAME), m_robot.getMotor(RIGHT_MOTOR_NAME)), + m_ledYellow(m_robot.getLED(LED_YELLOW_NAME)), + m_imu(m_simTime, m_robot.getAccelerometer(ACCELEROMETER_NAME), m_robot.getGyro(GYRO_NAME), + m_robot.getCompass(MAGNETOMETER_NAME)) + { + } + + /** + * 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; + } + + /** + * 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/HALSensorFusionSim/library.json b/lib/HALSensorFusionSim/library.json index 789a7f54..5444939d 100644 --- a/lib/HALSensorFusionSim/library.json +++ b/lib/HALSensorFusionSim/library.json @@ -1,21 +1,21 @@ -{ - "name": "HALSensorFusionSim", - "version": "0.1.0", - "description": "...", - "authors": [{ - "name": "Juliane Kerpe", - "email": "juliane-98@web.de", - "url": "https://github.com/jkerpe", - "maintainer": true - }], - "license": "MIT", - "dependencies": [{ - "name": "HALInterfaces" - }, { - "name": "HALSim" - }, { - "name": "Webots" - }], - "frameworks": "*", - "platforms": "*" +{ + "name": "HALSensorFusionSim", + "version": "0.1.0", + "description": "...", + "authors": [{ + "name": "Juliane Kerpe", + "email": "juliane-98@web.de", + "url": "https://github.com/jkerpe", + "maintainer": true + }], + "license": "MIT", + "dependencies": [{ + "name": "HALInterfaces" + }, { + "name": "HALSim" + }, { + "name": "Webots" + }], + "frameworks": "*", + "platforms": "*" } \ No newline at end of file diff --git a/lib/HALSensorFusionTarget/Board.cpp b/lib/HALSensorFusionTarget/Board.cpp index 19ce9c3e..8a4e1b07 100644 --- a/lib/HALSensorFusionTarget/Board.cpp +++ b/lib/HALSensorFusionTarget/Board.cpp @@ -1,87 +1,87 @@ -/* MIT License - * - * Copyright (c) 2019 - 2023 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 physical robot board realization. - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -void Board::init() -{ - m_encoders.init(); - m_lineSensors.init(); - m_motors.init(); - /* TODO: TD084 React if IMU initialization fails */ - (void)m_imu.init(); - m_imu.enableDefault(); - m_imu.configureForTurnSensing(); - m_imu.calibrate(); -} - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2019 - 2023 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 physical robot board realization. + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +void Board::init() +{ + m_encoders.init(); + m_lineSensors.init(); + m_motors.init(); + /* TODO: TD084 React if IMU initialization fails */ + (void)m_imu.init(); + m_imu.enableDefault(); + m_imu.configureForTurnSensing(); + m_imu.calibrate(); +} + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALSensorFusionTarget/Board.h b/lib/HALSensorFusionTarget/Board.h index 72044227..ce8b17d0 100644 --- a/lib/HALSensorFusionTarget/Board.h +++ b/lib/HALSensorFusionTarget/Board.h @@ -1,206 +1,206 @@ -/* MIT License - * - * Copyright (c) 2023 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 physical robot board realization. - * @author Andreas Merkle - * - * @addtogroup HALTarget - * - * @{ - */ -#ifndef BOARD_H -#define BOARD_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** - * The concrete physical board. - */ -class Board -{ -public: - /** - * Get board instance. - * - * @return Board instance - */ - static Board& getInstance() - { - static Board instance; /* idiom */ - - return instance; - } - - /** - * Initialize the hardware. - */ - void init(); - - /** - * Get button A driver. - * - * @return Button A driver. - */ - IButton& getButtonA() - { - return m_buttonA; - } - - /** - * Get buzzer driver. - * - * @return Buzzer driver. - */ - IBuzzer& getBuzzer() - { - return m_buzzer; - } - - /** - * Get encoders. - * - * @return Encoders driver. - */ - IEncoders& getEncoders() - { - return m_encoders; - } - - /** - * Get line sensors driver. - * - * @return Line sensor driver. - */ - ILineSensors& getLineSensors() - { - return m_lineSensors; - } - - /** - * Get motor driver. - * - * @return Motor driver. - */ - IMotors& getMotors() - { - return m_motors; - } - - /** - * Get yellow LED driver. - * - * @return Yellow LED driver. - */ - ILed& getYellowLed() - { - return m_ledYellow; - } - - /** - * Get IMU (Inertial Measurement Unit) driver. - * - * @return IMU driver - */ - IIMU& getIMU() - { - return m_imu; - } - -protected: -private: - /** Button A driver */ - ButtonA m_buttonA; - - /** Buzzer driver */ - Buzzer m_buzzer; - - /** Encoders driver */ - Encoders m_encoders; - - /** Line sensors driver */ - LineSensors m_lineSensors; - - /** Motors driver */ - Motors m_motors; - - /** Yellow LED driver */ - LedYellow m_ledYellow; - - /** IMU Driver */ - IMU m_imu; - - /** - * Constructs the concrete board. - */ - Board() : - m_buttonA(), - m_buzzer(), - m_encoders(), - m_lineSensors(), - m_motors(), - m_ledYellow(), - m_imu() - { - } - - /** - * Destroys the concrete board. - */ - ~Board() - { - } -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* BOARD_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 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 physical robot board realization. + * @author Andreas Merkle + * + * @addtogroup HALTarget + * + * @{ + */ +#ifndef BOARD_H +#define BOARD_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** + * The concrete physical board. + */ +class Board +{ +public: + /** + * Get board instance. + * + * @return Board instance + */ + static Board& getInstance() + { + static Board instance; /* idiom */ + + return instance; + } + + /** + * Initialize the hardware. + */ + void init(); + + /** + * Get button A driver. + * + * @return Button A driver. + */ + IButton& getButtonA() + { + return m_buttonA; + } + + /** + * Get buzzer driver. + * + * @return Buzzer driver. + */ + IBuzzer& getBuzzer() + { + return m_buzzer; + } + + /** + * Get encoders. + * + * @return Encoders driver. + */ + IEncoders& getEncoders() + { + return m_encoders; + } + + /** + * Get line sensors driver. + * + * @return Line sensor driver. + */ + ILineSensors& getLineSensors() + { + return m_lineSensors; + } + + /** + * Get motor driver. + * + * @return Motor driver. + */ + IMotors& getMotors() + { + return m_motors; + } + + /** + * Get yellow LED driver. + * + * @return Yellow LED driver. + */ + ILed& getYellowLed() + { + return m_ledYellow; + } + + /** + * Get IMU (Inertial Measurement Unit) driver. + * + * @return IMU driver + */ + IIMU& getIMU() + { + return m_imu; + } + +protected: +private: + /** Button A driver */ + ButtonA m_buttonA; + + /** Buzzer driver */ + Buzzer m_buzzer; + + /** Encoders driver */ + Encoders m_encoders; + + /** Line sensors driver */ + LineSensors m_lineSensors; + + /** Motors driver */ + Motors m_motors; + + /** Yellow LED driver */ + LedYellow m_ledYellow; + + /** IMU Driver */ + IMU m_imu; + + /** + * Constructs the concrete board. + */ + Board() : + m_buttonA(), + m_buzzer(), + m_encoders(), + m_lineSensors(), + m_motors(), + m_ledYellow(), + m_imu() + { + } + + /** + * Destroys the concrete board. + */ + ~Board() + { + } +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* BOARD_H */ +/** @} */ diff --git a/lib/HALSensorFusionTarget/library.json b/lib/HALSensorFusionTarget/library.json index 5c4faca5..569f7ad4 100644 --- a/lib/HALSensorFusionTarget/library.json +++ b/lib/HALSensorFusionTarget/library.json @@ -1,23 +1,23 @@ -{ - "name": "HALSensorFusionTarget", - "version": "0.1.0", - "description": "...", - "authors": [{ - "name": "Juliane Kerpe", - "email": "juliane-98@web.de", - "url": "https://github.com/jkerpe", - "maintainer": true - }], - "license": "MIT", - "dependencies": [{ - "name": "HALInterfaces" - }, { - "name": "HALTarget" - }, { - "owner": "pololu", - "name": "Zumo32U4", - "version": "^2.0.1" - }], - "frameworks": "arduino", - "platforms": "*" +{ + "name": "HALSensorFusionTarget", + "version": "0.1.0", + "description": "...", + "authors": [{ + "name": "Juliane Kerpe", + "email": "juliane-98@web.de", + "url": "https://github.com/jkerpe", + "maintainer": true + }], + "license": "MIT", + "dependencies": [{ + "name": "HALInterfaces" + }, { + "name": "HALTarget" + }, { + "owner": "pololu", + "name": "Zumo32U4", + "version": "^2.0.1" + }], + "frameworks": "arduino", + "platforms": "*" } \ No newline at end of file diff --git a/lib/HALSim/IMU.cpp b/lib/HALSim/IMU.cpp index 2aa66bbf..a4d095c5 100644 --- a/lib/HALSim/IMU.cpp +++ b/lib/HALSim/IMU.cpp @@ -1,185 +1,185 @@ -/* MIT License - * - * Copyright (c) 2023 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 IMU implementation for the Simulation - * @author Juliane Kerpe - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "IMU.h" -#include -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -bool IMU::init() -{ - bool isInitializationSuccessful = false; - if ((nullptr != m_accelerometer) && (nullptr != m_gyro) && (nullptr != m_magnetometer)) - { - m_accelerometer->enable(m_simTime.getTimeStep()); - m_gyro->enable(m_simTime.getTimeStep()); - m_magnetometer->enable(m_simTime.getTimeStep()); - isInitializationSuccessful = true; - } - return isInitializationSuccessful; -} - -void IMU::readAccelerometer() -{ - if (nullptr != m_accelerometer) - { - const double* accelerationValues = m_accelerometer->getValues(); - if (nullptr != accelerationValues) - { - m_accelerationValues.valueX = convertFromDoubleToInt16(accelerationValues[0]); /* Index 0 is the x-axis. */ - m_accelerationValues.valueY = convertFromDoubleToInt16(accelerationValues[1]); /* Index 1 is the y-axis. */ - m_accelerationValues.valueZ = convertFromDoubleToInt16(accelerationValues[2]); /* Index 2 is the z-axis. */ - } - } -} - -void IMU::readGyro() -{ - if (nullptr != m_gyro) - { - const double* gyroValues = m_gyro->getValues(); - - if (nullptr != gyroValues) - { - m_gyroValues.valueX = convertFromDoubleToInt16(gyroValues[0]); /* Index 0 is the x-axis. */ - m_gyroValues.valueY = convertFromDoubleToInt16(gyroValues[1]); /* Index 1 is the y-axis. */ - m_gyroValues.valueZ = convertFromDoubleToInt16(gyroValues[2]); /* Index 2 is the z-axis. */ - } - } -} - -void IMU::readMagnetometer() -{ - if (nullptr != m_magnetometer) - { - const double* magnetometerValues = m_magnetometer->getValues(); - - if (nullptr != magnetometerValues) - { - m_magnetometerValues.valueX = convertFromDoubleToInt16(magnetometerValues[0]); /* Index 0 is the x-axis. */ - m_magnetometerValues.valueY = convertFromDoubleToInt16(magnetometerValues[1]); /* Index 1 is the y-axis. */ - m_magnetometerValues.valueZ = convertFromDoubleToInt16(magnetometerValues[2]); /* Index 2 is the z-axis. */ - } - } -} - -const void IMU::getAccelerationValues(IMUData* accelerationValues) -{ - if (nullptr != accelerationValues) - { - accelerationValues->valueX = m_accelerationValues.valueX; - accelerationValues->valueY = m_accelerationValues.valueY; - accelerationValues->valueZ = m_accelerationValues.valueZ; - } -} - -const void IMU::getTurnRates(IMUData* turnRates) -{ - if (nullptr != turnRates) - { - turnRates->valueX = m_gyroValues.valueX; - turnRates->valueY = m_gyroValues.valueY; - turnRates->valueZ = m_gyroValues.valueZ; - } -} - -const void IMU::getMagnetometerValues(IMUData* magnetometerValues) -{ - if (nullptr != magnetometerValues) - { - magnetometerValues->valueX = m_magnetometerValues.valueX; - magnetometerValues->valueY = m_magnetometerValues.valueY; - magnetometerValues->valueZ = m_magnetometerValues.valueZ; - } -} - -void IMU::calibrate() -{ - /* TODO: implement TD067 */ -} - -int16_t IMU::convertFromDoubleToInt16(double originalValue) -{ - int16_t convertedValue; - - if (originalValue <= INT16_MIN) - { - convertedValue = INT16_MIN; - } - else if (originalValue >= INT16_MAX) - { - convertedValue = INT16_MAX; - } - else - { - convertedValue = static_cast(originalValue); - } - return convertedValue; -} -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 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 IMU implementation for the Simulation + * @author Juliane Kerpe + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "IMU.h" +#include +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +bool IMU::init() +{ + bool isInitializationSuccessful = false; + if ((nullptr != m_accelerometer) && (nullptr != m_gyro) && (nullptr != m_magnetometer)) + { + m_accelerometer->enable(m_simTime.getTimeStep()); + m_gyro->enable(m_simTime.getTimeStep()); + m_magnetometer->enable(m_simTime.getTimeStep()); + isInitializationSuccessful = true; + } + return isInitializationSuccessful; +} + +void IMU::readAccelerometer() +{ + if (nullptr != m_accelerometer) + { + const double* accelerationValues = m_accelerometer->getValues(); + if (nullptr != accelerationValues) + { + m_accelerationValues.valueX = convertFromDoubleToInt16(accelerationValues[0]); /* Index 0 is the x-axis. */ + m_accelerationValues.valueY = convertFromDoubleToInt16(accelerationValues[1]); /* Index 1 is the y-axis. */ + m_accelerationValues.valueZ = convertFromDoubleToInt16(accelerationValues[2]); /* Index 2 is the z-axis. */ + } + } +} + +void IMU::readGyro() +{ + if (nullptr != m_gyro) + { + const double* gyroValues = m_gyro->getValues(); + + if (nullptr != gyroValues) + { + m_gyroValues.valueX = convertFromDoubleToInt16(gyroValues[0]); /* Index 0 is the x-axis. */ + m_gyroValues.valueY = convertFromDoubleToInt16(gyroValues[1]); /* Index 1 is the y-axis. */ + m_gyroValues.valueZ = convertFromDoubleToInt16(gyroValues[2]); /* Index 2 is the z-axis. */ + } + } +} + +void IMU::readMagnetometer() +{ + if (nullptr != m_magnetometer) + { + const double* magnetometerValues = m_magnetometer->getValues(); + + if (nullptr != magnetometerValues) + { + m_magnetometerValues.valueX = convertFromDoubleToInt16(magnetometerValues[0]); /* Index 0 is the x-axis. */ + m_magnetometerValues.valueY = convertFromDoubleToInt16(magnetometerValues[1]); /* Index 1 is the y-axis. */ + m_magnetometerValues.valueZ = convertFromDoubleToInt16(magnetometerValues[2]); /* Index 2 is the z-axis. */ + } + } +} + +const void IMU::getAccelerationValues(IMUData* accelerationValues) +{ + if (nullptr != accelerationValues) + { + accelerationValues->valueX = m_accelerationValues.valueX; + accelerationValues->valueY = m_accelerationValues.valueY; + accelerationValues->valueZ = m_accelerationValues.valueZ; + } +} + +const void IMU::getTurnRates(IMUData* turnRates) +{ + if (nullptr != turnRates) + { + turnRates->valueX = m_gyroValues.valueX; + turnRates->valueY = m_gyroValues.valueY; + turnRates->valueZ = m_gyroValues.valueZ; + } +} + +const void IMU::getMagnetometerValues(IMUData* magnetometerValues) +{ + if (nullptr != magnetometerValues) + { + magnetometerValues->valueX = m_magnetometerValues.valueX; + magnetometerValues->valueY = m_magnetometerValues.valueY; + magnetometerValues->valueZ = m_magnetometerValues.valueZ; + } +} + +void IMU::calibrate() +{ + /* TODO: implement TD067 */ +} + +int16_t IMU::convertFromDoubleToInt16(double originalValue) +{ + int16_t convertedValue; + + if (originalValue <= INT16_MIN) + { + convertedValue = INT16_MIN; + } + else if (originalValue >= INT16_MAX) + { + convertedValue = INT16_MAX; + } + else + { + convertedValue = static_cast(originalValue); + } + return convertedValue; +} +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALSim/IMU.h b/lib/HALSim/IMU.h index f43638bd..8c7d1dee 100644 --- a/lib/HALSim/IMU.h +++ b/lib/HALSim/IMU.h @@ -1,217 +1,217 @@ -/* MIT License - * - * Copyright (c) 2023 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 IMU (Inertial Measurement Unit) implementation of the simulation - * @author Juliane Kerpe - * - * @addtogroup HALSim - * - * @{ - */ -#ifndef IMU_H -#define IMU_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include "IIMU.h" - -#include "SimTime.h" -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The IMU adapter. - * IMU stands for Inertial Measurement Unit. - */ -class IMU : public IIMU -{ -public: - /** - * Constructs the IMU adapter. - * - * @param[in] simTime Simulation time - * @param[in] accelerometer The accelerometer - * @param[in] gyro The gyro - * @param[in] compass The compass - */ - IMU(const SimTime& simTime, webots::Accelerometer* accelerometer, webots::Gyro* gyro, webots::Compass* compass) : - IIMU(), - m_accelerationValues{0, 0, 0}, - m_gyroValues{0, 0, 0}, - m_magnetometerValues{0, 0, 0}, - m_simTime(simTime), - m_accelerometer(accelerometer), - m_gyro(gyro), - m_magnetometer(compass) - { - } - - /** - * Destroys the interface. - */ - ~IMU() - { - } - - /** - * Initializes the inertial sensors and detects their type. - * - * @return True if the sensor type was detected succesfully; false otherwise. - */ - bool init(); - - /** - * Enables all of the inertial sensors with a default configuration. - */ - void enableDefault() - { - } - - /** - * Configures the sensors with settings optimized for turn sensing. - */ - void configureForTurnSensing() - { - } - - /** - * Takes a reading from the accelerometer and makes the measurements available in a. - */ - void readAccelerometer(); - - /** - * Takes a reading from the gyro and makes the measurements available in g. - */ - void readGyro(); - - /** - * Takes a reading from the magnetometer and makes the measurements available in m. - */ - void readMagnetometer(); - - /** - * Indicates whether the accelerometer has new measurement data ready. - * - * @return True if there is new accelerometer data available; false otherwise. - */ - bool accelerometerDataReady() - { - return true; - } - - /** - * Indicates whether the gyro has new measurement data ready. - * - * @return True if there is new gyro data available; false otherwise. - */ - bool gyroDataReady() - { - return true; - } - - /** - * Indicates whether the magnetometer has new measurement data ready. - * - * @return True if there is new magnetometer data available; false otherwise. - */ - bool magnetometerDataReady() - { - return true; - } - - /** - * Get last raw Accelerometer values as an IMUData struct containing values in x, y and z in digits. - * - * @param[in] accelerationValues Pointer to IMUData struct where the raw acceleration values in digits in - * x, y and z direction will be written into. The values can be converted into physical values in mm/s^2 via the - * multiplication with a sensitivity factor in mm/s^2/digit. - */ - void const getAccelerationValues(IMUData* accelerationValues); - - /** - * Get last raw Gyro values as an IMUData struct containing values in x, y and z in digits. - * - * @param[in] turnRates Pointer to IMUData struct where the raw turn Rates in digits in x, y and z - * direction will be written into. The values can be converted into physical values in mrad/s via the multiplication - * with a sensitivity factor in mrad/s/digit. - */ - void const getTurnRates(IMUData* turnRates); - - /** - * Get last raw Magnetometer values as an IMUData struct containing values in x, y and z in digits. - * - * @param[in] magnetometerValues Pointer to IMUData struct where the raw magnetometer values in digits in - * x, y and z direction will be written into. The values can be converted into physical values in mgauss via the - * multiplication with a sensitivity factor in mgauss/digit. - */ - void const getMagnetometerValues(IMUData* magnetometerValues); - - /** - * Calibrate the IMU. - */ - void calibrate(); - -private: - /** - * Convert a parameter from double to int16_t. Clip the value to the minimum / maximum range of the int16_t data - * type if the input value exceeds the limits of int16_t. - * - * @param[in] originalValue The Original double value which shall be converted. - * @return The converted value to int16_t, which might be clipped. - */ - int16_t convertFromDoubleToInt16(double originalValue); - - IMUData m_accelerationValues; /**< Struct to store the current raw accelerometer data. */ - IMUData m_gyroValues; /**< Struct to store the current gyro data. */ - IMUData m_magnetometerValues; /**< Struct to store the current magnotometer data. */ - - const SimTime& m_simTime; /**< Simulation time. */ - webots::Accelerometer* m_accelerometer; /**< The accelerometer of Webots. */ - webots::Gyro* m_gyro; /**< The gyro of Webots. */ - webots::Compass* m_magnetometer; /**< The magnetometer of Webots. */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* IMU_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 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 IMU (Inertial Measurement Unit) implementation of the simulation + * @author Juliane Kerpe + * + * @addtogroup HALSim + * + * @{ + */ +#ifndef IMU_H +#define IMU_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include "IIMU.h" + +#include "SimTime.h" +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The IMU adapter. + * IMU stands for Inertial Measurement Unit. + */ +class IMU : public IIMU +{ +public: + /** + * Constructs the IMU adapter. + * + * @param[in] simTime Simulation time + * @param[in] accelerometer The accelerometer + * @param[in] gyro The gyro + * @param[in] compass The compass + */ + IMU(const SimTime& simTime, webots::Accelerometer* accelerometer, webots::Gyro* gyro, webots::Compass* compass) : + IIMU(), + m_accelerationValues{0, 0, 0}, + m_gyroValues{0, 0, 0}, + m_magnetometerValues{0, 0, 0}, + m_simTime(simTime), + m_accelerometer(accelerometer), + m_gyro(gyro), + m_magnetometer(compass) + { + } + + /** + * Destroys the interface. + */ + ~IMU() + { + } + + /** + * Initializes the inertial sensors and detects their type. + * + * @return True if the sensor type was detected succesfully; false otherwise. + */ + bool init(); + + /** + * Enables all of the inertial sensors with a default configuration. + */ + void enableDefault() + { + } + + /** + * Configures the sensors with settings optimized for turn sensing. + */ + void configureForTurnSensing() + { + } + + /** + * Takes a reading from the accelerometer and makes the measurements available in a. + */ + void readAccelerometer(); + + /** + * Takes a reading from the gyro and makes the measurements available in g. + */ + void readGyro(); + + /** + * Takes a reading from the magnetometer and makes the measurements available in m. + */ + void readMagnetometer(); + + /** + * Indicates whether the accelerometer has new measurement data ready. + * + * @return True if there is new accelerometer data available; false otherwise. + */ + bool accelerometerDataReady() + { + return true; + } + + /** + * Indicates whether the gyro has new measurement data ready. + * + * @return True if there is new gyro data available; false otherwise. + */ + bool gyroDataReady() + { + return true; + } + + /** + * Indicates whether the magnetometer has new measurement data ready. + * + * @return True if there is new magnetometer data available; false otherwise. + */ + bool magnetometerDataReady() + { + return true; + } + + /** + * Get last raw Accelerometer values as an IMUData struct containing values in x, y and z in digits. + * + * @param[in] accelerationValues Pointer to IMUData struct where the raw acceleration values in digits in + * x, y and z direction will be written into. The values can be converted into physical values in mm/s^2 via the + * multiplication with a sensitivity factor in mm/s^2/digit. + */ + void const getAccelerationValues(IMUData* accelerationValues); + + /** + * Get last raw Gyro values as an IMUData struct containing values in x, y and z in digits. + * + * @param[in] turnRates Pointer to IMUData struct where the raw turn Rates in digits in x, y and z + * direction will be written into. The values can be converted into physical values in mrad/s via the multiplication + * with a sensitivity factor in mrad/s/digit. + */ + void const getTurnRates(IMUData* turnRates); + + /** + * Get last raw Magnetometer values as an IMUData struct containing values in x, y and z in digits. + * + * @param[in] magnetometerValues Pointer to IMUData struct where the raw magnetometer values in digits in + * x, y and z direction will be written into. The values can be converted into physical values in mgauss via the + * multiplication with a sensitivity factor in mgauss/digit. + */ + void const getMagnetometerValues(IMUData* magnetometerValues); + + /** + * Calibrate the IMU. + */ + void calibrate(); + +private: + /** + * Convert a parameter from double to int16_t. Clip the value to the minimum / maximum range of the int16_t data + * type if the input value exceeds the limits of int16_t. + * + * @param[in] originalValue The Original double value which shall be converted. + * @return The converted value to int16_t, which might be clipped. + */ + int16_t convertFromDoubleToInt16(double originalValue); + + IMUData m_accelerationValues; /**< Struct to store the current raw accelerometer data. */ + IMUData m_gyroValues; /**< Struct to store the current gyro data. */ + IMUData m_magnetometerValues; /**< Struct to store the current magnotometer data. */ + + const SimTime& m_simTime; /**< Simulation time. */ + webots::Accelerometer* m_accelerometer; /**< The accelerometer of Webots. */ + webots::Gyro* m_gyro; /**< The gyro of Webots. */ + webots::Compass* m_magnetometer; /**< The magnetometer of Webots. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* IMU_H */ +/** @} */ diff --git a/lib/HALTarget/IMU.cpp b/lib/HALTarget/IMU.cpp index bbcad686..a67b2aa5 100644 --- a/lib/HALTarget/IMU.cpp +++ b/lib/HALTarget/IMU.cpp @@ -1,227 +1,227 @@ -/* MIT License - * - * Copyright (c) 2023 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 IMU implementation - * @author Juliane Kerpe - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "IMU.h" -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -bool IMU::init() -{ - bool isInitSuccessful = m_imuDrv.init(); - if (true == isInitSuccessful) - { - m_imuDrv.enableDefault(); - /* TODO: TD074 Make sure that a Full Scale of 245 dps of the gyros are enough */ - /* TODO: TD075 Make sure that it is valid to make the Full Scale setting in IMU::init() */ - switch (m_imuDrv.getType()) - { - case Zumo32U4IMUType::LSM303D_L3GD20H: - /* 0x00 means +/- 245 dps according to L3GD20H data sheet */ - m_imuDrv.writeReg(L3GD20H_ADDR, L3GD20H_REG_CTRL4, 0x00); - break; - case Zumo32U4IMUType::LSM6DS33_LIS3MDL: - /* 0x00 means +/- 245 dps according to LSM6DS33 data sheet */ - m_imuDrv.writeReg(LSM6DS33_ADDR, LSM6DS33_REG_CTRL2_G, 0x00); - break; - default: - isInitSuccessful = false; - break; - } - } - - return isInitSuccessful; -} - -void IMU::enableDefault() -{ - m_imuDrv.enableDefault(); -} - -void IMU::configureForTurnSensing() -{ - m_imuDrv.configureForTurnSensing(); -} - -void IMU::readAccelerometer() -{ - m_imuDrv.readAcc(); - m_accelerometerValues.valueX = m_imuDrv.a.x; - m_accelerometerValues.valueY = m_imuDrv.a.y; - m_accelerometerValues.valueZ = m_imuDrv.a.z; -} - -void IMU::readGyro() -{ - m_imuDrv.readGyro(); - m_gyroValues.valueX = m_imuDrv.g.x; - m_gyroValues.valueY = m_imuDrv.g.y; - m_gyroValues.valueZ = m_imuDrv.g.z; -} - -void IMU::readMagnetometer() -{ - m_imuDrv.readMag(); - m_magnetometerValues.valueX = m_imuDrv.m.x; - m_magnetometerValues.valueY = m_imuDrv.m.y; - m_magnetometerValues.valueZ = m_imuDrv.m.z; -} - -bool IMU::accelerometerDataReady() -{ - return m_imuDrv.accDataReady(); -} - -bool IMU::gyroDataReady() -{ - return m_imuDrv.gyroDataReady(); -} - -bool IMU::magnetometerDataReady() -{ - return m_imuDrv.magDataReady(); -} - -const void IMU::getAccelerationValues(IMUData* accelerationValues) -{ - if (nullptr != accelerationValues) - { - accelerationValues->valueX = m_accelerometerValues.valueX - m_rawAccelerometerOffsetX; - accelerationValues->valueY = m_accelerometerValues.valueY - m_rawAccelerometerOffsetY; - accelerationValues->valueZ = m_accelerometerValues.valueZ - m_rawAccelerometerOffsetZ; - } -} - -const void IMU::getTurnRates(IMUData* turnRates) -{ - if (nullptr != turnRates) - { - turnRates->valueX = m_gyroValues.valueX - m_rawGyroOffsetX; - turnRates->valueY = m_gyroValues.valueY - m_rawGyroOffsetY; - turnRates->valueZ = m_gyroValues.valueZ - m_rawGyroOffsetZ; - } -} - -const void IMU::getMagnetometerValues(IMUData* magnetometerValues) -{ - if (nullptr != magnetometerValues) - { - magnetometerValues->valueX = m_magnetometerValues.valueX; - magnetometerValues->valueY = m_magnetometerValues.valueY; - magnetometerValues->valueZ = m_magnetometerValues.valueZ; - } -} - -/* TODO: TD076 Make sure that it is necessary to read and calibrate all sensor values, if only 1 of the gyro values and - 2 of the acceleration values are being used.*/ -void IMU::calibrate() -{ - /* Define how many measurements should be made for calibration. */ - const uint8_t NUMBER_OF_MEASUREMENTS = 10; - - /* Calibration takes place while the robot doesn't move. Therefore the Acceleration and turn values are near 0 and - * int16_t values are enough. */ - int16_t sumOfRawAccelValuesX = 0; - int16_t sumOfRawAccelValuesY = 0; - int16_t sumOfRawAccelValuesZ = 0; - - int16_t sumOfRawGyroValuesX = 0; - int16_t sumOfRawGyroValuesY = 0; - int16_t sumOfRawGyroValuesZ = 0; - - for (uint8_t measurementIdx = 0; measurementIdx < NUMBER_OF_MEASUREMENTS; ++measurementIdx) - { - while (!m_imuDrv.accDataReady()) - { - /* Do nothing and wait until new Accelerometer Data is available. */ - } - m_imuDrv.readAcc(); - sumOfRawAccelValuesX += m_imuDrv.a.x; - sumOfRawAccelValuesY += m_imuDrv.a.y; - sumOfRawAccelValuesZ += m_imuDrv.a.z; - - while (!m_imuDrv.gyroDataReady()) - { - /* Do nothing and wait until new Gyro Data is available. */ - } - m_imuDrv.readGyro(); - sumOfRawGyroValuesX += m_imuDrv.g.x; - sumOfRawGyroValuesY += m_imuDrv.g.y; - sumOfRawGyroValuesZ += m_imuDrv.g.z; - } - - m_rawAccelerometerOffsetX = sumOfRawAccelValuesX / NUMBER_OF_MEASUREMENTS; - m_rawAccelerometerOffsetY = sumOfRawAccelValuesY / NUMBER_OF_MEASUREMENTS; - m_rawAccelerometerOffsetZ = sumOfRawAccelValuesZ / NUMBER_OF_MEASUREMENTS; - - m_rawGyroOffsetX = sumOfRawGyroValuesX / NUMBER_OF_MEASUREMENTS; - m_rawGyroOffsetY = sumOfRawGyroValuesY / NUMBER_OF_MEASUREMENTS; - m_rawGyroOffsetZ = sumOfRawGyroValuesZ / NUMBER_OF_MEASUREMENTS; -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 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 IMU implementation + * @author Juliane Kerpe + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "IMU.h" +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +bool IMU::init() +{ + bool isInitSuccessful = m_imuDrv.init(); + if (true == isInitSuccessful) + { + m_imuDrv.enableDefault(); + /* TODO: TD074 Make sure that a Full Scale of 245 dps of the gyros are enough */ + /* TODO: TD075 Make sure that it is valid to make the Full Scale setting in IMU::init() */ + switch (m_imuDrv.getType()) + { + case Zumo32U4IMUType::LSM303D_L3GD20H: + /* 0x00 means +/- 245 dps according to L3GD20H data sheet */ + m_imuDrv.writeReg(L3GD20H_ADDR, L3GD20H_REG_CTRL4, 0x00); + break; + case Zumo32U4IMUType::LSM6DS33_LIS3MDL: + /* 0x00 means +/- 245 dps according to LSM6DS33 data sheet */ + m_imuDrv.writeReg(LSM6DS33_ADDR, LSM6DS33_REG_CTRL2_G, 0x00); + break; + default: + isInitSuccessful = false; + break; + } + } + + return isInitSuccessful; +} + +void IMU::enableDefault() +{ + m_imuDrv.enableDefault(); +} + +void IMU::configureForTurnSensing() +{ + m_imuDrv.configureForTurnSensing(); +} + +void IMU::readAccelerometer() +{ + m_imuDrv.readAcc(); + m_accelerometerValues.valueX = m_imuDrv.a.x; + m_accelerometerValues.valueY = m_imuDrv.a.y; + m_accelerometerValues.valueZ = m_imuDrv.a.z; +} + +void IMU::readGyro() +{ + m_imuDrv.readGyro(); + m_gyroValues.valueX = m_imuDrv.g.x; + m_gyroValues.valueY = m_imuDrv.g.y; + m_gyroValues.valueZ = m_imuDrv.g.z; +} + +void IMU::readMagnetometer() +{ + m_imuDrv.readMag(); + m_magnetometerValues.valueX = m_imuDrv.m.x; + m_magnetometerValues.valueY = m_imuDrv.m.y; + m_magnetometerValues.valueZ = m_imuDrv.m.z; +} + +bool IMU::accelerometerDataReady() +{ + return m_imuDrv.accDataReady(); +} + +bool IMU::gyroDataReady() +{ + return m_imuDrv.gyroDataReady(); +} + +bool IMU::magnetometerDataReady() +{ + return m_imuDrv.magDataReady(); +} + +const void IMU::getAccelerationValues(IMUData* accelerationValues) +{ + if (nullptr != accelerationValues) + { + accelerationValues->valueX = m_accelerometerValues.valueX - m_rawAccelerometerOffsetX; + accelerationValues->valueY = m_accelerometerValues.valueY - m_rawAccelerometerOffsetY; + accelerationValues->valueZ = m_accelerometerValues.valueZ - m_rawAccelerometerOffsetZ; + } +} + +const void IMU::getTurnRates(IMUData* turnRates) +{ + if (nullptr != turnRates) + { + turnRates->valueX = m_gyroValues.valueX - m_rawGyroOffsetX; + turnRates->valueY = m_gyroValues.valueY - m_rawGyroOffsetY; + turnRates->valueZ = m_gyroValues.valueZ - m_rawGyroOffsetZ; + } +} + +const void IMU::getMagnetometerValues(IMUData* magnetometerValues) +{ + if (nullptr != magnetometerValues) + { + magnetometerValues->valueX = m_magnetometerValues.valueX; + magnetometerValues->valueY = m_magnetometerValues.valueY; + magnetometerValues->valueZ = m_magnetometerValues.valueZ; + } +} + +/* TODO: TD076 Make sure that it is necessary to read and calibrate all sensor values, if only 1 of the gyro values and + 2 of the acceleration values are being used.*/ +void IMU::calibrate() +{ + /* Define how many measurements should be made for calibration. */ + const uint8_t NUMBER_OF_MEASUREMENTS = 10; + + /* Calibration takes place while the robot doesn't move. Therefore the Acceleration and turn values are near 0 and + * int16_t values are enough. */ + int16_t sumOfRawAccelValuesX = 0; + int16_t sumOfRawAccelValuesY = 0; + int16_t sumOfRawAccelValuesZ = 0; + + int16_t sumOfRawGyroValuesX = 0; + int16_t sumOfRawGyroValuesY = 0; + int16_t sumOfRawGyroValuesZ = 0; + + for (uint8_t measurementIdx = 0; measurementIdx < NUMBER_OF_MEASUREMENTS; ++measurementIdx) + { + while (!m_imuDrv.accDataReady()) + { + /* Do nothing and wait until new Accelerometer Data is available. */ + } + m_imuDrv.readAcc(); + sumOfRawAccelValuesX += m_imuDrv.a.x; + sumOfRawAccelValuesY += m_imuDrv.a.y; + sumOfRawAccelValuesZ += m_imuDrv.a.z; + + while (!m_imuDrv.gyroDataReady()) + { + /* Do nothing and wait until new Gyro Data is available. */ + } + m_imuDrv.readGyro(); + sumOfRawGyroValuesX += m_imuDrv.g.x; + sumOfRawGyroValuesY += m_imuDrv.g.y; + sumOfRawGyroValuesZ += m_imuDrv.g.z; + } + + m_rawAccelerometerOffsetX = sumOfRawAccelValuesX / NUMBER_OF_MEASUREMENTS; + m_rawAccelerometerOffsetY = sumOfRawAccelValuesY / NUMBER_OF_MEASUREMENTS; + m_rawAccelerometerOffsetZ = sumOfRawAccelValuesZ / NUMBER_OF_MEASUREMENTS; + + m_rawGyroOffsetX = sumOfRawGyroValuesX / NUMBER_OF_MEASUREMENTS; + m_rawGyroOffsetY = sumOfRawGyroValuesY / NUMBER_OF_MEASUREMENTS; + m_rawGyroOffsetZ = sumOfRawGyroValuesZ / NUMBER_OF_MEASUREMENTS; +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALTarget/IMU.h b/lib/HALTarget/IMU.h index fe37e116..094827ef 100644 --- a/lib/HALTarget/IMU.h +++ b/lib/HALTarget/IMU.h @@ -1,193 +1,193 @@ -/* MIT License - * - * Copyright (c) 2023 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 IMU (Inertial Measurement Unit) implementation - * @author Juliane Kerpe - * - * @addtogroup HALSim - * - * @{ - */ -#ifndef IMU_H -#define IMU_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "IIMU.h" -#include "Zumo32U4IMU.h" - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The IMU adapter. - * IMU stands for Inertial Measurement Unit. - */ -class IMU : public IIMU -{ -public: - /** - * Constructs the IMU adapter. - * - */ - IMU() : - IIMU(), - m_accelerometerValues{0, 0, 0}, - m_gyroValues{0, 0, 0}, - m_magnetometerValues{0, 0, 0}, - m_rawAccelerometerOffsetX(0), - m_rawAccelerometerOffsetY(0), - m_rawAccelerometerOffsetZ(0), - m_rawGyroOffsetX(0), - m_rawGyroOffsetY(0), - m_rawGyroOffsetZ(0) - { - } - - /** - * Destroys the IMU adapter. - */ - ~IMU() - { - } - - /** - * Initializes the inertial sensors and detects their type. - * - * @return True if the sensor type was detected succesfully; false otherwise. - */ - bool init(); - - /** - * Enables all of the inertial sensors with a default configuration. - */ - void enableDefault(); - - /** - * Configures the sensors with settings optimized for turn sensing. - */ - void configureForTurnSensing(); - - /** - * Takes a reading from the accelerometer and makes the measurements available in a. - */ - void readAccelerometer(); - - /** - * Takes a reading from the gyro and makes the measurements available in g. - */ - void readGyro(); - - /** - * Takes a reading from the magnetometer and makes the measurements available in m. - */ - void readMagnetometer(); - - /** - * Indicates whether the accelerometer has new measurement data ready. - * - * @return True if there is new accelerometer data available; false otherwise. - */ - bool accelerometerDataReady(); - - /** - * Indicates whether the gyro has new measurement data ready. - * - * @return True if there is new gyro data available; false otherwise. - */ - bool gyroDataReady(); - - /** - * Indicates whether the magnetometer has new measurement data ready. - * - * @return True if there is new magnetometer data available; false otherwise. - */ - bool magnetometerDataReady(); - - /** - * Get last raw Accelerometer values as an IMUData struct containing values in x, y and z in digits. - * - * @param[in] accelerationValues Pointer to IMUData struct where the raw acceleration values in digits in - * x, y and z direction will be written into. The values can be converted into physical values in mm/s^2 via the - * multiplication with a sensitivity factor in mm/s^2/digit. - */ - const void getAccelerationValues(IMUData* accelerationValues); - - /** - * Get last raw Gyro values as an IMUData struct containing values in x, y and z in digits. - * - * @param[in] turnRates Pointer to IMUData struct where the raw turn Rates in digits in x, y and z - * direction will be written into. The values can be converted into physical values in mrad/s via the multiplication - * with a sensitivity factor in mrad/s/digit. - */ - const void getTurnRates(IMUData* turnRates); - - /** - * Get last raw Magnetometer values as an IMUData struct containing values in x, y and z in digits. - * - * @param[in] magnetometerValues Pointer to IMUData struct where the raw magnetometer values in digits in - * x, y and z direction will be written into. The values can be converted into physical values in mgauss via the - * multiplication with a sensitivity factor in mgauss/digit. - */ - const void getMagnetometerValues(IMUData* magnetometerValues); - - /** - * Calibrate the IMU. - */ - void calibrate(); - -protected: -private: - IMUData m_accelerometerValues; /**< Raw accelerometer readings. */ - IMUData m_gyroValues; /**< Raw gyro readings. */ - IMUData m_magnetometerValues; /**< Raw magnetometer readings. */ - - Zumo32U4IMU m_imuDrv; /**< IMU driver from Zumo32U4 Library. */ - - int16_t m_rawAccelerometerOffsetX; /**< Mean raw accelerometer offset in x-direction determined by calibration. */ - int16_t m_rawAccelerometerOffsetY; /**< Mean raw accelerometer offset in y-direction determined by calibration. */ - int16_t m_rawAccelerometerOffsetZ; /**< Mean raw accelerometer offset in z-direction determined by calibration. */ - int16_t m_rawGyroOffsetX; /**< Mean raw gyro offset in x-direction determined by calibration. */ - int16_t m_rawGyroOffsetY; /**< Mean raw gyro offset in y-direction determined by calibration. */ - int16_t m_rawGyroOffsetZ; /**< Mean raw gyro offset in z-direction determined by calibration. */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* IMU_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 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 IMU (Inertial Measurement Unit) implementation + * @author Juliane Kerpe + * + * @addtogroup HALSim + * + * @{ + */ +#ifndef IMU_H +#define IMU_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "IIMU.h" +#include "Zumo32U4IMU.h" + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The IMU adapter. + * IMU stands for Inertial Measurement Unit. + */ +class IMU : public IIMU +{ +public: + /** + * Constructs the IMU adapter. + * + */ + IMU() : + IIMU(), + m_accelerometerValues{0, 0, 0}, + m_gyroValues{0, 0, 0}, + m_magnetometerValues{0, 0, 0}, + m_rawAccelerometerOffsetX(0), + m_rawAccelerometerOffsetY(0), + m_rawAccelerometerOffsetZ(0), + m_rawGyroOffsetX(0), + m_rawGyroOffsetY(0), + m_rawGyroOffsetZ(0) + { + } + + /** + * Destroys the IMU adapter. + */ + ~IMU() + { + } + + /** + * Initializes the inertial sensors and detects their type. + * + * @return True if the sensor type was detected succesfully; false otherwise. + */ + bool init(); + + /** + * Enables all of the inertial sensors with a default configuration. + */ + void enableDefault(); + + /** + * Configures the sensors with settings optimized for turn sensing. + */ + void configureForTurnSensing(); + + /** + * Takes a reading from the accelerometer and makes the measurements available in a. + */ + void readAccelerometer(); + + /** + * Takes a reading from the gyro and makes the measurements available in g. + */ + void readGyro(); + + /** + * Takes a reading from the magnetometer and makes the measurements available in m. + */ + void readMagnetometer(); + + /** + * Indicates whether the accelerometer has new measurement data ready. + * + * @return True if there is new accelerometer data available; false otherwise. + */ + bool accelerometerDataReady(); + + /** + * Indicates whether the gyro has new measurement data ready. + * + * @return True if there is new gyro data available; false otherwise. + */ + bool gyroDataReady(); + + /** + * Indicates whether the magnetometer has new measurement data ready. + * + * @return True if there is new magnetometer data available; false otherwise. + */ + bool magnetometerDataReady(); + + /** + * Get last raw Accelerometer values as an IMUData struct containing values in x, y and z in digits. + * + * @param[in] accelerationValues Pointer to IMUData struct where the raw acceleration values in digits in + * x, y and z direction will be written into. The values can be converted into physical values in mm/s^2 via the + * multiplication with a sensitivity factor in mm/s^2/digit. + */ + const void getAccelerationValues(IMUData* accelerationValues); + + /** + * Get last raw Gyro values as an IMUData struct containing values in x, y and z in digits. + * + * @param[in] turnRates Pointer to IMUData struct where the raw turn Rates in digits in x, y and z + * direction will be written into. The values can be converted into physical values in mrad/s via the multiplication + * with a sensitivity factor in mrad/s/digit. + */ + const void getTurnRates(IMUData* turnRates); + + /** + * Get last raw Magnetometer values as an IMUData struct containing values in x, y and z in digits. + * + * @param[in] magnetometerValues Pointer to IMUData struct where the raw magnetometer values in digits in + * x, y and z direction will be written into. The values can be converted into physical values in mgauss via the + * multiplication with a sensitivity factor in mgauss/digit. + */ + const void getMagnetometerValues(IMUData* magnetometerValues); + + /** + * Calibrate the IMU. + */ + void calibrate(); + +protected: +private: + IMUData m_accelerometerValues; /**< Raw accelerometer readings. */ + IMUData m_gyroValues; /**< Raw gyro readings. */ + IMUData m_magnetometerValues; /**< Raw magnetometer readings. */ + + Zumo32U4IMU m_imuDrv; /**< IMU driver from Zumo32U4 Library. */ + + int16_t m_rawAccelerometerOffsetX; /**< Mean raw accelerometer offset in x-direction determined by calibration. */ + int16_t m_rawAccelerometerOffsetY; /**< Mean raw accelerometer offset in y-direction determined by calibration. */ + int16_t m_rawAccelerometerOffsetZ; /**< Mean raw accelerometer offset in z-direction determined by calibration. */ + int16_t m_rawGyroOffsetX; /**< Mean raw gyro offset in x-direction determined by calibration. */ + int16_t m_rawGyroOffsetY; /**< Mean raw gyro offset in y-direction determined by calibration. */ + int16_t m_rawGyroOffsetZ; /**< Mean raw gyro offset in z-direction determined by calibration. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* IMU_H */ +/** @} */ diff --git a/lib/HALTest/IMU.cpp b/lib/HALTest/IMU.cpp index 3b38f516..fbabf3e2 100644 --- a/lib/HALTest/IMU.cpp +++ b/lib/HALTest/IMU.cpp @@ -1,75 +1,75 @@ -/* MIT License - * - * Copyright (c) 2023 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 IMU implementation - * @author Juliane Kerpe - * - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "IMU.h" -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 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 IMU implementation + * @author Juliane Kerpe + * + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "IMU.h" +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALTest/IMU.h b/lib/HALTest/IMU.h index befc7282..958737f0 100644 --- a/lib/HALTest/IMU.h +++ b/lib/HALTest/IMU.h @@ -1,192 +1,192 @@ -/* MIT License - * - * Copyright (c) 2023 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 IMU (Inertial Measurement Unit) realization - * @author Juliane Kerpe - * - * @addtogroup HALSim - * - * @{ - */ - -#ifndef IMU_H -#define IMU_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "IIMU.h" - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The IMU adapter. - * IMU stands for Inertial Measurement Unit. - */ -class IMU : public IIMU -{ -public: - /** - * Destroys the interface. - */ - ~IMU() - { - } - - /** - * Initializes the inertial sensors and detects their type. - * - * @return True if the sensor type was detected succesfully; false otherwise. - */ - bool init() - { - return true; - } - - /** - * Enables all of the inertial sensors with a default configuration. - */ - void enableDefault() - { - }; - - /** - * Configures the sensors with settings optimized for turn sensing. - */ - void configureForTurnSensing() - { - } - - /** - * Takes a reading from the accelerometer and makes the measurements available in a. - */ - void readAccelerometer() - { - } - - /** - * Takes a reading from the gyro and makes the measurements available in g. - */ - void readGyro() - { - } - - /** - * Takes a reading from the magnetometer and makes the measurements available in m. - */ - void readMagnetometer() - { - } - - /** - * Indicates whether the accelerometer has new measurement data ready. - * - * @return True if there is new accelerometer data available; false otherwise. - */ - bool accelerometerDataReady() - { - return true; - } - - /** - * Indicates whether the gyro has new measurement data ready. - * - * @return True if there is new gyro data available; false otherwise. - */ - bool gyroDataReady() - { - return true; - } - - /** - * Indicates whether the magnetometer has new measurement data ready. - * - * @return True if there is new magnetometer data available; false otherwise. - */ - bool magnetometerDataReady() - { - return true; - } - - /** - * Get last raw Accelerometer values as an IMUData struct containing values in x, y and z in digits. - * - * @param[in] accelerationValues Pointer to IMUData struct where the raw acceleration values in digits in - * x, y and z direction will be written into. The values can be converted into physical values in mm/s^2 via the - * multiplication with a sensitivity factor in mm/s^2/digit. - */ - const void getAccelerationValues(IMUData* accelerationValues) - { - } - - /** - * Get last raw Gyro values as an IMUData struct containing values in x, y and z in digits. - * - * @param[in] turnRates Pointer to IMUData struct where the raw turn Rates in digits in x, y and z - * direction will be written into. The values can be converted into physical values in mrad/s via the multiplication - * with a sensitivity factor in mrad/s/digit. - */ - const void getTurnRates(IMUData* turnRates) - { - } - - /** - * Get last raw Magnetometer values as an IMUData struct containing values in x, y and z in digits. - * - * @param[in] magnetometerValues Pointer to IMUData struct where the raw magnetometer values in digits in - * x, y and z direction will be written into. The values can be converted into physical values in mgauss via the - * multiplication with a sensitivity factor in mgauss/digit. - */ - const void getMagnetometerValues(IMUData* magnetometerValues) - { - } - - /** - * Calibrate the IMU. - */ - virtual void calibrate() - { - } - -private: -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* IMU_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 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 IMU (Inertial Measurement Unit) realization + * @author Juliane Kerpe + * + * @addtogroup HALSim + * + * @{ + */ + +#ifndef IMU_H +#define IMU_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "IIMU.h" + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The IMU adapter. + * IMU stands for Inertial Measurement Unit. + */ +class IMU : public IIMU +{ +public: + /** + * Destroys the interface. + */ + ~IMU() + { + } + + /** + * Initializes the inertial sensors and detects their type. + * + * @return True if the sensor type was detected succesfully; false otherwise. + */ + bool init() + { + return true; + } + + /** + * Enables all of the inertial sensors with a default configuration. + */ + void enableDefault() + { + }; + + /** + * Configures the sensors with settings optimized for turn sensing. + */ + void configureForTurnSensing() + { + } + + /** + * Takes a reading from the accelerometer and makes the measurements available in a. + */ + void readAccelerometer() + { + } + + /** + * Takes a reading from the gyro and makes the measurements available in g. + */ + void readGyro() + { + } + + /** + * Takes a reading from the magnetometer and makes the measurements available in m. + */ + void readMagnetometer() + { + } + + /** + * Indicates whether the accelerometer has new measurement data ready. + * + * @return True if there is new accelerometer data available; false otherwise. + */ + bool accelerometerDataReady() + { + return true; + } + + /** + * Indicates whether the gyro has new measurement data ready. + * + * @return True if there is new gyro data available; false otherwise. + */ + bool gyroDataReady() + { + return true; + } + + /** + * Indicates whether the magnetometer has new measurement data ready. + * + * @return True if there is new magnetometer data available; false otherwise. + */ + bool magnetometerDataReady() + { + return true; + } + + /** + * Get last raw Accelerometer values as an IMUData struct containing values in x, y and z in digits. + * + * @param[in] accelerationValues Pointer to IMUData struct where the raw acceleration values in digits in + * x, y and z direction will be written into. The values can be converted into physical values in mm/s^2 via the + * multiplication with a sensitivity factor in mm/s^2/digit. + */ + const void getAccelerationValues(IMUData* accelerationValues) + { + } + + /** + * Get last raw Gyro values as an IMUData struct containing values in x, y and z in digits. + * + * @param[in] turnRates Pointer to IMUData struct where the raw turn Rates in digits in x, y and z + * direction will be written into. The values can be converted into physical values in mrad/s via the multiplication + * with a sensitivity factor in mrad/s/digit. + */ + const void getTurnRates(IMUData* turnRates) + { + } + + /** + * Get last raw Magnetometer values as an IMUData struct containing values in x, y and z in digits. + * + * @param[in] magnetometerValues Pointer to IMUData struct where the raw magnetometer values in digits in + * x, y and z direction will be written into. The values can be converted into physical values in mgauss via the + * multiplication with a sensitivity factor in mgauss/digit. + */ + const void getMagnetometerValues(IMUData* magnetometerValues) + { + } + + /** + * Calibrate the IMU. + */ + virtual void calibrate() + { + } + +private: +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* IMU_H */ +/** @} */ diff --git a/test/test_RelativeEncoders/test_RelativeEncoders.cpp b/test/test_RelativeEncoders/test_RelativeEncoders.cpp index 73356a65..cb470922 100644 --- a/test/test_RelativeEncoders/test_RelativeEncoders.cpp +++ b/test/test_RelativeEncoders/test_RelativeEncoders.cpp @@ -1,181 +1,181 @@ -/* MIT License - * - * Copyright (c) 2023 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. - */ - -/** - * @author Andreas Merkle - * @brief This module contains the program entry point for the tests. - */ - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ - -#include -#include -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/** Test vector element */ -typedef struct -{ - int16_t encoderStepsLeft; /**< Absolute encoder steps left */ - int16_t encoderStepsRight; /**< Absolute encoder steps right */ - bool isTest; /**< Test it or is it for test preparation? */ - int16_t expectedDiffLeft; /**< Expected step difference left */ - int16_t expectedDiffRight; /**< Expected step difference right */ - -} Elem; - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -static void testRelativeEncoders(); - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/** - * Program setup routine, which is called once at startup. - */ -void setup() -{ -#ifndef TARGET_NATIVE - /* https://docs.platformio.org/en/latest/plus/unit-testing.html#demo */ - delay(2000); -#endif /* Not defined TARGET_NATIVE */ -} - -/** - * Main entry point. - */ -void loop() -{ - UNITY_BEGIN(); - - RUN_TEST(testRelativeEncoders); - - UNITY_END(); - -#ifndef TARGET_NATIVE - /* Don't exit on the robot to avoid a endless test loop. - * If the test runs on the pc, it must exit. - */ - for (;;) - { - } -#endif /* Not defined TARGET_NATIVE */ -} - -/** - * Initialize the test setup. - */ -extern void setUp(void) -{ - /* Not used. */ -} - -/** - * Clean up test setup. - */ -extern void tearDown(void) -{ - /* Not used. */ -} - -/****************************************************************************** - * Local Functions - *****************************************************************************/ - -/** - * Test the relative encoders unit. - */ -static void testRelativeEncoders() -{ - IEncoders& absEncoders = Board::getInstance().getEncoders(); - IEncodersTest& absEncodersTest = Board::getInstance().getEncodersTest(); - RelativeEncoders relEncoders(absEncoders); - Elem testVector[] = - { - { 0, 0, true, 0, 0 }, - { 10, 10, true, 10, 10 }, - { -10, -10, true, -10, -10 }, - { INT16_MAX, INT16_MAX, false, 0, 0 }, /* Preparation for wrap-around */ - { INT16_MIN, INT16_MIN, true, 1, 1 }, - { INT16_MIN, INT16_MIN, false, 0, 0 }, /* Preparation for wrap-around */ - { INT16_MAX, INT16_MAX, true, -1, -1 } - }; - uint8_t idx = 0; - - idx = 0; - while ((sizeof(testVector) / sizeof(testVector[0])) > idx) - { - absEncodersTest.setCountsLeft(testVector[idx].encoderStepsLeft); - absEncodersTest.setCountsRight(testVector[idx].encoderStepsRight); - - if (false == testVector[idx].isTest) - { - relEncoders.clearLeft(); - relEncoders.clearRight(); - } - else - { - TEST_ASSERT_EQUAL_INT16(testVector[idx].expectedDiffLeft, relEncoders.getCountsLeft()); - TEST_ASSERT_EQUAL_INT16(testVector[idx].expectedDiffRight, relEncoders.getCountsRight()); - } - - ++idx; - } -} +/* MIT License + * + * Copyright (c) 2023 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. + */ + +/** + * @author Andreas Merkle + * @brief This module contains the program entry point for the tests. + */ + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ + +#include +#include +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/** Test vector element */ +typedef struct +{ + int16_t encoderStepsLeft; /**< Absolute encoder steps left */ + int16_t encoderStepsRight; /**< Absolute encoder steps right */ + bool isTest; /**< Test it or is it for test preparation? */ + int16_t expectedDiffLeft; /**< Expected step difference left */ + int16_t expectedDiffRight; /**< Expected step difference right */ + +} Elem; + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +static void testRelativeEncoders(); + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/** + * Program setup routine, which is called once at startup. + */ +void setup() +{ +#ifndef TARGET_NATIVE + /* https://docs.platformio.org/en/latest/plus/unit-testing.html#demo */ + delay(2000); +#endif /* Not defined TARGET_NATIVE */ +} + +/** + * Main entry point. + */ +void loop() +{ + UNITY_BEGIN(); + + RUN_TEST(testRelativeEncoders); + + UNITY_END(); + +#ifndef TARGET_NATIVE + /* Don't exit on the robot to avoid a endless test loop. + * If the test runs on the pc, it must exit. + */ + for (;;) + { + } +#endif /* Not defined TARGET_NATIVE */ +} + +/** + * Initialize the test setup. + */ +extern void setUp(void) +{ + /* Not used. */ +} + +/** + * Clean up test setup. + */ +extern void tearDown(void) +{ + /* Not used. */ +} + +/****************************************************************************** + * Local Functions + *****************************************************************************/ + +/** + * Test the relative encoders unit. + */ +static void testRelativeEncoders() +{ + IEncoders& absEncoders = Board::getInstance().getEncoders(); + IEncodersTest& absEncodersTest = Board::getInstance().getEncodersTest(); + RelativeEncoders relEncoders(absEncoders); + Elem testVector[] = + { + { 0, 0, true, 0, 0 }, + { 10, 10, true, 10, 10 }, + { -10, -10, true, -10, -10 }, + { INT16_MAX, INT16_MAX, false, 0, 0 }, /* Preparation for wrap-around */ + { INT16_MIN, INT16_MIN, true, 1, 1 }, + { INT16_MIN, INT16_MIN, false, 0, 0 }, /* Preparation for wrap-around */ + { INT16_MAX, INT16_MAX, true, -1, -1 } + }; + uint8_t idx = 0; + + idx = 0; + while ((sizeof(testVector) / sizeof(testVector[0])) > idx) + { + absEncodersTest.setCountsLeft(testVector[idx].encoderStepsLeft); + absEncodersTest.setCountsRight(testVector[idx].encoderStepsRight); + + if (false == testVector[idx].isTest) + { + relEncoders.clearLeft(); + relEncoders.clearRight(); + } + else + { + TEST_ASSERT_EQUAL_INT16(testVector[idx].expectedDiffLeft, relEncoders.getCountsLeft()); + TEST_ASSERT_EQUAL_INT16(testVector[idx].expectedDiffRight, relEncoders.getCountsRight()); + } + + ++idx; + } +} diff --git a/webots/controllers/Supervisor/Supervisor.py b/webots/controllers/Supervisor/Supervisor.py index bf730559..aca52fea 100644 --- a/webots/controllers/Supervisor/Supervisor.py +++ b/webots/controllers/Supervisor/Supervisor.py @@ -1,129 +1,129 @@ -"""Supervisor controller. - Details: https://github.com/cyberbotics/webots/blob/master/docs/guide/supervisor-programming.md -""" - -import math -import sys -from controller import Supervisor - -# Create the Supervisor instance. -supervisor = Supervisor() - -# The PROTO DEF name must be given! -ROBOT_NAME = "ROBOT" - -def get_robot_translation_values(robot_node): - """Get robot translation field values. - - Args: - robot_node (robot): The robot node. - - Returns: - list: List of translation field values for x, y and z. - """ - robot_field_translation = robot_node.getField("translation") - return robot_field_translation.getSFVec3f() - -def get_robot_rotation_values(robot_node): - """Get robot rotation field values. - - Args: - robot_node (robot): The robot node. - - Returns: - list: List of rotation field values for x, y, z and angle. - """ - robot_field_rotation = robot_node.getField("rotation") - return robot_field_rotation.getSFRotation() - -def get_robot_3d_rel_position(robot_node, prev_translation_values): - """Get robot 3d relative position (x, y). - - Args: - robot_node (robot): The robot node. - prev_translation_values (list): Absolute translation values to calculate the relative from. - - Returns: - list: List with position (x, y, z) in [mm] - """ - vector_idx_x = 0 - vector_idx_y = 1 - vector_idx_z = 2 - factor_m_to_mm = 1000 # Conversion factor from [m] to [mm] - - robot_translation_values = get_robot_translation_values(robot_node) - - rel_pos_x = ( robot_translation_values[vector_idx_x] - prev_translation_values[vector_idx_x] ) \ - * factor_m_to_mm # [mm] - rel_pos_y = ( robot_translation_values[vector_idx_y] - prev_translation_values[vector_idx_y] ) \ - * factor_m_to_mm # [mm] - rel_pos_z = ( robot_translation_values[vector_idx_z] - prev_translation_values[vector_idx_z] ) \ - * factor_m_to_mm # [mm] - - return rel_pos_x, rel_pos_y, rel_pos_z - -def get_robot_2d_orientation(robot_node): - """Get the robot 2d orientation angle [-PI, PI]. - - Args: - robot_node (Robot): The robot node. - - Returns: - float: Orientation angle in [rad] - """ - vector_idx_angle = 3 - - robot_rotation_values = get_robot_rotation_values(robot_node) - - return robot_rotation_values[vector_idx_angle] - -def main_loop(): - """Main loop: - - Perform simulation steps until Webots is stopping the controller- - - Returns: - number: Status - """ - status = 0 - - # Get the time step of the current world. - timestep = int(supervisor.getBasicTimeStep()) - - # 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: - - # Get robot absolute start position - robot_start_translation_values = get_robot_translation_values(robot_node) - - prev_pos_x = -1 - prev_pos_y = -1 - prev_orientation_deg = -1 - - while supervisor.step(timestep) != -1: - pos_x, pos_y, _ = get_robot_3d_rel_position(robot_node, robot_start_translation_values) - orientation = get_robot_2d_orientation(robot_node) - orientation_deg = math.degrees(orientation) - - # Only interested in the floor part - pos_x = int(pos_x) - pos_y = int(pos_y) - orientation_deg = int(orientation_deg) - - # pylint: disable=line-too-long - if (prev_pos_x != pos_x) or (prev_pos_y != pos_y) or (prev_orientation_deg != orientation_deg): - print(f"{pos_x}, {pos_y}, {orientation_deg}\n") - - prev_pos_x = pos_x - prev_pos_y = pos_y - prev_orientation_deg = orientation_deg - - return status - -sys.exit(main_loop()) +"""Supervisor controller. + Details: https://github.com/cyberbotics/webots/blob/master/docs/guide/supervisor-programming.md +""" + +import math +import sys +from controller import Supervisor + +# Create the Supervisor instance. +supervisor = Supervisor() + +# The PROTO DEF name must be given! +ROBOT_NAME = "ROBOT" + +def get_robot_translation_values(robot_node): + """Get robot translation field values. + + Args: + robot_node (robot): The robot node. + + Returns: + list: List of translation field values for x, y and z. + """ + robot_field_translation = robot_node.getField("translation") + return robot_field_translation.getSFVec3f() + +def get_robot_rotation_values(robot_node): + """Get robot rotation field values. + + Args: + robot_node (robot): The robot node. + + Returns: + list: List of rotation field values for x, y, z and angle. + """ + robot_field_rotation = robot_node.getField("rotation") + return robot_field_rotation.getSFRotation() + +def get_robot_3d_rel_position(robot_node, prev_translation_values): + """Get robot 3d relative position (x, y). + + Args: + robot_node (robot): The robot node. + prev_translation_values (list): Absolute translation values to calculate the relative from. + + Returns: + list: List with position (x, y, z) in [mm] + """ + vector_idx_x = 0 + vector_idx_y = 1 + vector_idx_z = 2 + factor_m_to_mm = 1000 # Conversion factor from [m] to [mm] + + robot_translation_values = get_robot_translation_values(robot_node) + + rel_pos_x = ( robot_translation_values[vector_idx_x] - prev_translation_values[vector_idx_x] ) \ + * factor_m_to_mm # [mm] + rel_pos_y = ( robot_translation_values[vector_idx_y] - prev_translation_values[vector_idx_y] ) \ + * factor_m_to_mm # [mm] + rel_pos_z = ( robot_translation_values[vector_idx_z] - prev_translation_values[vector_idx_z] ) \ + * factor_m_to_mm # [mm] + + return rel_pos_x, rel_pos_y, rel_pos_z + +def get_robot_2d_orientation(robot_node): + """Get the robot 2d orientation angle [-PI, PI]. + + Args: + robot_node (Robot): The robot node. + + Returns: + float: Orientation angle in [rad] + """ + vector_idx_angle = 3 + + robot_rotation_values = get_robot_rotation_values(robot_node) + + return robot_rotation_values[vector_idx_angle] + +def main_loop(): + """Main loop: + - Perform simulation steps until Webots is stopping the controller- + + Returns: + number: Status + """ + status = 0 + + # Get the time step of the current world. + timestep = int(supervisor.getBasicTimeStep()) + + # 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: + + # Get robot absolute start position + robot_start_translation_values = get_robot_translation_values(robot_node) + + prev_pos_x = -1 + prev_pos_y = -1 + prev_orientation_deg = -1 + + while supervisor.step(timestep) != -1: + pos_x, pos_y, _ = get_robot_3d_rel_position(robot_node, robot_start_translation_values) + orientation = get_robot_2d_orientation(robot_node) + orientation_deg = math.degrees(orientation) + + # Only interested in the floor part + pos_x = int(pos_x) + pos_y = int(pos_y) + orientation_deg = int(orientation_deg) + + # pylint: disable=line-too-long + if (prev_pos_x != pos_x) or (prev_pos_y != pos_y) or (prev_orientation_deg != orientation_deg): + print(f"{pos_x}, {pos_y}, {orientation_deg}\n") + + prev_pos_x = pos_x + prev_pos_y = pos_y + prev_orientation_deg = orientation_deg + + return status + +sys.exit(main_loop()) diff --git a/webots/protos/LineFollowerTrack.proto b/webots/protos/LineFollowerTrack.proto index 221447eb..d9f6ad83 100644 --- a/webots/protos/LineFollowerTrack.proto +++ b/webots/protos/LineFollowerTrack.proto @@ -1,39 +1,39 @@ -#VRML_SIM R2023a utf8 - -EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/floors/protos/RectangleArena.proto" - -PROTO LineFollowerTrack [ - -] -{ - RectangleArena { - floorSize 2 2 - floorTileSize 4 4 - floorAppearance PBRAppearance { - baseColorMap ImageTexture { - url [ - "./track.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 - } +#VRML_SIM R2023a utf8 + +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/floors/protos/RectangleArena.proto" + +PROTO LineFollowerTrack [ + +] +{ + RectangleArena { + floorSize 2 2 + floorTileSize 4 4 + floorAppearance PBRAppearance { + baseColorMap ImageTexture { + url [ + "./track.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/protos/Supervisor.proto b/webots/protos/Supervisor.proto index 4fba2fb6..91ac0ae0 100644 --- a/webots/protos/Supervisor.proto +++ b/webots/protos/Supervisor.proto @@ -1,15 +1,15 @@ -#VRML_SIM R2023b utf8 -# The supervisor observes the world and its robots. On demand it may influence them. - -PROTO Supervisor [ +#VRML_SIM R2023b utf8 +# The supervisor observes the world and its robots. On demand it may influence them. + +PROTO Supervisor [ field SFString name "robot" field SFString controller "" - field SFBool supervisor FALSE -] -{ + field SFBool supervisor FALSE +] +{ Robot { name IS name controller IS controller supervisor IS supervisor - } -} + } +} diff --git a/webots/protos/Zumo32U4.proto b/webots/protos/Zumo32U4.proto index ad7e31ea..0ef47325 100644 --- a/webots/protos/Zumo32U4.proto +++ b/webots/protos/Zumo32U4.proto @@ -1,566 +1,566 @@ -#VRML_SIM R2023a utf8 - -EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/appearances/protos/Rubber.proto" - -PROTO Zumo32U4 [ - field SFVec3f translation 0 0 0 - field SFRotation rotation 0 0 1 0 - field SFString name "robot" -] -{ - Robot { - translation IS translation - rotation IS rotation - children [ - DEF SHIELD Group { - children [ - Transform { - translation 0.047 0 0.007 - rotation 0 1 0 -0.42 - children [ - Shape { - appearance PBRAppearance { - emissiveColor 0.4 0.4 0.4 - } - geometry Box { - size 0.001 0.1 0.04 - } - } - ] - } - ] - } - DEF TRACK_LEFT Track { - translation 0 0.0375 0 - rotation 0 1 0 0 - children [ - DEF WHEEL_FL TrackWheel { - position 0.0225 0.005 - rotation 0.6681343103444981 -0.5261152487176364 0.5261171812507756 1.9635600942618647 - radius 0.018 - children [ - DEF TRACK_WHEEL Transform { - children [ - Shape { - appearance DEF WHEEL_COLOR PBRAppearance { - baseColor 0 0 0 - metalness 0 - } - geometry Cylinder { - height 0.01 - radius 0.0175 - } - } - ] - } - ] - } - DEF WHEEL_RL TrackWheel { - position -0.0225 0.005 - rotation 0.6681343103444981 -0.5261152487176364 0.5261171812507756 1.9635600942618647 - radius 0.018 - children [ - USE TRACK_WHEEL - ] - } - ] - name "left_track" - boundingObject DEF TRACK_BO Group { - children [ - Transform { - translation 0 0 0.005 - children [ - Box { - size 0.01 0.01 0.038 - } - ] - } - Transform { - translation 0.0225 0 0.005 - rotation 1 0 0 1.5708 - children [ - DEF WHEEL_BO Cylinder { - height 0.01 - radius 0.019 - } - ] - } - Transform { - translation -0.0225 0 0.00499976 - rotation 1 0 0 1.5708 - children [ - USE WHEEL_BO - ] - } - ] - } - physics DEF TRACK_PH Physics { - } - device [ - LinearMotor { - name "motor_left" - maxVelocity 0.3 - } - PositionSensor { - name "position_sensor_left" - } - ] - animatedGeometry DEF ANIM_WHEEL Shape { - appearance Rubber { - } - geometry Box { - size 0.003 0.008 0.002 - } - } - geometriesCount 70 - } - DEF TRACK_RIGHT Track { - translation 0 -0.0375 0 - children [ - DEF WHEEL_FR TrackWheel { - position 0.0225 0.005 - rotation 0.6681343103444981 0.5261152487176364 -0.5261171812507756 1.9635600942618647 - radius 0.018 - children [ - DEF TRACK_WHEEL Transform { - children [ - Shape { - appearance DEF WHEEL_COLOR PBRAppearance { - baseColor 0 0 0 - metalness 0 - } - geometry Cylinder { - height 0.01 - radius 0.018 - } - } - ] - } - ] - } - DEF WHEEL_RR TrackWheel { - position -0.0225 0.005 - rotation 0.6681343103444981 0.5261152487176364 -0.5261171812507756 1.9635600942618647 - radius 0.018 - children [ - USE TRACK_WHEEL - ] - } - ] - name "track_right" - boundingObject USE TRACK_BO - physics USE TRACK_PH - device [ - LinearMotor { - name "motor_right" - maxVelocity 0.3 - } - PositionSensor { - name "position_sensor_right" - } - ] - animatedGeometry USE ANIM_WHEEL - geometriesCount 70 - } - DEF BODY Group { - children [ - Shape { - appearance DEF BODY_COLOR PBRAppearance { - emissiveColor 0.2 0.2 0.2 - } - geometry DEF BODY_GEOMETRY Box { - size 0.06 0.065 0.02 - } - } - Transform { - translation 0.0325 0 0.0025 - children [ - Shape { - appearance USE BODY_COLOR - geometry Box { - size 0.025 0.065 0.015 - } - } - ] - } - Transform { - translation -0.01 0 0 - children [ - Shape { - appearance USE BODY_COLOR - geometry USE BODY_GEOMETRY - } - ] - } - ] - } - DEF LINE_SENSOR_ARRAY Group { - children [ - Transform { - translation 0.0425 0 -0.0075 - children [ - Shape { - appearance USE BODY_COLOR - geometry Box { - size 0.005 0.01 0.005 - } - } - ] - } - Transform { - translation 0.049 0 -0.009 - children [ - Shape { - appearance USE BODY_COLOR - geometry Box { - size 0.008 0.095 0.002 - } - } - ] - } - DistanceSensor { - translation 0.05 0.001 -0.01 - rotation 0 1 0 1.5708 - children [ - Shape { - appearance DEF LSAPPEARANCE PBRAppearance { - baseColor 0 0 0 - metalness 0 - } - geometry DEF LIGHTSENSOR Box { - size 0.00025 0.002 0.004 - } - } - ] - name "lightsensor_m" - boundingObject USE LIGHTSENSOR - physics DEF LIGHTSENSOR_PH Physics { - } - lookupTable [ - 0.008 0 0 - 0.02 1000 0 - ] - type "infra-red" - numberOfRays 2 - aperture 0.4 - } - Emitter { - translation 0.05 -0.001 -0.01 - rotation 0 1 0 1.5708 - children [ - Shape { - appearance DEF EMAPPEARANCE PBRAppearance { - baseColor 1 0 0 - metalness 0 - } - geometry DEF EMITTER Box { - size 0.00025 0.002 0.004 - } - } - ] - name "emitter_m" - boundingObject USE EMITTER - physics DEF EMITTER_PH Physics { - } - type "infra-red" - aperture 0.35 - } - DistanceSensor { - translation 0.05 0.011 -0.01 - rotation 0 1 0 1.5708 - children [ - Shape { - appearance USE LSAPPEARANCE - geometry USE LIGHTSENSOR - } - ] - name "lightsensor_lm" - boundingObject USE LIGHTSENSOR - physics USE LIGHTSENSOR_PH - lookupTable [ - 0.008 0 0 - 0.02 1000 0 - ] - type "infra-red" - numberOfRays 2 - aperture 0.4 - } - Emitter { - translation 0.05 0.009 -0.01 - rotation 0 1 0 1.5708 - children [ - Shape { - appearance USE EMAPPEARANCE - geometry USE EMITTER - } - ] - name "emitter_lm" - boundingObject USE EMITTER - physics USE EMITTER_PH - type "infra-red" - aperture 0.35 - } - DistanceSensor { - translation 0.05 -0.009 -0.01 - rotation 0 1 0 1.5708 - children [ - Shape { - appearance USE LSAPPEARANCE - geometry USE LIGHTSENSOR - } - ] - name "lightsensor_rm" - boundingObject USE LIGHTSENSOR - physics USE LIGHTSENSOR_PH - lookupTable [ - 0.008 0 0 - 0.02 1000 0 - ] - type "infra-red" - numberOfRays 2 - aperture 0.4 - } - Emitter { - translation 0.05 -0.011 -0.01 - rotation 0 1 0 1.5708 - children [ - Shape { - appearance USE EMAPPEARANCE - geometry USE EMITTER - } - ] - name "emitter_rm" - boundingObject USE EMITTER - physics USE EMITTER_PH - type "infra-red" - aperture 0.35 - } - DistanceSensor { - translation 0.05 0.046 -0.01 - rotation 0 1 0 1.5708 - children [ - Shape { - appearance USE LSAPPEARANCE - geometry USE LIGHTSENSOR - } - ] - name "lightsensor_l" - boundingObject USE LIGHTSENSOR - physics USE LIGHTSENSOR_PH - lookupTable [ - 0.008 0 0 - 0.02 1000 0 - ] - type "infra-red" - numberOfRays 2 - aperture 0.4 - } - Emitter { - translation 0.05 0.044 -0.01 - rotation 0 1 0 1.5708 - children [ - Shape { - appearance USE EMAPPEARANCE - geometry USE EMITTER - } - ] - name "emitter_l" - boundingObject USE EMITTER - physics USE EMITTER_PH - type "infra-red" - aperture 0.35 - } - DistanceSensor { - translation 0.05 -0.044 -0.01 - rotation 0 1 0 1.5708 - children [ - Shape { - appearance USE LSAPPEARANCE - geometry USE LIGHTSENSOR - } - ] - name "lightsensor_r" - boundingObject USE LIGHTSENSOR - physics USE LIGHTSENSOR_PH - lookupTable [ - 0.008 0 0 - 0.02 1000 0 - ] - type "infra-red" - numberOfRays 2 - aperture 0.4 - } - Emitter { - translation 0.05 -0.046 -0.01 - rotation 0 1 0 1.5708 - children [ - Shape { - appearance USE EMAPPEARANCE - geometry USE EMITTER - } - ] - name "emitter_r" - boundingObject USE EMITTER - physics USE EMITTER_PH - type "infra-red" - aperture 0.35 - } - DistanceSensor { - translation 0.055 0.02 0.02 - name "proxim_sensor_fl" - lookupTable [ - 0 6 0 - 0.05 6 0 - 0.1 5 0 - 0.15 6 0 - 0.2 3 0 - 0.25 2 0 - 0.3 1 0 - 0.301 0 0 - ] - type "infra-red" - numberOfRays 4 - aperture 0.7 - } - DistanceSensor { - translation 0.055 -0.02 0.02 - name "proxim_sensor_fr" - lookupTable [ - 0 6 0 - 0.05 6 0 - 0.1 5 0 - 0.15 6 0 - 0.2 3 0 - 0.25 2 0 - 0.3 1 0 - 0.301 0 0 - ] - type "infra-red" - numberOfRays 4 - aperture 0.7 - } - ] - } - DEF IMU Group { - children [ - Accelerometer { - name "accelerometer" - xAxis TRUE - yAxis TRUE - zAxis TRUE - resolution 0.001 - lookupTable [ - -19.62 -32768 0 - 0 0 0 - 19.62 32767 0 - ] - } - Gyro { - name "gyro" - xAxis TRUE - yAxis TRUE - zAxis TRUE - resolution 0.001 - lookupTable [ - -4.276 -32768 0 - 0 0 0 - 4.276 32767 0 - ] - } - Compass { - name "magnetometer" - xAxis TRUE - yAxis TRUE - zAxis TRUE - resolution 0.001 - lookupTable [ - -1 -32000 0 - 0 0 0 - 1 32000 0 - ] - } - ] - } - - DEF LEDS Group { - children [ - LED { - translation -0.03 0.02 0.01 - rotation 0 1 0 0 - children [ - DEF LEDGSHAPE Shape { - appearance PBRAppearance { - baseColor 0 0 0 - metalness 0 - } - geometry Sphere { - radius 0.005 - } - } - ] - name "led_green" - boundingObject USE LEDGSHAPE - physics Physics { - } - color [ - 0 1 0 - ] - gradual TRUE - } - LED { - translation -0.03 0.005 0.01 - rotation 0 1 0 0 - children [ - DEF LEDGSHAPE Shape { - appearance PBRAppearance { - baseColor 0 0 0 - metalness 0 - } - geometry Sphere { - radius 0.005 - } - } - ] - name "led_yellow" - boundingObject USE LEDGSHAPE - physics Physics { - } - color [ - 1 1 0 - ] - gradual TRUE - } - LED { - translation -0.03 -0.01 0.01 - rotation 0 1 0 0 - children [ - DEF LEDGSHAPE Shape { - appearance PBRAppearance { - baseColor 0 0 0 - metalness 0 - } - geometry Sphere { - radius 0.005 - } - } - ] - name "led_red" - boundingObject USE LEDGSHAPE - physics Physics { - } - gradual TRUE - } - Display { - name "robot_display" - height 32 - } - ] - } - ] - boundingObject USE BODY - physics Physics { - } - name IS name - controller "" - } +#VRML_SIM R2023a utf8 + +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/appearances/protos/Rubber.proto" + +PROTO Zumo32U4 [ + field SFVec3f translation 0 0 0 + field SFRotation rotation 0 0 1 0 + field SFString name "robot" +] +{ + Robot { + translation IS translation + rotation IS rotation + children [ + DEF SHIELD Group { + children [ + Transform { + translation 0.047 0 0.007 + rotation 0 1 0 -0.42 + children [ + Shape { + appearance PBRAppearance { + emissiveColor 0.4 0.4 0.4 + } + geometry Box { + size 0.001 0.1 0.04 + } + } + ] + } + ] + } + DEF TRACK_LEFT Track { + translation 0 0.0375 0 + rotation 0 1 0 0 + children [ + DEF WHEEL_FL TrackWheel { + position 0.0225 0.005 + rotation 0.6681343103444981 -0.5261152487176364 0.5261171812507756 1.9635600942618647 + radius 0.018 + children [ + DEF TRACK_WHEEL Transform { + children [ + Shape { + appearance DEF WHEEL_COLOR PBRAppearance { + baseColor 0 0 0 + metalness 0 + } + geometry Cylinder { + height 0.01 + radius 0.0175 + } + } + ] + } + ] + } + DEF WHEEL_RL TrackWheel { + position -0.0225 0.005 + rotation 0.6681343103444981 -0.5261152487176364 0.5261171812507756 1.9635600942618647 + radius 0.018 + children [ + USE TRACK_WHEEL + ] + } + ] + name "left_track" + boundingObject DEF TRACK_BO Group { + children [ + Transform { + translation 0 0 0.005 + children [ + Box { + size 0.01 0.01 0.038 + } + ] + } + Transform { + translation 0.0225 0 0.005 + rotation 1 0 0 1.5708 + children [ + DEF WHEEL_BO Cylinder { + height 0.01 + radius 0.019 + } + ] + } + Transform { + translation -0.0225 0 0.00499976 + rotation 1 0 0 1.5708 + children [ + USE WHEEL_BO + ] + } + ] + } + physics DEF TRACK_PH Physics { + } + device [ + LinearMotor { + name "motor_left" + maxVelocity 0.3 + } + PositionSensor { + name "position_sensor_left" + } + ] + animatedGeometry DEF ANIM_WHEEL Shape { + appearance Rubber { + } + geometry Box { + size 0.003 0.008 0.002 + } + } + geometriesCount 70 + } + DEF TRACK_RIGHT Track { + translation 0 -0.0375 0 + children [ + DEF WHEEL_FR TrackWheel { + position 0.0225 0.005 + rotation 0.6681343103444981 0.5261152487176364 -0.5261171812507756 1.9635600942618647 + radius 0.018 + children [ + DEF TRACK_WHEEL Transform { + children [ + Shape { + appearance DEF WHEEL_COLOR PBRAppearance { + baseColor 0 0 0 + metalness 0 + } + geometry Cylinder { + height 0.01 + radius 0.018 + } + } + ] + } + ] + } + DEF WHEEL_RR TrackWheel { + position -0.0225 0.005 + rotation 0.6681343103444981 0.5261152487176364 -0.5261171812507756 1.9635600942618647 + radius 0.018 + children [ + USE TRACK_WHEEL + ] + } + ] + name "track_right" + boundingObject USE TRACK_BO + physics USE TRACK_PH + device [ + LinearMotor { + name "motor_right" + maxVelocity 0.3 + } + PositionSensor { + name "position_sensor_right" + } + ] + animatedGeometry USE ANIM_WHEEL + geometriesCount 70 + } + DEF BODY Group { + children [ + Shape { + appearance DEF BODY_COLOR PBRAppearance { + emissiveColor 0.2 0.2 0.2 + } + geometry DEF BODY_GEOMETRY Box { + size 0.06 0.065 0.02 + } + } + Transform { + translation 0.0325 0 0.0025 + children [ + Shape { + appearance USE BODY_COLOR + geometry Box { + size 0.025 0.065 0.015 + } + } + ] + } + Transform { + translation -0.01 0 0 + children [ + Shape { + appearance USE BODY_COLOR + geometry USE BODY_GEOMETRY + } + ] + } + ] + } + DEF LINE_SENSOR_ARRAY Group { + children [ + Transform { + translation 0.0425 0 -0.0075 + children [ + Shape { + appearance USE BODY_COLOR + geometry Box { + size 0.005 0.01 0.005 + } + } + ] + } + Transform { + translation 0.049 0 -0.009 + children [ + Shape { + appearance USE BODY_COLOR + geometry Box { + size 0.008 0.095 0.002 + } + } + ] + } + DistanceSensor { + translation 0.05 0.001 -0.01 + rotation 0 1 0 1.5708 + children [ + Shape { + appearance DEF LSAPPEARANCE PBRAppearance { + baseColor 0 0 0 + metalness 0 + } + geometry DEF LIGHTSENSOR Box { + size 0.00025 0.002 0.004 + } + } + ] + name "lightsensor_m" + boundingObject USE LIGHTSENSOR + physics DEF LIGHTSENSOR_PH Physics { + } + lookupTable [ + 0.008 0 0 + 0.02 1000 0 + ] + type "infra-red" + numberOfRays 2 + aperture 0.4 + } + Emitter { + translation 0.05 -0.001 -0.01 + rotation 0 1 0 1.5708 + children [ + Shape { + appearance DEF EMAPPEARANCE PBRAppearance { + baseColor 1 0 0 + metalness 0 + } + geometry DEF EMITTER Box { + size 0.00025 0.002 0.004 + } + } + ] + name "emitter_m" + boundingObject USE EMITTER + physics DEF EMITTER_PH Physics { + } + type "infra-red" + aperture 0.35 + } + DistanceSensor { + translation 0.05 0.011 -0.01 + rotation 0 1 0 1.5708 + children [ + Shape { + appearance USE LSAPPEARANCE + geometry USE LIGHTSENSOR + } + ] + name "lightsensor_lm" + boundingObject USE LIGHTSENSOR + physics USE LIGHTSENSOR_PH + lookupTable [ + 0.008 0 0 + 0.02 1000 0 + ] + type "infra-red" + numberOfRays 2 + aperture 0.4 + } + Emitter { + translation 0.05 0.009 -0.01 + rotation 0 1 0 1.5708 + children [ + Shape { + appearance USE EMAPPEARANCE + geometry USE EMITTER + } + ] + name "emitter_lm" + boundingObject USE EMITTER + physics USE EMITTER_PH + type "infra-red" + aperture 0.35 + } + DistanceSensor { + translation 0.05 -0.009 -0.01 + rotation 0 1 0 1.5708 + children [ + Shape { + appearance USE LSAPPEARANCE + geometry USE LIGHTSENSOR + } + ] + name "lightsensor_rm" + boundingObject USE LIGHTSENSOR + physics USE LIGHTSENSOR_PH + lookupTable [ + 0.008 0 0 + 0.02 1000 0 + ] + type "infra-red" + numberOfRays 2 + aperture 0.4 + } + Emitter { + translation 0.05 -0.011 -0.01 + rotation 0 1 0 1.5708 + children [ + Shape { + appearance USE EMAPPEARANCE + geometry USE EMITTER + } + ] + name "emitter_rm" + boundingObject USE EMITTER + physics USE EMITTER_PH + type "infra-red" + aperture 0.35 + } + DistanceSensor { + translation 0.05 0.046 -0.01 + rotation 0 1 0 1.5708 + children [ + Shape { + appearance USE LSAPPEARANCE + geometry USE LIGHTSENSOR + } + ] + name "lightsensor_l" + boundingObject USE LIGHTSENSOR + physics USE LIGHTSENSOR_PH + lookupTable [ + 0.008 0 0 + 0.02 1000 0 + ] + type "infra-red" + numberOfRays 2 + aperture 0.4 + } + Emitter { + translation 0.05 0.044 -0.01 + rotation 0 1 0 1.5708 + children [ + Shape { + appearance USE EMAPPEARANCE + geometry USE EMITTER + } + ] + name "emitter_l" + boundingObject USE EMITTER + physics USE EMITTER_PH + type "infra-red" + aperture 0.35 + } + DistanceSensor { + translation 0.05 -0.044 -0.01 + rotation 0 1 0 1.5708 + children [ + Shape { + appearance USE LSAPPEARANCE + geometry USE LIGHTSENSOR + } + ] + name "lightsensor_r" + boundingObject USE LIGHTSENSOR + physics USE LIGHTSENSOR_PH + lookupTable [ + 0.008 0 0 + 0.02 1000 0 + ] + type "infra-red" + numberOfRays 2 + aperture 0.4 + } + Emitter { + translation 0.05 -0.046 -0.01 + rotation 0 1 0 1.5708 + children [ + Shape { + appearance USE EMAPPEARANCE + geometry USE EMITTER + } + ] + name "emitter_r" + boundingObject USE EMITTER + physics USE EMITTER_PH + type "infra-red" + aperture 0.35 + } + DistanceSensor { + translation 0.055 0.02 0.02 + name "proxim_sensor_fl" + lookupTable [ + 0 6 0 + 0.05 6 0 + 0.1 5 0 + 0.15 6 0 + 0.2 3 0 + 0.25 2 0 + 0.3 1 0 + 0.301 0 0 + ] + type "infra-red" + numberOfRays 4 + aperture 0.7 + } + DistanceSensor { + translation 0.055 -0.02 0.02 + name "proxim_sensor_fr" + lookupTable [ + 0 6 0 + 0.05 6 0 + 0.1 5 0 + 0.15 6 0 + 0.2 3 0 + 0.25 2 0 + 0.3 1 0 + 0.301 0 0 + ] + type "infra-red" + numberOfRays 4 + aperture 0.7 + } + ] + } + DEF IMU Group { + children [ + Accelerometer { + name "accelerometer" + xAxis TRUE + yAxis TRUE + zAxis TRUE + resolution 0.001 + lookupTable [ + -19.62 -32768 0 + 0 0 0 + 19.62 32767 0 + ] + } + Gyro { + name "gyro" + xAxis TRUE + yAxis TRUE + zAxis TRUE + resolution 0.001 + lookupTable [ + -4.276 -32768 0 + 0 0 0 + 4.276 32767 0 + ] + } + Compass { + name "magnetometer" + xAxis TRUE + yAxis TRUE + zAxis TRUE + resolution 0.001 + lookupTable [ + -1 -32000 0 + 0 0 0 + 1 32000 0 + ] + } + ] + } + + DEF LEDS Group { + children [ + LED { + translation -0.03 0.02 0.01 + rotation 0 1 0 0 + children [ + DEF LEDGSHAPE Shape { + appearance PBRAppearance { + baseColor 0 0 0 + metalness 0 + } + geometry Sphere { + radius 0.005 + } + } + ] + name "led_green" + boundingObject USE LEDGSHAPE + physics Physics { + } + color [ + 0 1 0 + ] + gradual TRUE + } + LED { + translation -0.03 0.005 0.01 + rotation 0 1 0 0 + children [ + DEF LEDGSHAPE Shape { + appearance PBRAppearance { + baseColor 0 0 0 + metalness 0 + } + geometry Sphere { + radius 0.005 + } + } + ] + name "led_yellow" + boundingObject USE LEDGSHAPE + physics Physics { + } + color [ + 1 1 0 + ] + gradual TRUE + } + LED { + translation -0.03 -0.01 0.01 + rotation 0 1 0 0 + children [ + DEF LEDGSHAPE Shape { + appearance PBRAppearance { + baseColor 0 0 0 + metalness 0 + } + geometry Sphere { + radius 0.005 + } + } + ] + name "led_red" + boundingObject USE LEDGSHAPE + physics Physics { + } + gradual TRUE + } + Display { + name "robot_display" + height 32 + } + ] + } + ] + boundingObject USE BODY + physics Physics { + } + name IS name + controller "" + } } \ No newline at end of file diff --git a/webots/worlds/HeadingCalculation.wbt b/webots/worlds/HeadingCalculation.wbt index a9a85516..ad9fbae6 100644 --- a/webots/worlds/HeadingCalculation.wbt +++ b/webots/worlds/HeadingCalculation.wbt @@ -1,38 +1,38 @@ -#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/HeadingCalculationTrack.proto" - -WorldInfo { - info [ - "Test track for heading calculation." - ] - title "Heading Calculation Track" - basicTimeStep 8 - contactProperties [ - ContactProperties { - material1 "track material" - coulombFriction [ - 500 - ] - softCFM 0.0001 - } - ] -} -Viewpoint { - orientation -0.5773502691896258 0.5773502691896258 0.5773502691896258 2.0944 - position 0.10671498723053877 -0.025801636297686177 4.366758398391875 -} -TexturedBackground { -} -TexturedBackgroundLight { -} -HeadingCalculationTrack { -} -Zumo32U4 { - translation 0 0 0.0139993 - rotation 6.60216e-09 2.69589e-07 -1 -1.5286253071795866 - name "zumo_0" -} +#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/HeadingCalculationTrack.proto" + +WorldInfo { + info [ + "Test track for heading calculation." + ] + title "Heading Calculation Track" + basicTimeStep 8 + contactProperties [ + ContactProperties { + material1 "track material" + coulombFriction [ + 500 + ] + softCFM 0.0001 + } + ] +} +Viewpoint { + orientation -0.5773502691896258 0.5773502691896258 0.5773502691896258 2.0944 + position 0.10671498723053877 -0.025801636297686177 4.366758398391875 +} +TexturedBackground { +} +TexturedBackgroundLight { +} +HeadingCalculationTrack { +} +Zumo32U4 { + translation 0 0 0.0139993 + rotation 6.60216e-09 2.69589e-07 -1 -1.5286253071795866 + name "zumo_0" +} diff --git a/webots/worlds/LineFollowerTrack.wbt b/webots/worlds/LineFollowerTrack.wbt index ded9d513..9271fa60 100644 --- a/webots/worlds/LineFollowerTrack.wbt +++ b/webots/worlds/LineFollowerTrack.wbt @@ -1,44 +1,44 @@ -#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 "track material" - coulombFriction [ - 500 - ] - softCFM 0.0001 - } - ] -} -Viewpoint { - orientation 0.3787583266277594 0.09714544028170935 -0.9203830145339561 2.677946076660944 - position 1.2569587324159737 0.7945022889765716 1.5111964909293643 -} -TexturedBackground { -} -TexturedBackgroundLight { -} -LineFollowerTrack { -} -DEF ROBOT Zumo32U4 { - translation -0.24713614078815466 -0.04863962992854465 0.013994298332013683 - rotation -1.0564747468923541e-06 8.746699709178704e-07 0.9999999999990595 1.5880805820884731 - name "Zumo" -} -Supervisor { - name "Supervisor" - controller "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/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 "track material" + coulombFriction [ + 500 + ] + softCFM 0.0001 + } + ] +} +Viewpoint { + orientation 0.3787583266277594 0.09714544028170935 -0.9203830145339561 2.677946076660944 + position 1.2569587324159737 0.7945022889765716 1.5111964909293643 +} +TexturedBackground { +} +TexturedBackgroundLight { +} +LineFollowerTrack { +} +DEF ROBOT Zumo32U4 { + translation -0.24713614078815466 -0.04863962992854465 0.013994298332013683 + rotation -1.0564747468923541e-06 8.746699709178704e-07 0.9999999999990595 1.5880805820884731 + name "Zumo" +} +Supervisor { + name "Supervisor" + controller "Supervisor" + supervisor TRUE +}