From 49a1056898478b5c89db1c9b17f28dc09d0135b6 Mon Sep 17 00:00:00 2001 From: greyes Date: Sat, 1 Jun 2024 20:32:57 +0200 Subject: [PATCH] Introduce end-of-line normalization --- .github/workflows/main.yml | 382 ++-- README.md | 366 ++-- create_list_file.bat | 22 +- create_symbols_file.bat | 22 +- .../LineFollowerTrackVF0.csv | 1478 +++++++------- doc/architecture/CONVOYLEADER.md | 140 +- doc/architecture/LINEFOLLOWER.md | 150 +- doc/architecture/README.md | 230 +-- doc/architecture/REMOTECONTROL.md | 230 +-- doc/architecture/SENSORFUSION.md | 170 +- .../uml/DevelopmentView/Layers.plantuml | 60 +- .../LogicalView/DifferentialDrive.plantuml | 58 +- doc/architecture/uml/LogicalView/HAL.plantuml | 362 ++-- .../LineFollower/Application.plantuml | 238 +-- .../LineFollower/DrivingState.plantuml | 256 +-- .../LineFollower/ErrorState.plantuml | 102 +- .../LineSensorsCalibrationState.plantuml | 180 +- .../MotorSpeedCalibrationState.plantuml | 168 +- .../LineFollower/ReadyState.plantuml | 124 +- .../LineFollower/ReleaseTrackState.plantuml | 150 +- .../LineFollower/StartupState.plantuml | 134 +- .../uml/LogicalView/Odometry.plantuml | 54 +- .../RemoteControl/Application.plantuml | 180 +- .../RemoteControl/ErrorState.plantuml | 110 +- .../LineSensorsCalibrationState.plantuml | 180 +- .../MotorSpeedCalibrationState.plantuml | 132 +- .../RemoteControl/RemoteCtrlState.plantuml | 184 +- .../RemoteControl/StartupState.plantuml | 86 +- .../uml/LogicalView/SensorFusion/HAL_IMU.puml | 236 +-- .../SensorFusion/HAL_SensorFusion.puml | 298 +-- .../uml/LogicalView/Service.plantuml | 162 +- .../uml/LogicalView/Speedometer.plantuml | 58 +- .../uml/PhysicalView/Deployment.plantuml | 110 +- .../ProcessView/DifferentialDrive.plantuml | 76 +- .../LineFollower/SystemStates.plantuml | 120 +- .../LineFollower/SystemStates2.plantuml | 86 +- .../RemoteControl/SystemStates.plantuml | 94 +- doc/architecture/uml/ViewModels.plantuml | 116 +- doc/configuration/README.md | 68 +- doc/configuration/uml/RemoteControlSim.puml | 76 +- doc/configuration/uml/configuration.plantuml | 222 +- doc/doxygen/mainpage.dox | 52 +- lib/APPCalib/App.cpp | 238 +-- lib/APPCalib/App.h | 218 +- lib/APPCalib/DrivingState.cpp | 354 ++-- lib/APPCalib/DrivingState.h | 300 +-- lib/APPCalib/ErrorState.cpp | 274 +-- lib/APPCalib/ErrorState.h | 268 +-- lib/APPCalib/MotorSpeedCalibrationState.cpp | 492 ++--- lib/APPCalib/MotorSpeedCalibrationState.h | 330 +-- lib/APPCalib/ReadyState.cpp | 392 ++-- lib/APPCalib/ReadyState.h | 300 +-- lib/APPCalib/StartupState.cpp | 384 ++-- lib/APPCalib/StartupState.h | 294 +-- lib/APPCalib/library.json | 38 +- lib/APPConvoyFollower/App.h | 412 ++-- lib/APPConvoyFollower/DrivingState.cpp | 218 +- lib/APPConvoyFollower/DrivingState.h | 260 +-- lib/APPConvoyFollower/ErrorState.cpp | 226 +-- lib/APPConvoyFollower/ErrorState.h | 268 +-- .../LineSensorsCalibrationState.cpp | 426 ++-- .../LineSensorsCalibrationState.h | 322 +-- lib/APPConvoyFollower/SerialMuxChannels.h | 422 ++-- lib/APPConvoyFollower/StartupState.cpp | 248 +-- lib/APPConvoyFollower/StartupState.h | 272 +-- lib/APPConvoyFollower/library.json | 58 +- lib/APPConvoyLeader/DrivingState.cpp | 754 +++---- lib/APPConvoyLeader/DrivingState.h | 448 ++-- lib/APPConvoyLeader/ErrorState.cpp | 240 +-- lib/APPConvoyLeader/ErrorState.h | 268 +-- .../LineSensorsCalibrationState.cpp | 426 ++-- .../LineSensorsCalibrationState.h | 322 +-- lib/APPConvoyLeader/ParameterSets.cpp | 300 +-- lib/APPConvoyLeader/ParameterSets.h | 290 +-- lib/APPConvoyLeader/StartupState.h | 262 +-- lib/APPConvoyLeader/library.json | 58 +- lib/APPLineFollower/App.h | 218 +- lib/APPLineFollower/DrivingState.cpp | 1796 ++++++++--------- lib/APPLineFollower/DrivingState.h | 692 +++---- lib/APPLineFollower/ErrorState.cpp | 274 +-- lib/APPLineFollower/ErrorState.h | 268 +-- .../LineSensorsCalibrationState.cpp | 426 ++-- .../LineSensorsCalibrationState.h | 322 +-- .../MotorSpeedCalibrationState.cpp | 490 ++--- .../MotorSpeedCalibrationState.h | 330 +-- lib/APPLineFollower/ParameterSets.cpp | 300 +-- lib/APPLineFollower/ParameterSets.h | 290 +-- lib/APPLineFollower/ReadyState.cpp | 336 +-- lib/APPLineFollower/ReadyState.h | 258 +-- lib/APPLineFollower/ReleaseTrackState.cpp | 258 +-- lib/APPLineFollower/ReleaseTrackState.h | 258 +-- lib/APPLineFollower/StartupState.h | 296 +-- lib/APPLineFollower/library.json | 38 +- lib/APPRemoteControl/App.h | 412 ++-- lib/APPRemoteControl/DrivingState.cpp | 218 +- lib/APPRemoteControl/DrivingState.h | 260 +-- lib/APPRemoteControl/ErrorState.cpp | 226 +-- lib/APPRemoteControl/ErrorState.h | 268 +-- .../LineSensorsCalibrationState.cpp | 426 ++-- .../LineSensorsCalibrationState.h | 322 +-- lib/APPRemoteControl/SerialMuxChannels.h | 422 ++-- lib/APPRemoteControl/StartupState.cpp | 248 +-- lib/APPRemoteControl/StartupState.h | 272 +-- lib/APPRemoteControl/library.json | 56 +- lib/APPSensorFusion/App.cpp | 354 ++-- lib/APPSensorFusion/App.h | 276 +-- lib/APPSensorFusion/DrivingState.cpp | 752 +++---- lib/APPSensorFusion/DrivingState.h | 440 ++-- lib/APPSensorFusion/ErrorState.cpp | 226 +-- lib/APPSensorFusion/ErrorState.h | 268 +-- lib/APPSensorFusion/IBoard.h | 318 +-- .../LineSensorsCalibrationState.cpp | 414 ++-- .../LineSensorsCalibrationState.h | 322 +-- lib/APPSensorFusion/ParameterSets.cpp | 278 +-- lib/APPSensorFusion/ParameterSets.h | 290 +-- lib/APPSensorFusion/ReadyState.cpp | 310 +-- lib/APPSensorFusion/ReadyState.h | 258 +-- lib/APPSensorFusion/ReleaseTrackState.cpp | 242 +-- lib/APPSensorFusion/ReleaseTrackState.h | 258 +-- lib/APPSensorFusion/SerialMuxChannels.h | 182 +- lib/APPSensorFusion/StartupState.cpp | 234 +-- lib/APPSensorFusion/StartupState.h | 236 +-- lib/APPSensorFusion/library.json | 58 +- lib/APPTest/library.json | 46 +- lib/ArduinoNative/Arduino.cpp | 828 ++++---- lib/ArduinoNative/Print.h | 336 +-- lib/ArduinoNative/Serial.cpp | 342 ++-- lib/ArduinoNative/Serial.h | 484 ++--- lib/ArduinoNative/Stream.h | 166 +- lib/ArduinoNative/Terminal.cpp | 344 ++-- lib/ArduinoNative/Terminal.h | 376 ++-- lib/ArduinoNative/library.json | 28 +- lib/HALCalibSim/Board.cpp | 266 +-- lib/HALCalibSim/Board.h | 738 +++---- lib/HALCalibSim/library.json | 40 +- lib/HALCalibTarget/Board.cpp | 206 +- lib/HALCalibTarget/Board.h | 606 +++--- lib/HALCalibTarget/library.json | 44 +- lib/HALConvoyFollowerSim/Board.cpp | 266 +-- lib/HALConvoyFollowerSim/library.json | 40 +- lib/HALConvoyFollowerTarget/Board.cpp | 202 +- lib/HALConvoyFollowerTarget/library.json | 44 +- lib/HALConvoyLeaderSim/Board.cpp | 268 +-- lib/HALConvoyLeaderSim/library.json | 40 +- lib/HALConvoyLeaderTarget/Board.cpp | 202 +- lib/HALConvoyLeaderTarget/library.json | 44 +- lib/HALInterfaces/IButton.h | 190 +- lib/HALInterfaces/IDisplay.h | 316 +-- lib/HALInterfaces/IEncoders.h | 266 +-- lib/HALInterfaces/IIMU.h | 356 ++-- lib/HALInterfaces/ILed.h | 178 +- lib/HALInterfaces/ILineSensors.h | 340 ++-- lib/HALInterfaces/IMotors.h | 240 +-- lib/HALInterfaces/IProximitySensors.h | 244 +-- lib/HALInterfaces/ISender.h | 210 +- lib/HALInterfaces/ISettings.h | 206 +- lib/HALInterfaces/library.json | 28 +- lib/HALInterfacesTest/IEncodersTest.h | 192 +- lib/HALInterfacesTest/IMotorsTest.h | 192 +- lib/HALInterfacesTest/library.json | 28 +- lib/HALLineFollowerSim/Board.cpp | 260 +-- lib/HALLineFollowerSim/library.json | 40 +- lib/HALLineFollowerTarget/Board.cpp | 198 +- lib/HALLineFollowerTarget/library.json | 44 +- lib/HALRemoteControlSim/Board.cpp | 266 +-- lib/HALRemoteControlSim/library.json | 40 +- lib/HALRemoteControlTarget/Board.cpp | 202 +- lib/HALRemoteControlTarget/library.json | 44 +- lib/HALSensorFusionSim/Board.cpp | 268 +-- lib/HALSensorFusionSim/Board.h | 574 +++--- lib/HALSensorFusionSim/library.json | 40 +- lib/HALSensorFusionTarget/Board.cpp | 202 +- lib/HALSensorFusionTarget/Board.h | 436 ++-- lib/HALSensorFusionTarget/library.json | 44 +- lib/HALSim/ButtonA.cpp | 170 +- lib/HALSim/ButtonA.h | 204 +- lib/HALSim/ButtonB.cpp | 170 +- lib/HALSim/ButtonB.h | 204 +- lib/HALSim/ButtonC.cpp | 170 +- lib/HALSim/ButtonC.h | 204 +- lib/HALSim/Display.cpp | 150 +- lib/HALSim/Display.h | 608 +++--- lib/HALSim/Encoders.cpp | 384 ++-- lib/HALSim/Encoders.h | 342 ++-- lib/HALSim/IMU.cpp | 356 ++-- lib/HALSim/IMU.h | 428 ++-- lib/HALSim/Keyboard.cpp | 248 +-- lib/HALSim/Keyboard.h | 574 +++--- lib/HALSim/LedGreen.cpp | 180 +- lib/HALSim/LedGreen.h | 210 +- lib/HALSim/LedRed.cpp | 180 +- lib/HALSim/LedRed.h | 210 +- lib/HALSim/LedYellow.cpp | 180 +- lib/HALSim/LedYellow.h | 210 +- lib/HALSim/LineSensors.cpp | 440 ++-- lib/HALSim/LineSensors.h | 450 ++--- lib/HALSim/Motors.cpp | 282 +-- lib/HALSim/Motors.h | 320 +-- lib/HALSim/ProximitySensors.cpp | 166 +- lib/HALSim/ProximitySensors.h | 376 ++-- lib/HALSim/RobotConstants.h | 204 +- lib/HALSim/RobotDeviceNames.h | 292 +-- lib/HALSim/Sender.cpp | 200 +- lib/HALSim/Sender.h | 230 +-- lib/HALSim/Settings.cpp | 412 ++-- lib/HALSim/Settings.h | 270 +-- lib/HALSim/SimTime.cpp | 150 +- lib/HALSim/SimTime.h | 258 +-- lib/HALSim/WebotsSerialDrv.cpp | 618 +++--- lib/HALSim/WebotsSerialDrv.h | 462 ++--- lib/HALSim/library.json | 36 +- lib/HALTarget/ButtonA.cpp | 150 +- lib/HALTarget/ButtonA.h | 204 +- lib/HALTarget/ButtonB.cpp | 150 +- lib/HALTarget/ButtonB.h | 204 +- lib/HALTarget/ButtonC.cpp | 150 +- lib/HALTarget/ButtonC.h | 204 +- lib/HALTarget/Buzzer.cpp | 182 +- lib/HALTarget/Display.cpp | 150 +- lib/HALTarget/Display.h | 374 ++-- lib/HALTarget/Encoders.cpp | 150 +- lib/HALTarget/Encoders.h | 302 +-- lib/HALTarget/IMU.cpp | 454 ++--- lib/HALTarget/IMU.h | 400 ++-- lib/HALTarget/LedGreen.cpp | 150 +- lib/HALTarget/LedGreen.h | 186 +- lib/HALTarget/LedRed.cpp | 150 +- lib/HALTarget/LedRed.h | 186 +- lib/HALTarget/LedYellow.cpp | 150 +- lib/HALTarget/LedYellow.h | 186 +- lib/HALTarget/LineSensors.cpp | 236 +-- lib/HALTarget/LineSensors.h | 452 ++--- lib/HALTarget/Motors.cpp | 150 +- lib/HALTarget/Motors.h | 294 +-- lib/HALTarget/ProximitySensors.cpp | 150 +- lib/HALTarget/ProximitySensors.h | 286 +-- lib/HALTarget/RobotConstants.h | 204 +- lib/HALTarget/Sender.cpp | 150 +- lib/HALTarget/Sender.h | 224 +- lib/HALTarget/Settings.cpp | 340 ++-- lib/HALTarget/Settings.h | 264 +-- lib/HALTarget/library.json | 40 +- lib/HALTest/ButtonA.cpp | 150 +- lib/HALTest/ButtonA.h | 200 +- lib/HALTest/ButtonB.cpp | 150 +- lib/HALTest/ButtonB.h | 200 +- lib/HALTest/ButtonC.cpp | 150 +- lib/HALTest/ButtonC.h | 200 +- lib/HALTest/Display.cpp | 150 +- lib/HALTest/Display.h | 366 ++-- lib/HALTest/Encoders.cpp | 150 +- lib/HALTest/Encoders.h | 366 ++-- lib/HALTest/IMU.cpp | 150 +- lib/HALTest/IMU.h | 384 ++-- lib/HALTest/LedGreen.cpp | 150 +- lib/HALTest/LedGreen.h | 182 +- lib/HALTest/LedRed.cpp | 150 +- lib/HALTest/LedRed.h | 182 +- lib/HALTest/LedYellow.cpp | 150 +- lib/HALTest/LedYellow.h | 182 +- lib/HALTest/LineSensors.cpp | 160 +- lib/HALTest/LineSensors.h | 380 ++-- lib/HALTest/Motors.cpp | 150 +- lib/HALTest/Motors.h | 332 +-- lib/HALTest/ProximitySensors.cpp | 150 +- lib/HALTest/ProximitySensors.h | 278 +-- lib/HALTest/RobotConstants.h | 204 +- lib/HALTest/Settings.cpp | 178 +- lib/HALTest/Settings.h | 222 +- lib/HALTest/library.json | 36 +- lib/HALTestSim/library.json | 36 +- lib/Service/DifferentialDrive.cpp | 572 +++--- lib/Service/DifferentialDrive.h | 550 ++--- lib/Service/FPMath.cpp | 150 +- lib/Service/FPMath.h | 156 +- lib/Service/IState.h | 204 +- lib/Service/Logging.cpp | 326 +-- lib/Service/Logging.h | 696 +++---- lib/Service/MovAvg.hpp | 328 +-- lib/Service/Odometry.cpp | 548 ++--- lib/Service/Odometry.h | 606 +++--- lib/Service/PIDController.cpp | 150 +- lib/Service/PIDController.h | 1342 ++++++------ lib/Service/RelativeEncoders.cpp | 202 +- lib/Service/RelativeEncoders.h | 284 +-- lib/Service/SimpleTimer.cpp | 256 +-- lib/Service/SimpleTimer.h | 314 +-- lib/Service/Speedometer.cpp | 460 ++--- lib/Service/Speedometer.h | 390 ++-- lib/Service/StateMachine.cpp | 200 +- lib/Service/StateMachine.h | 228 +-- lib/Service/Util.cpp | 400 ++-- lib/Service/Util.h | 216 +- lib/Service/library.json | 32 +- platformio.ini | 1368 ++++++------- scripts/copy_sounds.py | 142 +- scripts/copy_webots_shared_libs.py | 182 +- scripts/create_webots_library.py | 238 +-- scripts/package.py | 230 +-- scripts/webots_launcher.py | 228 +-- src/main.cpp | 160 +- start_platoon.bat | 200 +- test/test_Odometry/test_Odometry.cpp | 652 +++--- .../test_PIDController/test_PIDController.cpp | 366 ++-- .../test_RelativeEncoders.cpp | 362 ++-- test/test_SimpleTimer/test_SimpleTimer.cpp | 336 +-- test/test_Util/test_Util.cpp | 380 ++-- webots/controllers/Obstacle/Obstacle.py | 106 +- .../PlatoonSupervisor/PlatoonSupervisor.py | 170 +- webots/worlds/HeadingCalculation.wbt | 80 +- .../SensorFusionTrack.wbt | 104 +- 311 files changed, 41236 insertions(+), 41236 deletions(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 1b284219..42cd29ed 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -1,192 +1,192 @@ -name: main - -# Controls when the action will run. -on: - push: - branches: [ '**' ] - pull_request: - branches: [ main ] - release: - # A release, pre-release, or draft of a release is published. - types: [ published ] - # Allows you to run this workflow manually from the Actions tab. - workflow_dispatch: - -# A workflow run is made up of one or more jobs that can run sequentially or in parallel. -jobs: - # The introduction just shows some useful informations. - intro: - # The type of runner that the job will run on. - runs-on: ubuntu-latest - # Steps represent a sequence of tasks that will be executed as part of the job. - steps: - - run: echo "The job was automatically triggered by a ${{ github.event_name }} event." - - run: echo "The name of the branch is ${{ github.ref }} and the repository is ${{ github.repository }}." - - # Build all targets - build: - # The type of runner that the job will run on. - runs-on: ubuntu-latest - needs: intro - strategy: - matrix: - environment: ["CalibTarget", "ConvoyFollowerTarget", "ConvoyLeaderTarget", "LineFollowerTarget", "RemoteControlTarget", "SensorFusionTarget"] - - # Steps represent a sequence of tasks that will be executed as part of the job. - steps: - - name: Checkout repository - uses: actions/checkout@v4 - - - name: Cache pip - uses: actions/cache@v4 - with: - path: ~/.cache/pip - key: ${{ runner.os }}-pip-${{ hashFiles('**/requirements.txt') }} - restore-keys: | - ${{ runner.os }}-pip- - - - name: Cache PlatformIO - uses: actions/cache@v4 - with: - path: ~/.platformio - key: ${{ runner.os }}-${{ hashFiles('**/lockfiles') }} - - - name: Set up Python - uses: actions/setup-python@v5 - with: - python-version: '3.9' - - - name: Install PlatformIO - run: | - python -m pip install --upgrade pip - pip install --upgrade platformio - - - name: Compile ${{ matrix.environment }} firmware - run: platformio run --environment ${{ matrix.environment }} - - # Perform static checks and test - check: - # The type of runner that the job will run on. - runs-on: ubuntu-latest - needs: intro - strategy: - matrix: - environment: ["CalibTarget", "ConvoyFollowerTarget", "ConvoyLeaderTarget", "LineFollowerTarget", "RemoteControlTarget", "SensorFusionTarget", "TestSim"] - - steps: - - name: Checkout repository - uses: actions/checkout@v4 - - - name: Cache pip - uses: actions/cache@v4 - with: - path: ~/.cache/pip - key: ${{ runner.os }}-pip-${{ hashFiles('**/requirements.txt') }} - restore-keys: | - ${{ runner.os }}-pip- - - - name: Cache PlatformIO - uses: actions/cache@v4 - with: - path: ~/.platformio - key: ${{ runner.os }}-${{ hashFiles('**/lockfiles') }} - - - name: Set up Python - uses: actions/setup-python@v5 - with: - python-version: '3.9' - - - name: Install PlatformIO - run: | - python -m pip install --upgrade pip - pip install --upgrade platformio - - - name: Perform static checks on ${{ matrix.environment }} - run: platformio check --environment ${{ matrix.environment }} --fail-on-defect=medium --fail-on-defect=high - - # Perform tests - test: - # The type of runner that the job will run on. - runs-on: ubuntu-latest - needs: intro - - steps: - - name: Checkout repository - uses: actions/checkout@v4 - - - name: Cache pip - uses: actions/cache@v4 - with: - path: ~/.cache/pip - key: ${{ runner.os }}-pip-${{ hashFiles('**/requirements.txt') }} - restore-keys: | - ${{ runner.os }}-pip- - - - name: Cache PlatformIO - uses: actions/cache@v4 - with: - path: ~/.platformio - key: ${{ runner.os }}-${{ hashFiles('**/lockfiles') }} - - - name: Set up Python - uses: actions/setup-python@v5 - with: - python-version: '3.9' - - - name: Install PlatformIO - run: | - python -m pip install --upgrade pip - pip install --upgrade platformio - - - name: Run tests on native environment - run: platformio test --environment TestSim -vvv - - # Build documentation - doc: - # The type of runner that the job will run on. - runs-on: ubuntu-latest - needs: intro - strategy: - matrix: - environment: ["CalibTarget", "ConvoyFollowerTarget", "ConvoyLeaderTarget", "LineFollowerTarget", "RemoteControlTarget", "SensorFusionTarget", "CalibSim", "ConvoyFollowerSim", "ConvoyLeaderSim", "LineFollowerSim", "RemoteControlSim", "SensorFusionSim"] - - steps: - - name: Checkout repository - uses: actions/checkout@v4 - - - name: Cache pip - uses: actions/cache@v4 - with: - path: ~/.cache/pip - key: ${{ runner.os }}-pip-${{ hashFiles('**/requirements.txt') }} - restore-keys: | - ${{ runner.os }}-pip- - - - name: Cache PlatformIO - uses: actions/cache@v4 - with: - path: ~/.platformio - key: ${{ runner.os }}-${{ hashFiles('**/lockfiles') }} - - - name: Set up Python - uses: actions/setup-python@v5 - with: - python-version: '3.9' - - - name: Install PlatformIO - run: | - python -m pip install --upgrade pip - pip install --upgrade platformio - - - name: Set up graphviz - uses: ts-graphviz/setup-graphviz@v2 - - - name: Set up doxygen and generate documentation for ${{ matrix.environment }} - uses: mattnotmitt/doxygen-action@v1.9.5 - with: - working-directory: './doc/doxygen' - doxyfile-path: './${{ matrix.environment }}Doxyfile' - - - name: Print doxygen warnings - if: ${{ failure() }} +name: main + +# Controls when the action will run. +on: + push: + branches: [ '**' ] + pull_request: + branches: [ main ] + release: + # A release, pre-release, or draft of a release is published. + types: [ published ] + # Allows you to run this workflow manually from the Actions tab. + workflow_dispatch: + +# A workflow run is made up of one or more jobs that can run sequentially or in parallel. +jobs: + # The introduction just shows some useful informations. + intro: + # The type of runner that the job will run on. + runs-on: ubuntu-latest + # Steps represent a sequence of tasks that will be executed as part of the job. + steps: + - run: echo "The job was automatically triggered by a ${{ github.event_name }} event." + - run: echo "The name of the branch is ${{ github.ref }} and the repository is ${{ github.repository }}." + + # Build all targets + build: + # The type of runner that the job will run on. + runs-on: ubuntu-latest + needs: intro + strategy: + matrix: + environment: ["CalibTarget", "ConvoyFollowerTarget", "ConvoyLeaderTarget", "LineFollowerTarget", "RemoteControlTarget", "SensorFusionTarget"] + + # Steps represent a sequence of tasks that will be executed as part of the job. + steps: + - name: Checkout repository + uses: actions/checkout@v4 + + - name: Cache pip + uses: actions/cache@v4 + with: + path: ~/.cache/pip + key: ${{ runner.os }}-pip-${{ hashFiles('**/requirements.txt') }} + restore-keys: | + ${{ runner.os }}-pip- + + - name: Cache PlatformIO + uses: actions/cache@v4 + with: + path: ~/.platformio + key: ${{ runner.os }}-${{ hashFiles('**/lockfiles') }} + + - name: Set up Python + uses: actions/setup-python@v5 + with: + python-version: '3.9' + + - name: Install PlatformIO + run: | + python -m pip install --upgrade pip + pip install --upgrade platformio + + - name: Compile ${{ matrix.environment }} firmware + run: platformio run --environment ${{ matrix.environment }} + + # Perform static checks and test + check: + # The type of runner that the job will run on. + runs-on: ubuntu-latest + needs: intro + strategy: + matrix: + environment: ["CalibTarget", "ConvoyFollowerTarget", "ConvoyLeaderTarget", "LineFollowerTarget", "RemoteControlTarget", "SensorFusionTarget", "TestSim"] + + steps: + - name: Checkout repository + uses: actions/checkout@v4 + + - name: Cache pip + uses: actions/cache@v4 + with: + path: ~/.cache/pip + key: ${{ runner.os }}-pip-${{ hashFiles('**/requirements.txt') }} + restore-keys: | + ${{ runner.os }}-pip- + + - name: Cache PlatformIO + uses: actions/cache@v4 + with: + path: ~/.platformio + key: ${{ runner.os }}-${{ hashFiles('**/lockfiles') }} + + - name: Set up Python + uses: actions/setup-python@v5 + with: + python-version: '3.9' + + - name: Install PlatformIO + run: | + python -m pip install --upgrade pip + pip install --upgrade platformio + + - name: Perform static checks on ${{ matrix.environment }} + run: platformio check --environment ${{ matrix.environment }} --fail-on-defect=medium --fail-on-defect=high + + # Perform tests + test: + # The type of runner that the job will run on. + runs-on: ubuntu-latest + needs: intro + + steps: + - name: Checkout repository + uses: actions/checkout@v4 + + - name: Cache pip + uses: actions/cache@v4 + with: + path: ~/.cache/pip + key: ${{ runner.os }}-pip-${{ hashFiles('**/requirements.txt') }} + restore-keys: | + ${{ runner.os }}-pip- + + - name: Cache PlatformIO + uses: actions/cache@v4 + with: + path: ~/.platformio + key: ${{ runner.os }}-${{ hashFiles('**/lockfiles') }} + + - name: Set up Python + uses: actions/setup-python@v5 + with: + python-version: '3.9' + + - name: Install PlatformIO + run: | + python -m pip install --upgrade pip + pip install --upgrade platformio + + - name: Run tests on native environment + run: platformio test --environment TestSim -vvv + + # Build documentation + doc: + # The type of runner that the job will run on. + runs-on: ubuntu-latest + needs: intro + strategy: + matrix: + environment: ["CalibTarget", "ConvoyFollowerTarget", "ConvoyLeaderTarget", "LineFollowerTarget", "RemoteControlTarget", "SensorFusionTarget", "CalibSim", "ConvoyFollowerSim", "ConvoyLeaderSim", "LineFollowerSim", "RemoteControlSim", "SensorFusionSim"] + + steps: + - name: Checkout repository + uses: actions/checkout@v4 + + - name: Cache pip + uses: actions/cache@v4 + with: + path: ~/.cache/pip + key: ${{ runner.os }}-pip-${{ hashFiles('**/requirements.txt') }} + restore-keys: | + ${{ runner.os }}-pip- + + - name: Cache PlatformIO + uses: actions/cache@v4 + with: + path: ~/.platformio + key: ${{ runner.os }}-${{ hashFiles('**/lockfiles') }} + + - name: Set up Python + uses: actions/setup-python@v5 + with: + python-version: '3.9' + + - name: Install PlatformIO + run: | + python -m pip install --upgrade pip + pip install --upgrade platformio + + - name: Set up graphviz + uses: ts-graphviz/setup-graphviz@v2 + + - name: Set up doxygen and generate documentation for ${{ matrix.environment }} + uses: mattnotmitt/doxygen-action@v1.9.5 + with: + working-directory: './doc/doxygen' + doxyfile-path: './${{ matrix.environment }}Doxyfile' + + - name: Print doxygen warnings + if: ${{ failure() }} run: cat ./doc/doxygen/doxygen_warnings.txt \ No newline at end of file diff --git a/README.md b/README.md index 5dd7dfed..6f71b86c 100644 --- a/README.md +++ b/README.md @@ -1,183 +1,183 @@ -# Radon Ulzer - Firmware for Zumo32U4 - -[![License](https://img.shields.io/badge/license-MIT-blue.svg)](http://choosealicense.com/licenses/mit/) -[![Repo Status](https://www.repostatus.org/badges/latest/wip.svg)](https://www.repostatus.org/#wip) -[![Release](https://img.shields.io/github/release/BlueAndi/RadonUlzer.svg)](https://github.com/BlueAndi/RadonUlzer/releases) -[![Build Status](https://github.com/BlueAndi/RadonUlzer/actions/workflows/main.yml/badge.svg?branch=main)](https://github.com/BlueAndi/RadonUlzer/actions/workflows/main.yml) - -A robot as fast as the famous pod racer driven by Anakin Skywalker with the powerful engines from Radon Ulzer. :-) - -Several kind of exclusive applications are available: -* Calib - Application used for motor speed calibration. -* Convoy Leader - A line follower, providing information to the [DroidControlShip](https://github.com/BlueAndi/DroidControlShip) in a convoy leader role. -* Line Follower - Just a line follower, using a PID controller. -* Remote Control - The robot is remote controlled by e.g. the [DroidControlShip](https://github.com/BlueAndi/DroidControlShip) in a convoy follower role. -* Sensor Fusion - The robot provides odometry and inertial data to the [DroidControlShip](https://github.com/BlueAndi/DroidControlShip), which calculates the sensor fusion based location information. - -## Table of content - -* [The robot](#the-robot) -* [The simulation](#the-simulation) - * [Installation](#installation) - * [The Webots library](#the-webots-library) - * [Build](#build) - * [Preparation](#preparation) - * [Run](#run) - * [Run without Webots launcher](#run-without-webots-launcher) - * [Run with Webots launcher](#run-with-webots-launcher) - * [Run via terminal](#run-via-terminal) - * [Running the robot on track](#running-the-robot-on-track) - * [Communicate with the DroidControlShip](#communicate-with-the-droidcontrolship) -* [The target](#the-target) - * [Build and flash procedure](#build-and-flash-procedure) -* [The Applications](#the-applications) -* [Documentation](#documentation) -* [Used Libraries](#used-libraries) -* [Issues, Ideas And Bugs](#issues-ideas-and-bugs) -* [License](#license) -* [Contribution](#contribution) - -# The robot -The main target of the firmware is the Pololu Zumo32U4 robot (see https://www.pololu.com/category/129/zumo-robots-and-accessories) from Pololu. - -![deployment](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/master/doc/architecture/uml/PhysicalView/Deployment.plantuml) - -# The simulation -The simulation is based on the open source robot simulator *Webots*. The application and the services are equal to the target firmware. Only the HAL is different in the simulation. - -* Website: https://cyberbotics.com/#cyberbotics -* Github: https://github.com/cyberbotics/webots -* Compatible webots versions: - * v2023a - * v2023b - -## Installation - -1. Install [Webots](https://cyberbotics.com). -2. Setup for [external controllers](https://www.cyberbotics.com/doc/guide/running-extern-robot-controllers): - 1. Set environment variable ```WEBOTS_HOME``` to installation directory of Webots. - 2. Add to path: - * Linux: ```${WEBOTS_HOME}/lib/controller``` - * Windows: ```%WEBOTS_HOME%\lib\controller``` -3. Install the native compiler toolchain: - * Linux: Install the gcc toolchain, depended on your distribution. - * Windows - * Install the [MSYS2](https://www.msys2.org) toolchain. - * Open MSYS2 shell. - * Update package database: ```pacman -Sy pacman``` - * Install GCC: ```pacman -Sy mingw-w64-ucrt-x86_64-gcc``` -4. Ensure a current (>=3.9) Python3 version is installed on your machine. - -## The Webots library -To adapt the HAL to the simulation, some sourcecode files from Webots are necessary. Currently there is no Webots library in the platformio registry available. Therefore a local library is created during the build. Ensure that that Webots is already installed, before you try to build it! - -The library creation is handled in the ```./scripts/create_webots_library.py``` script and runs automatically after building for the WebotsSim environment. - -## Build -1. Start VSCode. -2. PlatformIO project tasks --> <APP-NAME> --> General --> Build\ - Example for **LineFollowerSim** application:\ - ![Example](./doc/images/pio_build.jpg) - -For the simulation use only the applications with "Sim" as postfix, e.g. LineFollowerSim. - -## Preparation -The preparation is shown with the line follower application as example. It expects that the **LineFollowerSim** is already built. - -1. Start the Webots simulation. -2. File --> Open World -3. Select ```./webots/worlds/LineFollowerTrack.wbt```. -4. The loaded world should now look like this: ![webots_world](./doc/images/webots_world.jpg) -5. The simulation waits now for the external controller, like the RadonUlzer. - -## Run -There are 3 ways how to run now the application. Choose according to your needs. - -### Run without Webots launcher -This can be choosen in case the simulation waits just for one robot. - -PlatformIO project tasks --> <APP-NAME> --> General --> Upload - -Example for **LineFollowerSim** application: - -![Example](./doc/images/pio_upload.jpg) - -### Run with Webots launcher -Choose this one in case the simulation waits for more than one robot. Adapt the robot name in the _platformio.ini_. - -See (Single Simulation and Multiple Local Extern Robot Controllers)[https://cyberbotics.com/doc/guide/running-extern-robot-controllers?tab-os=windows#single-simulation-and-multiple-local-extern-robot-controllers] for details. - -PlatformIO project tasks --> <APP-NAME> --> Custom --> WebotsLauncher - -Example for **LineFollowerSim** application: - -![Example](./doc/images/pio_webots_launcher.jpg) - -### Run via terminal -1. Open a command line (shell) and change to the folder with the built executable in ```.pio/build/LineFollowerSim```. This folder contains all necessary shared libraries as well. -2. Start the executable. - -## Running the robot on track - -1. Click in the simulation on the display to focus the simulation. -2. Now the keyboard keys a, b and c can be used to control the robot according to the implemented application logic. - -## Communicate with the DroidControlShip -The communication with the DroidControlShip goes via a Webots serial connection, which is disabled by default. - -Use the -c flag to enable it with default channels (see _webots_robot_serial_rx_channel_ and _webots_robot_serial_tx_channel_ in [platformio.ini](./platformio.ini)). Note, this will disable the standard logging, because the serial communication uses the SerialMuxProt procotol for data interchange. -```bash -$ program.exe -c -``` - -For simplicity a Platformio project task was added, which enables the Webots serial connection as well. - -![Example](./doc/images/pio_webots_launcher_zumo_com_system.jpg) - -# The target - -## Build and flash procedure -1. PlatformIO project tasks --> <APP-NAME> --> General --> Build - * For the target use only the applications with "Target" as postfix, e.g. LineFollowerTarget. -2. Start the bootloader by triggering twice the reset button. The yellow led will start blinking for 10s. Note, after 10s the target will leave the bootloader! -3. PlatformIO project tasks --> <APP-NAME> --> General --> Upload -4. Ready. - -Example for the **LineFollowerTarget** application: - -![Example](./doc/images/pio_target_build_upload.jpg) - -# The Applications - -| Application | Description | Standalone | DroidControlShop Required | Webots World | -| - | - | - | - | - | -| Calib | Application used for motor speed calibration. | Yes | No | ./webots/worlds/LargeTrack.wbt ./webots/worlds/LineFollowerTrack.wbt | -| ConvoyLeader | A line follower, providing information to the [DroidControlShip](https://github.com/BlueAndi/DroidControlShip) in a convoy leader role. | No | Yes | ./webots/worlds/zumo_with_com_system/PlatoonTrack.wbt | -| ConvoyFollower | Convoy follower, providing information to the [DroidControlShip](https://github.com/BlueAndi/DroidControlShip) to drive to its target. | No | Yes | ./webots/worlds/zumo_with_com_system/PlatoonTrack.wbt | -| LineFollower | Just a line follower, using a PID controller. | Yes | No | ./webots/worlds/ETrack.wbt ./webots/worlds/LargeTrack.wbt ./webots/worlds/LineFollowerTrack.wbt | -| RemoteControl | The robot is remote controlled by e.g. the [DroidControlShip](https://github.com/BlueAndi/DroidControlShip) in a convoy follower role. | No | Yes | ./webots/world/zumo_with_com_system/* | -| SensorFusion | The robot provides odometry and inertial data to the [DroidControlShip](https://github.com/BlueAndi/DroidControlShip), which calculates the sensor fusion based location information. | No | Yes | ./webots/worlds/zumo_with_com_system/SensorFusionTrack.wbt | -| Test | Only for testing purposes on native environment. | Yes | No | N/A | - -# Documentation - -* [SW Architecture](./doc/architecture/README.md) -* [SW Configuration](./doc/configuration/README.md) - -# Used Libraries - -| Library | Description | License | -| ----------------------------------------------------------------------- | ----------------------------------------- | ------- | -| [Zumo32U4 library](https://github.com/pololu/zumo-32u4-arduino-library) | Provides access to the Zumo32U4 hardware. | MIT | -| [SerialMuxProt](https://github.com/gabryelreyes/SerialMuxProt) | Multiplexing Communication Protocol | MIT | - -# 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 - Firmware for Zumo32U4 + +[![License](https://img.shields.io/badge/license-MIT-blue.svg)](http://choosealicense.com/licenses/mit/) +[![Repo Status](https://www.repostatus.org/badges/latest/wip.svg)](https://www.repostatus.org/#wip) +[![Release](https://img.shields.io/github/release/BlueAndi/RadonUlzer.svg)](https://github.com/BlueAndi/RadonUlzer/releases) +[![Build Status](https://github.com/BlueAndi/RadonUlzer/actions/workflows/main.yml/badge.svg?branch=main)](https://github.com/BlueAndi/RadonUlzer/actions/workflows/main.yml) + +A robot as fast as the famous pod racer driven by Anakin Skywalker with the powerful engines from Radon Ulzer. :-) + +Several kind of exclusive applications are available: +* Calib - Application used for motor speed calibration. +* Convoy Leader - A line follower, providing information to the [DroidControlShip](https://github.com/BlueAndi/DroidControlShip) in a convoy leader role. +* Line Follower - Just a line follower, using a PID controller. +* Remote Control - The robot is remote controlled by e.g. the [DroidControlShip](https://github.com/BlueAndi/DroidControlShip) in a convoy follower role. +* Sensor Fusion - The robot provides odometry and inertial data to the [DroidControlShip](https://github.com/BlueAndi/DroidControlShip), which calculates the sensor fusion based location information. + +## Table of content + +* [The robot](#the-robot) +* [The simulation](#the-simulation) + * [Installation](#installation) + * [The Webots library](#the-webots-library) + * [Build](#build) + * [Preparation](#preparation) + * [Run](#run) + * [Run without Webots launcher](#run-without-webots-launcher) + * [Run with Webots launcher](#run-with-webots-launcher) + * [Run via terminal](#run-via-terminal) + * [Running the robot on track](#running-the-robot-on-track) + * [Communicate with the DroidControlShip](#communicate-with-the-droidcontrolship) +* [The target](#the-target) + * [Build and flash procedure](#build-and-flash-procedure) +* [The Applications](#the-applications) +* [Documentation](#documentation) +* [Used Libraries](#used-libraries) +* [Issues, Ideas And Bugs](#issues-ideas-and-bugs) +* [License](#license) +* [Contribution](#contribution) + +# The robot +The main target of the firmware is the Pololu Zumo32U4 robot (see https://www.pololu.com/category/129/zumo-robots-and-accessories) from Pololu. + +![deployment](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/master/doc/architecture/uml/PhysicalView/Deployment.plantuml) + +# The simulation +The simulation is based on the open source robot simulator *Webots*. The application and the services are equal to the target firmware. Only the HAL is different in the simulation. + +* Website: https://cyberbotics.com/#cyberbotics +* Github: https://github.com/cyberbotics/webots +* Compatible webots versions: + * v2023a + * v2023b + +## Installation + +1. Install [Webots](https://cyberbotics.com). +2. Setup for [external controllers](https://www.cyberbotics.com/doc/guide/running-extern-robot-controllers): + 1. Set environment variable ```WEBOTS_HOME``` to installation directory of Webots. + 2. Add to path: + * Linux: ```${WEBOTS_HOME}/lib/controller``` + * Windows: ```%WEBOTS_HOME%\lib\controller``` +3. Install the native compiler toolchain: + * Linux: Install the gcc toolchain, depended on your distribution. + * Windows + * Install the [MSYS2](https://www.msys2.org) toolchain. + * Open MSYS2 shell. + * Update package database: ```pacman -Sy pacman``` + * Install GCC: ```pacman -Sy mingw-w64-ucrt-x86_64-gcc``` +4. Ensure a current (>=3.9) Python3 version is installed on your machine. + +## The Webots library +To adapt the HAL to the simulation, some sourcecode files from Webots are necessary. Currently there is no Webots library in the platformio registry available. Therefore a local library is created during the build. Ensure that that Webots is already installed, before you try to build it! + +The library creation is handled in the ```./scripts/create_webots_library.py``` script and runs automatically after building for the WebotsSim environment. + +## Build +1. Start VSCode. +2. PlatformIO project tasks --> <APP-NAME> --> General --> Build\ + Example for **LineFollowerSim** application:\ + ![Example](./doc/images/pio_build.jpg) + +For the simulation use only the applications with "Sim" as postfix, e.g. LineFollowerSim. + +## Preparation +The preparation is shown with the line follower application as example. It expects that the **LineFollowerSim** is already built. + +1. Start the Webots simulation. +2. File --> Open World +3. Select ```./webots/worlds/LineFollowerTrack.wbt```. +4. The loaded world should now look like this: ![webots_world](./doc/images/webots_world.jpg) +5. The simulation waits now for the external controller, like the RadonUlzer. + +## Run +There are 3 ways how to run now the application. Choose according to your needs. + +### Run without Webots launcher +This can be choosen in case the simulation waits just for one robot. + +PlatformIO project tasks --> <APP-NAME> --> General --> Upload + +Example for **LineFollowerSim** application: + +![Example](./doc/images/pio_upload.jpg) + +### Run with Webots launcher +Choose this one in case the simulation waits for more than one robot. Adapt the robot name in the _platformio.ini_. + +See (Single Simulation and Multiple Local Extern Robot Controllers)[https://cyberbotics.com/doc/guide/running-extern-robot-controllers?tab-os=windows#single-simulation-and-multiple-local-extern-robot-controllers] for details. + +PlatformIO project tasks --> <APP-NAME> --> Custom --> WebotsLauncher + +Example for **LineFollowerSim** application: + +![Example](./doc/images/pio_webots_launcher.jpg) + +### Run via terminal +1. Open a command line (shell) and change to the folder with the built executable in ```.pio/build/LineFollowerSim```. This folder contains all necessary shared libraries as well. +2. Start the executable. + +## Running the robot on track + +1. Click in the simulation on the display to focus the simulation. +2. Now the keyboard keys a, b and c can be used to control the robot according to the implemented application logic. + +## Communicate with the DroidControlShip +The communication with the DroidControlShip goes via a Webots serial connection, which is disabled by default. + +Use the -c flag to enable it with default channels (see _webots_robot_serial_rx_channel_ and _webots_robot_serial_tx_channel_ in [platformio.ini](./platformio.ini)). Note, this will disable the standard logging, because the serial communication uses the SerialMuxProt procotol for data interchange. +```bash +$ program.exe -c +``` + +For simplicity a Platformio project task was added, which enables the Webots serial connection as well. + +![Example](./doc/images/pio_webots_launcher_zumo_com_system.jpg) + +# The target + +## Build and flash procedure +1. PlatformIO project tasks --> <APP-NAME> --> General --> Build + * For the target use only the applications with "Target" as postfix, e.g. LineFollowerTarget. +2. Start the bootloader by triggering twice the reset button. The yellow led will start blinking for 10s. Note, after 10s the target will leave the bootloader! +3. PlatformIO project tasks --> <APP-NAME> --> General --> Upload +4. Ready. + +Example for the **LineFollowerTarget** application: + +![Example](./doc/images/pio_target_build_upload.jpg) + +# The Applications + +| Application | Description | Standalone | DroidControlShop Required | Webots World | +| - | - | - | - | - | +| Calib | Application used for motor speed calibration. | Yes | No | ./webots/worlds/LargeTrack.wbt ./webots/worlds/LineFollowerTrack.wbt | +| ConvoyLeader | A line follower, providing information to the [DroidControlShip](https://github.com/BlueAndi/DroidControlShip) in a convoy leader role. | No | Yes | ./webots/worlds/zumo_with_com_system/PlatoonTrack.wbt | +| ConvoyFollower | Convoy follower, providing information to the [DroidControlShip](https://github.com/BlueAndi/DroidControlShip) to drive to its target. | No | Yes | ./webots/worlds/zumo_with_com_system/PlatoonTrack.wbt | +| LineFollower | Just a line follower, using a PID controller. | Yes | No | ./webots/worlds/ETrack.wbt ./webots/worlds/LargeTrack.wbt ./webots/worlds/LineFollowerTrack.wbt | +| RemoteControl | The robot is remote controlled by e.g. the [DroidControlShip](https://github.com/BlueAndi/DroidControlShip) in a convoy follower role. | No | Yes | ./webots/world/zumo_with_com_system/* | +| SensorFusion | The robot provides odometry and inertial data to the [DroidControlShip](https://github.com/BlueAndi/DroidControlShip), which calculates the sensor fusion based location information. | No | Yes | ./webots/worlds/zumo_with_com_system/SensorFusionTrack.wbt | +| Test | Only for testing purposes on native environment. | Yes | No | N/A | + +# Documentation + +* [SW Architecture](./doc/architecture/README.md) +* [SW Configuration](./doc/configuration/README.md) + +# Used Libraries + +| Library | Description | License | +| ----------------------------------------------------------------------- | ----------------------------------------- | ------- | +| [Zumo32U4 library](https://github.com/pololu/zumo-32u4-arduino-library) | Provides access to the Zumo32U4 hardware. | MIT | +| [SerialMuxProt](https://github.com/gabryelreyes/SerialMuxProt) | Multiplexing Communication Protocol | MIT | + +# 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/create_list_file.bat b/create_list_file.bat index dc0fcf5c..daf2fdd0 100644 --- a/create_list_file.bat +++ b/create_list_file.bat @@ -1,11 +1,11 @@ -@echo off -set PIO_PATH=%USERPROFILE%\.platformio\packages\toolchain-atmelavr\bin -set OBJDUMP=%PIO_PATH%\avr-objdump.exe -set ENV=ConvoyLeaderTarget -set SRC=.pio\build\%ENV%\firmware.elf -set DST=firmware.lst - -@echo Create list file of %ENV% to %DST% -%OBJDUMP% -C -d -S %SRC% > %DST% -@echo Ready -pause +@echo off +set PIO_PATH=%USERPROFILE%\.platformio\packages\toolchain-atmelavr\bin +set OBJDUMP=%PIO_PATH%\avr-objdump.exe +set ENV=ConvoyLeaderTarget +set SRC=.pio\build\%ENV%\firmware.elf +set DST=firmware.lst + +@echo Create list file of %ENV% to %DST% +%OBJDUMP% -C -d -S %SRC% > %DST% +@echo Ready +pause diff --git a/create_symbols_file.bat b/create_symbols_file.bat index 32d6ad29..8223357d 100644 --- a/create_symbols_file.bat +++ b/create_symbols_file.bat @@ -1,11 +1,11 @@ -@echo off -set PIO_PATH=%USERPROFILE%\.platformio\packages\toolchain-atmelavr\bin -set NM=%PIO_PATH%\avr-nm.exe -set ENV=ConvoyLeaderTarget -set SRC=.pio\build\%ENV%\firmware.elf -set DST=firmware.sym - -@echo Create symbol file of %ENV% to %DST% -%NM% -C -S --size-sort %SRC% > %DST% -@echo Ready -pause +@echo off +set PIO_PATH=%USERPROFILE%\.platformio\packages\toolchain-atmelavr\bin +set NM=%PIO_PATH%\avr-nm.exe +set ENV=ConvoyLeaderTarget +set SRC=.pio\build\%ENV%\firmware.elf +set DST=firmware.sym + +@echo Create symbol file of %ENV% to %DST% +%NM% -C -S --size-sort %SRC% > %DST% +@echo Ready +pause diff --git a/doc/analysis/LineFollowerTrack/LineFollowerTrackVF0.csv b/doc/analysis/LineFollowerTrack/LineFollowerTrackVF0.csv index a21814fc..bdab2492 100644 --- a/doc/analysis/LineFollowerTrack/LineFollowerTrackVF0.csv +++ b/doc/analysis/LineFollowerTrack/LineFollowerTrackVF0.csv @@ -1,739 +1,739 @@ -Timestamp;Sensor 0;Sensor 1;Sensor 2;Sensor 3;Sensor 4;Position;Position3;Scenario;Speed Left; Speed Right -14056;0;1000;1000;0;0;1500;1500;"Normal";-222;4222 -14064;0;1000;1000;0;0;1500;1500;"Normal";-222;4222 -14072;0;1000;1000;0;0;1500;1500;"Normal";2000;4222 -14080;0;1000;1000;0;0;1500;1500;"Normal";2000;4222 -14088;0;214;1000;0;0;1823;1823;"Normal";4222;2770 -14096;0;215;1000;0;0;1823;1823;"Normal";4222;2770 -14104;0;232;1000;0;0;1811;1811;"Normal";3172;4222 -14112;0;233;1000;0;0;1811;1811;"Normal";3172;4222 -14120;0;222;1000;0;0;1818;1818;"Normal";3314;4222 -14128;0;223;1000;0;0;1817;1817;"Normal";3314;4222 -14136;0;224;1000;0;0;1816;1816;"Normal";3252;4222 -14144;0;222;1000;0;0;1818;1818;"Normal";3252;4222 -14152;0;0;1000;0;0;2000;2000;"Normal";4222;2896 -14160;0;0;1000;0;0;2000;2000;"Normal";4222;2896 -14168;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14176;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14184;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14192;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14200;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14208;0;0;1000;237;0;2191;2191;"Normal";4000;4000 -14216;0;0;1000;228;0;2185;2185;"Normal";4222;2150 -14224;0;0;1000;207;0;2171;2171;"Normal";4222;2150 -14232;0;0;1000;228;1000;3000;2185;"Right angle curve right";4222;-222 -14240;0;0;1000;228;1000;3000;2185;"Right angle curve right";4222;-222 -14248;1000;0;1000;229;1000;2070;2186;"Start-/Stop-line";4222;3300 -14256;1000;0;1000;231;1000;2071;2187;"Start-/Stop-line";4222;3300 -14264;1000;0;1000;205;1000;2063;2170;"Start-/Stop-line";4210;3790 -14272;1000;0;1000;0;0;1000;2000;"Start-/Stop-line";4210;3790 -14280;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14288;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14296;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14304;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14312;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14320;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14328;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14336;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14344;0;235;1000;0;0;1809;1809;"Normal";2090;4222 -14352;0;237;1000;0;0;1808;1808;"Normal";2090;4222 -14360;0;205;1000;0;0;1829;1829;"Normal";3436;4222 -14368;0;223;1000;0;0;1817;1817;"Normal";3436;4222 -14376;0;223;1000;0;0;1817;1817;"Normal";3196;4222 -14384;0;226;1000;0;0;1815;1815;"Normal";3196;4222 -14392;0;0;1000;0;0;2000;2000;"Normal";4222;2902 -14400;0;0;1000;0;0;2000;2000;"Normal";4222;2902 -14408;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14416;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14424;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14432;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14440;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14448;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14456;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14464;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14472;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14480;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14488;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14496;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14504;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14512;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14520;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14528;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14536;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14544;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14552;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14560;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14568;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14576;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14584;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14592;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14600;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14608;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14616;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14624;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14632;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14640;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14648;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14656;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14664;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14672;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14680;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14688;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14696;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14704;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14712;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14720;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14728;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14736;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14744;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14752;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14760;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14768;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14776;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14784;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14792;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14800;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14808;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14816;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14824;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14832;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14840;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14848;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14856;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14864;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14872;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -14880;0;0;1000;224;0;2183;2183;"Normal";4000;4000 -14888;0;0;1000;220;0;2180;2180;"Normal";4222;2200 -14896;0;0;1000;1000;0;2500;2500;"Normal";4222;2200 -14904;0;0;1000;1000;0;2500;2500;"Normal";4222;80 -14912;0;0;1000;1000;0;2500;2500;"Normal";4222;80 -14920;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -14928;0;0;222;1000;0;2818;2818;"Normal";4222;2000 -14936;0;0;226;1000;0;2815;2815;"Normal";4222;-222 -14944;0;0;220;1000;0;2819;2819;"Normal";4222;-222 -14952;0;0;0;1000;0;3000;3000;"Normal";4222;-222 -14960;0;0;0;1000;0;3000;3000;"Normal";4222;-222 -14968;0;0;0;1000;0;3000;3000;"Normal";4222;0 -14976;0;0;0;1000;0;3000;3000;"Normal";4222;0 -14984;0;0;0;1000;0;3000;3000;"Normal";4222;0 -14992;0;0;0;1000;0;3000;3000;"Normal";4222;0 -15000;0;0;0;1000;0;3000;3000;"Normal";4222;0 -15008;0;0;0;1000;0;3000;3000;"Normal";4222;0 -15016;0;0;230;1000;0;2813;2813;"Normal";4222;1870 -15024;0;0;1000;1000;0;2500;2500;"Normal";4222;1870 -15032;0;0;1000;1000;0;2500;2500;"Normal";4122;3878 -15040;0;0;1000;1000;0;2500;2500;"Normal";4122;3878 -15048;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -15056;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -15064;0;0;1000;236;0;2190;2190;"Normal";2900;4222 -15072;0;0;1000;1000;0;2500;2500;"Normal";2900;4222 -15080;0;0;1000;1000;0;2500;2500;"Normal";4222;140 -15088;0;0;1000;1000;0;2500;2500;"Normal";4222;140 -15096;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -15104;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -15112;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -15120;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -15128;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -15136;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -15144;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -15152;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -15160;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -15168;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -15176;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -15184;0;0;233;1000;0;2811;2811;"Normal";4222;2000 -15192;0;0;236;1000;0;2809;2809;"Normal";4222;-222 -15200;0;0;0;1000;0;3000;3000;"Normal";4222;-222 -15208;0;0;0;1000;0;3000;3000;"Normal";4222;-222 -15216;0;0;0;1000;0;3000;3000;"Normal";4222;-222 -15224;0;0;0;1000;0;3000;3000;"Normal";4222;0 -15232;0;0;0;1000;0;3000;3000;"Normal";4222;0 -15240;0;0;0;1000;0;3000;3000;"Normal";4222;0 -15248;0;0;236;1000;0;2809;2809;"Normal";4222;0 -15256;0;0;1000;1000;0;2500;2500;"Normal";3000;4222 -15264;0;0;1000;1000;0;2500;2500;"Normal";3000;4222 -15272;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -15280;0;0;1000;236;0;2190;2190;"Normal";4222;2000 -15288;0;0;1000;236;0;2190;2190;"Normal";2900;4222 -15296;0;0;1000;0;0;2000;2000;"Normal";2900;4222 -15304;0;0;1000;0;0;2000;2000;"Normal";2860;4222 -15312;0;0;1000;0;0;2000;2000;"Normal";2860;4222 -15320;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15328;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15336;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15344;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15352;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15360;0;227;1000;0;0;1814;1814;"Normal";4000;4000 -15368;0;231;1000;0;0;1812;1812;"Normal";2120;4222 -15376;0;234;1000;0;0;1810;1810;"Normal";2120;4222 -15384;0;235;1000;0;0;1809;1809;"Normal";3218;4222 -15392;0;235;1000;0;0;1809;1809;"Normal";3218;4222 -15400;0;0;1000;0;0;2000;2000;"Normal";4222;2854 -15408;0;0;1000;0;0;2000;2000;"Normal";4222;2854 -15416;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15424;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15432;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15440;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15448;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15456;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15464;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15472;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15480;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15488;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15496;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15504;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15512;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15520;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15528;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15536;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15544;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15552;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15560;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15568;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15576;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15584;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15592;0;0;1000;230;0;2186;2186;"Normal";4222;2140 -15600;0;0;1000;228;0;2185;2185;"Normal";4222;2140 -15608;0;0;1000;230;0;2186;2186;"Normal";4222;3256 -15616;0;0;1000;226;0;2184;2184;"Normal";4222;3256 -15624;0;0;1000;0;0;2000;2000;"Normal";2884;4222 -15632;0;0;1000;0;0;2000;2000;"Normal";2884;4222 -15640;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15648;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15656;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15664;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15672;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15680;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15688;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15696;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15704;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15712;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15720;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15728;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15736;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15744;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15752;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15760;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15768;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15776;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15784;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15792;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15800;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15808;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15816;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15824;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15832;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15840;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15848;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15856;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15864;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15872;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15880;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15888;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15896;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15904;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15912;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15920;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15928;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15936;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15944;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15952;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15960;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15968;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15976;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15984;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -15992;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16000;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16008;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16016;0;0;1000;231;0;2187;2187;"Normal";4000;4000 -16024;0;0;1000;230;0;2186;2186;"Normal";4222;2140 -16032;0;0;1000;1000;0;2500;2500;"Normal";4222;2140 -16040;0;0;1000;1000;0;2500;2500;"Normal";4222;116 -16048;0;0;1000;1000;0;2500;2500;"Normal";4222;116 -16056;0;0;233;1000;0;2811;2811;"Normal";4222;-222 -16064;0;0;0;1000;0;3000;3000;"Normal";4222;-222 -16072;0;0;0;1000;0;3000;3000;"Normal";4222;-222 -16080;0;0;0;1000;0;3000;3000;"Normal";4222;-222 -16088;0;0;0;1000;0;3000;3000;"Normal";4222;0 -16096;0;0;0;1000;0;3000;3000;"Normal";4222;0 -16104;0;0;0;1000;0;3000;3000;"Normal";4222;0 -16112;0;0;0;1000;0;3000;3000;"Normal";4222;0 -16120;0;0;0;1000;0;3000;3000;"Normal";4222;0 -16128;0;0;0;1000;0;3000;3000;"Normal";4222;0 -16136;0;0;0;1000;0;3000;3000;"Normal";4222;0 -16144;0;0;0;1000;0;3000;3000;"Normal";4222;0 -16152;0;0;0;1000;0;3000;3000;"Normal";4222;0 -16160;0;0;0;1000;0;3000;3000;"Normal";4222;0 -16168;0;0;238;1000;0;2807;2807;"Normal";4222;1930 -16176;0;0;238;1000;0;2807;2807;"Normal";4222;1930 -16184;0;0;1000;1000;0;2500;2500;"Normal";4158;3842 -16192;0;0;1000;1000;0;2500;2500;"Normal";4158;3842 -16200;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -16208;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -16216;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -16224;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -16232;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -16240;0;0;1000;218;0;2178;2178;"Normal";4222;2000 -16248;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -16256;0;0;1000;234;0;2189;2189;"Normal";4222;2000 -16264;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -16272;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -16280;0;0;1000;205;0;2170;2170;"Normal";2700;4222 -16288;0;0;1000;1000;0;2500;2500;"Normal";2700;4222 -16296;0;0;1000;1000;0;2500;2500;"Normal";4222;20 -16304;0;0;1000;1000;0;2500;2500;"Normal";4222;20 -16312;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -16320;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -16328;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -16336;0;0;226;1000;0;2815;2815;"Normal";4222;2000 -16344;0;0;231;1000;0;2812;2812;"Normal";4222;-222 -16352;0;0;236;1000;0;2809;2809;"Normal";4222;-222 -16360;0;0;232;1000;0;2811;2811;"Normal";4222;762 -16368;0;0;237;1000;0;2808;2808;"Normal";4222;762 -16376;0;0;237;1000;0;2808;2808;"Normal";4222;786 -16384;0;0;237;1000;0;2808;2808;"Normal";4222;786 -16392;0;0;1000;1000;0;2500;2500;"Normal";4152;3848 -16400;0;0;1000;1000;0;2500;2500;"Normal";4152;3848 -16408;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -16416;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -16424;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -16432;0;0;1000;223;0;2182;2182;"Normal";4222;2000 -16440;0;0;1000;0;0;2000;2000;"Normal";1000;4222 -16448;0;0;1000;0;0;2000;2000;"Normal";1000;4222 -16456;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16464;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16472;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16480;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16488;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16496;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16504;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16512;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16520;0;232;1000;0;0;1811;1811;"Normal";2110;4222 -16528;0;226;1000;0;0;1815;1815;"Normal";2110;4222 -16536;0;223;1000;0;0;1817;1817;"Normal";3304;4222 -16544;0;215;1000;0;0;1823;1823;"Normal";3304;4222 -16552;0;0;1000;0;0;2000;2000;"Normal";4222;2902 -16560;0;0;1000;0;0;2000;2000;"Normal";4222;2902 -16568;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16576;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16584;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16592;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16600;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16608;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16616;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16624;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16632;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16640;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16648;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16656;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16664;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16672;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16680;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16688;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16696;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16704;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16712;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16720;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16728;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16736;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16744;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16752;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16760;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16768;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16776;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16784;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16792;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16800;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16808;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16816;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16824;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16832;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16840;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16848;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16856;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16864;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16872;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16880;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16888;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16896;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16904;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16912;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16920;0;0;1000;230;0;2186;2186;"Normal";4222;2140 -16928;0;0;1000;225;0;2183;2183;"Normal";4222;2140 -16936;0;0;1000;230;0;2186;2186;"Normal";4222;3256 -16944;0;0;1000;0;0;2000;2000;"Normal";4222;3256 -16952;0;0;1000;0;0;2000;2000;"Normal";2884;4222 -16960;0;0;1000;0;0;2000;2000;"Normal";2884;4222 -16968;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16976;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16984;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -16992;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17000;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17008;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17016;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17024;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17032;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17040;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17048;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17056;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17064;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 -17072;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 -17080;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 -17088;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 -17096;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 -17104;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 -17112;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 -17120;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 -17128;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 -17136;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 -17144;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 -17152;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 -17160;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 -17168;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 -17176;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 -17184;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 -17192;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 -17200;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 -17208;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 -17216;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 -17224;0;210;1000;0;0;1826;1826;"Normal";2260;4222 -17232;0;222;1000;0;0;1818;1818;"Normal";2260;4222 -17240;0;227;1000;0;0;1814;1814;"Normal";3184;4222 -17248;0;215;1000;0;0;1823;1823;"Normal";3184;4222 -17256;0;219;1000;0;0;1820;1820;"Normal";3316;4222 -17264;0;0;1000;0;0;2000;2000;"Normal";3316;4222 -17272;0;0;1000;0;0;2000;2000;"Normal";4222;2920 -17280;0;0;1000;0;0;2000;2000;"Normal";4222;2920 -17288;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17296;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17304;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17312;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17320;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17328;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17336;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17344;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17352;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17360;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17368;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17376;0;0;1000;189;0;2158;2158;"Normal";4000;4000 -17384;0;0;1000;215;0;2176;2176;"Normal";4222;2240 -17392;0;0;1000;230;0;2186;2186;"Normal";4222;2240 -17400;0;0;1000;226;0;2184;2184;"Normal";4222;3216 -17408;0;0;1000;225;0;2183;2183;"Normal";4222;3216 -17416;0;0;1000;187;0;2157;2157;"Normal";4222;3534 -17424;0;0;1000;0;0;2000;2000;"Normal";4222;3534 -17432;0;0;1000;0;0;2000;2000;"Normal";3058;4222 -17440;0;0;1000;0;0;2000;2000;"Normal";3058;4222 -17448;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17456;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17464;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17472;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17480;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17488;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17496;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17504;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17512;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17520;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17528;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17536;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17544;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17552;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17560;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17568;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17576;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17584;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17592;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17600;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17608;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17616;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17624;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17632;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17640;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17648;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17656;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17664;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17672;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17680;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17688;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17696;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17704;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17712;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17720;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17728;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17736;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17744;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17752;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -17760;0;0;1000;200;0;2166;2166;"Normal";4000;4000 -17768;0;0;1000;1000;0;2500;2500;"Normal";4222;-222 -17776;0;0;1000;1000;0;2500;2500;"Normal";4222;-222 -17784;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -17792;0;0;195;1000;0;2836;2836;"Normal";4222;2000 -17800;0;0;0;1000;0;3000;3000;"Normal";4222;-222 -17808;0;0;0;1000;0;3000;3000;"Normal";4222;-222 -17816;0;0;0;1000;0;3000;3000;"Normal";4222;0 -17824;0;0;0;1000;0;3000;3000;"Normal";4222;0 -17832;0;0;0;1000;0;3000;3000;"Normal";4222;0 -17840;0;0;0;1000;0;3000;3000;"Normal";4222;0 -17848;0;0;0;1000;0;3000;3000;"Normal";4222;0 -17856;0;0;0;1000;0;3000;3000;"Normal";4222;0 -17864;0;0;0;1000;0;3000;3000;"Normal";4222;0 -17872;0;0;0;1000;0;3000;3000;"Normal";4222;0 -17880;0;0;0;1000;0;3000;3000;"Normal";4222;0 -17888;0;0;0;1000;0;3000;3000;"Normal";4222;0 -17896;0;0;0;1000;0;3000;3000;"Normal";4222;0 -17904;0;0;0;1000;0;3000;3000;"Normal";4222;0 -17912;0;0;0;1000;0;3000;3000;"Normal";4222;0 -17920;0;0;213;1000;0;2824;2824;"Normal";4222;0 -17928;0;0;1000;1000;0;2500;2500;"Normal";3000;4222 -17936;0;0;1000;1000;0;2500;2500;"Normal";3000;4222 -17944;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -17952;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -17960;0;0;1000;209;0;2172;2172;"Normal";2720;4222 -17968;0;0;1000;1000;0;2500;2500;"Normal";2720;4222 -17976;0;0;1000;209;0;2172;2172;"Normal";4222;3312 -17984;0;0;1000;194;0;2162;2162;"Normal";4222;3312 -17992;0;0;1000;1000;0;2500;2500;"Normal";4222;32 -18000;0;0;1000;1000;0;2500;2500;"Normal";4222;32 -18008;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -18016;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -18024;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -18032;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -18040;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -18048;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -18056;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -18064;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -18072;0;0;231;1000;0;2812;2812;"Normal";4222;-222 -18080;0;0;236;1000;0;2809;2809;"Normal";4222;-222 -18088;0;0;236;1000;0;2809;2809;"Normal";4222;782 -18096;0;0;232;1000;0;2811;2811;"Normal";4222;782 -18104;0;0;235;1000;0;2809;2809;"Normal";4222;764 -18112;0;0;234;1000;0;2810;2810;"Normal";4222;764 -18120;0;0;235;1000;0;2809;2809;"Normal";4222;764 -18128;0;0;1000;1000;0;2500;2500;"Normal";4222;764 -18136;0;0;1000;1000;0;2500;2500;"Normal";4146;3854 -18144;0;0;1000;1000;0;2500;2500;"Normal";4146;3854 -18152;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -18160;0;0;1000;207;0;2171;2171;"Normal";4222;2000 -18168;0;0;1000;209;0;2172;2172;"Normal";2720;4222 -18176;0;0;1000;0;0;2000;2000;"Normal";2720;4222 -18184;0;0;1000;0;0;2000;2000;"Normal";2968;4222 -18192;0;0;1000;0;0;2000;2000;"Normal";2968;4222 -18200;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18208;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18216;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18224;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18232;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18240;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18248;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18256;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18264;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18272;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18280;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18288;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18296;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18304;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18312;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18320;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18328;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18336;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18344;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18352;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18360;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18368;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18376;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18384;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18392;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18400;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18408;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18416;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18424;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18432;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18440;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18448;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18456;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18464;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18472;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18480;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18488;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18496;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18504;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18512;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18520;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18528;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18536;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18544;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18552;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18560;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18568;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18576;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18584;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18592;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18600;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18608;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18616;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18624;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18632;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18640;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18648;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18656;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18664;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18672;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18680;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18688;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18696;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18704;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18712;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18720;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18728;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18736;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18744;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18752;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18760;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18768;0;0;1000;231;0;2187;2187;"Normal";4000;4000 -18776;0;0;1000;230;0;2186;2186;"Normal";4222;2140 -18784;0;0;1000;233;0;2188;2188;"Normal";4222;2140 -18792;0;0;1000;233;0;2188;2188;"Normal";4222;3236 -18800;0;0;1000;0;0;2000;2000;"Normal";4222;3236 -18808;0;0;1000;0;0;2000;2000;"Normal";2872;4222 -18816;0;0;1000;0;0;2000;2000;"Normal";2872;4222 -18824;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18832;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18840;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18848;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -18856;0;0;1000;231;0;2187;2187;"Normal";4222;2130 -18864;0;0;1000;233;0;2188;2188;"Normal";4222;2130 -18872;0;0;1000;1000;0;2500;2500;"Normal";4222;122 -18880;0;0;1000;1000;0;2500;2500;"Normal";4222;122 -18888;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -18896;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -18904;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -18912;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -18920;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -18928;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -18936;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -18944;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -18952;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -18960;0;0;0;1000;0;3000;3000;"Normal";4222;2000 -18968;0;0;224;1000;0;2816;2816;"Normal";4222;-222 -18976;0;0;0;1000;0;3000;3000;"Normal";4222;-222 -18984;0;0;0;1000;0;3000;3000;"Normal";4222;-222 -18992;0;0;0;1000;0;3000;3000;"Normal";4222;-222 -19000;0;0;0;1000;0;3000;3000;"Normal";4222;0 -19008;0;0;0;1000;0;3000;3000;"Normal";4222;0 -19016;0;0;232;1000;0;2811;2811;"Normal";4222;1890 -19024;0;0;0;1000;0;3000;3000;"Normal";4222;1890 -19032;0;0;187;1000;0;2842;2842;"Normal";4222;446 -19040;0;0;1000;1000;0;2500;2500;"Normal";4222;446 -19048;0;0;1000;1000;0;2500;2500;"Normal";3948;4052 -19056;0;0;1000;1000;0;2500;2500;"Normal";3948;4052 -19064;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -19072;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -19080;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -19088;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -19096;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -19104;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -19112;0;0;1000;205;0;2170;2170;"Normal";2700;4222 -19120;0;0;1000;1000;0;2500;2500;"Normal";2700;4222 -19128;0;0;1000;1000;0;2500;2500;"Normal";4222;20 -19136;0;0;1000;1000;0;2500;2500;"Normal";4222;20 -19144;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -19152;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -19160;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -19168;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -19176;0;0;227;1000;0;2814;2814;"Normal";4222;-222 -19184;0;0;236;1000;0;2809;2809;"Normal";4222;-222 -19192;0;0;231;1000;0;2812;2812;"Normal";4222;764 -19200;0;0;228;1000;0;2814;2814;"Normal";4222;764 -19208;0;0;206;1000;0;2829;2829;"Normal";4222;582 -19216;0;0;228;1000;0;2814;2814;"Normal";4222;582 -19224;0;0;1000;1000;0;2500;2500;"Normal";4026;3974 -19232;0;0;1000;1000;0;2500;2500;"Normal";4026;3974 -19240;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -19248;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -19256;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -19264;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -19272;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 -19280;0;0;1000;233;0;2188;2188;"Normal";4222;2000 -19288;0;0;1000;154;0;2133;2133;"Normal";2330;4222 -19296;0;0;1000;0;0;2000;2000;"Normal";2330;4222 -19304;0;0;1000;0;0;2000;2000;"Normal";3202;4222 -19312;0;0;1000;0;0;2000;2000;"Normal";3202;4222 -19320;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19328;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19336;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19344;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19352;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19360;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19368;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19376;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19384;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19392;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19400;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19408;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19416;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19424;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19432;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19440;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19448;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19456;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19464;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19472;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19480;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19488;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19496;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19504;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19512;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19520;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19528;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19536;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19544;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19552;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19560;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19568;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19576;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19584;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19592;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19600;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19608;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19616;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19624;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19632;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19640;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19648;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19656;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19664;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19672;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19680;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19688;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19696;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19704;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19712;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19720;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19728;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19736;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19744;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19752;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19760;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19768;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19776;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19784;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19792;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19800;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19808;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19816;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19824;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19832;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19840;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19848;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19856;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19864;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19872;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19880;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19888;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19896;0;0;1000;0;0;2000;2000;"Normal";4000;4000 -19904;0;235;1000;0;0;1809;1809;"Normal";4000;4000 -19912;0;236;1000;0;0;1809;1809;"Normal";2090;4222 -19920;0;233;1000;0;0;1811;1811;"Normal";2090;4222 -19928;0;235;1000;0;0;1809;1809;"Normal";3236;4222 -19936;0;0;1000;0;0;2000;2000;"Normal";3236;4222 -19944;0;0;1000;0;0;2000;2000;"Normal";4222;2854 -19952;1000;0;1000;0;1000;2000;2000;"Track finished";4222;2854 +Timestamp;Sensor 0;Sensor 1;Sensor 2;Sensor 3;Sensor 4;Position;Position3;Scenario;Speed Left; Speed Right +14056;0;1000;1000;0;0;1500;1500;"Normal";-222;4222 +14064;0;1000;1000;0;0;1500;1500;"Normal";-222;4222 +14072;0;1000;1000;0;0;1500;1500;"Normal";2000;4222 +14080;0;1000;1000;0;0;1500;1500;"Normal";2000;4222 +14088;0;214;1000;0;0;1823;1823;"Normal";4222;2770 +14096;0;215;1000;0;0;1823;1823;"Normal";4222;2770 +14104;0;232;1000;0;0;1811;1811;"Normal";3172;4222 +14112;0;233;1000;0;0;1811;1811;"Normal";3172;4222 +14120;0;222;1000;0;0;1818;1818;"Normal";3314;4222 +14128;0;223;1000;0;0;1817;1817;"Normal";3314;4222 +14136;0;224;1000;0;0;1816;1816;"Normal";3252;4222 +14144;0;222;1000;0;0;1818;1818;"Normal";3252;4222 +14152;0;0;1000;0;0;2000;2000;"Normal";4222;2896 +14160;0;0;1000;0;0;2000;2000;"Normal";4222;2896 +14168;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14176;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14184;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14192;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14200;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14208;0;0;1000;237;0;2191;2191;"Normal";4000;4000 +14216;0;0;1000;228;0;2185;2185;"Normal";4222;2150 +14224;0;0;1000;207;0;2171;2171;"Normal";4222;2150 +14232;0;0;1000;228;1000;3000;2185;"Right angle curve right";4222;-222 +14240;0;0;1000;228;1000;3000;2185;"Right angle curve right";4222;-222 +14248;1000;0;1000;229;1000;2070;2186;"Start-/Stop-line";4222;3300 +14256;1000;0;1000;231;1000;2071;2187;"Start-/Stop-line";4222;3300 +14264;1000;0;1000;205;1000;2063;2170;"Start-/Stop-line";4210;3790 +14272;1000;0;1000;0;0;1000;2000;"Start-/Stop-line";4210;3790 +14280;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14288;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14296;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14304;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14312;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14320;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14328;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14336;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14344;0;235;1000;0;0;1809;1809;"Normal";2090;4222 +14352;0;237;1000;0;0;1808;1808;"Normal";2090;4222 +14360;0;205;1000;0;0;1829;1829;"Normal";3436;4222 +14368;0;223;1000;0;0;1817;1817;"Normal";3436;4222 +14376;0;223;1000;0;0;1817;1817;"Normal";3196;4222 +14384;0;226;1000;0;0;1815;1815;"Normal";3196;4222 +14392;0;0;1000;0;0;2000;2000;"Normal";4222;2902 +14400;0;0;1000;0;0;2000;2000;"Normal";4222;2902 +14408;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14416;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14424;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14432;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14440;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14448;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14456;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14464;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14472;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14480;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14488;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14496;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14504;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14512;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14520;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14528;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14536;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14544;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14552;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14560;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14568;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14576;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14584;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14592;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14600;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14608;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14616;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14624;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14632;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14640;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14648;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14656;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14664;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14672;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14680;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14688;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14696;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14704;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14712;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14720;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14728;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14736;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14744;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14752;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14760;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14768;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14776;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14784;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14792;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14800;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14808;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14816;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14824;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14832;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14840;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14848;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14856;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14864;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14872;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +14880;0;0;1000;224;0;2183;2183;"Normal";4000;4000 +14888;0;0;1000;220;0;2180;2180;"Normal";4222;2200 +14896;0;0;1000;1000;0;2500;2500;"Normal";4222;2200 +14904;0;0;1000;1000;0;2500;2500;"Normal";4222;80 +14912;0;0;1000;1000;0;2500;2500;"Normal";4222;80 +14920;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +14928;0;0;222;1000;0;2818;2818;"Normal";4222;2000 +14936;0;0;226;1000;0;2815;2815;"Normal";4222;-222 +14944;0;0;220;1000;0;2819;2819;"Normal";4222;-222 +14952;0;0;0;1000;0;3000;3000;"Normal";4222;-222 +14960;0;0;0;1000;0;3000;3000;"Normal";4222;-222 +14968;0;0;0;1000;0;3000;3000;"Normal";4222;0 +14976;0;0;0;1000;0;3000;3000;"Normal";4222;0 +14984;0;0;0;1000;0;3000;3000;"Normal";4222;0 +14992;0;0;0;1000;0;3000;3000;"Normal";4222;0 +15000;0;0;0;1000;0;3000;3000;"Normal";4222;0 +15008;0;0;0;1000;0;3000;3000;"Normal";4222;0 +15016;0;0;230;1000;0;2813;2813;"Normal";4222;1870 +15024;0;0;1000;1000;0;2500;2500;"Normal";4222;1870 +15032;0;0;1000;1000;0;2500;2500;"Normal";4122;3878 +15040;0;0;1000;1000;0;2500;2500;"Normal";4122;3878 +15048;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +15056;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +15064;0;0;1000;236;0;2190;2190;"Normal";2900;4222 +15072;0;0;1000;1000;0;2500;2500;"Normal";2900;4222 +15080;0;0;1000;1000;0;2500;2500;"Normal";4222;140 +15088;0;0;1000;1000;0;2500;2500;"Normal";4222;140 +15096;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +15104;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +15112;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +15120;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +15128;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +15136;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +15144;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +15152;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +15160;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +15168;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +15176;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +15184;0;0;233;1000;0;2811;2811;"Normal";4222;2000 +15192;0;0;236;1000;0;2809;2809;"Normal";4222;-222 +15200;0;0;0;1000;0;3000;3000;"Normal";4222;-222 +15208;0;0;0;1000;0;3000;3000;"Normal";4222;-222 +15216;0;0;0;1000;0;3000;3000;"Normal";4222;-222 +15224;0;0;0;1000;0;3000;3000;"Normal";4222;0 +15232;0;0;0;1000;0;3000;3000;"Normal";4222;0 +15240;0;0;0;1000;0;3000;3000;"Normal";4222;0 +15248;0;0;236;1000;0;2809;2809;"Normal";4222;0 +15256;0;0;1000;1000;0;2500;2500;"Normal";3000;4222 +15264;0;0;1000;1000;0;2500;2500;"Normal";3000;4222 +15272;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +15280;0;0;1000;236;0;2190;2190;"Normal";4222;2000 +15288;0;0;1000;236;0;2190;2190;"Normal";2900;4222 +15296;0;0;1000;0;0;2000;2000;"Normal";2900;4222 +15304;0;0;1000;0;0;2000;2000;"Normal";2860;4222 +15312;0;0;1000;0;0;2000;2000;"Normal";2860;4222 +15320;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15328;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15336;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15344;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15352;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15360;0;227;1000;0;0;1814;1814;"Normal";4000;4000 +15368;0;231;1000;0;0;1812;1812;"Normal";2120;4222 +15376;0;234;1000;0;0;1810;1810;"Normal";2120;4222 +15384;0;235;1000;0;0;1809;1809;"Normal";3218;4222 +15392;0;235;1000;0;0;1809;1809;"Normal";3218;4222 +15400;0;0;1000;0;0;2000;2000;"Normal";4222;2854 +15408;0;0;1000;0;0;2000;2000;"Normal";4222;2854 +15416;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15424;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15432;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15440;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15448;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15456;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15464;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15472;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15480;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15488;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15496;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15504;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15512;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15520;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15528;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15536;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15544;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15552;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15560;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15568;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15576;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15584;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15592;0;0;1000;230;0;2186;2186;"Normal";4222;2140 +15600;0;0;1000;228;0;2185;2185;"Normal";4222;2140 +15608;0;0;1000;230;0;2186;2186;"Normal";4222;3256 +15616;0;0;1000;226;0;2184;2184;"Normal";4222;3256 +15624;0;0;1000;0;0;2000;2000;"Normal";2884;4222 +15632;0;0;1000;0;0;2000;2000;"Normal";2884;4222 +15640;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15648;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15656;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15664;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15672;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15680;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15688;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15696;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15704;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15712;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15720;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15728;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15736;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15744;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15752;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15760;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15768;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15776;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15784;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15792;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15800;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15808;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15816;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15824;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15832;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15840;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15848;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15856;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15864;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15872;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15880;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15888;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15896;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15904;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15912;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15920;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15928;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15936;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15944;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15952;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15960;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15968;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15976;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15984;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +15992;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16000;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16008;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16016;0;0;1000;231;0;2187;2187;"Normal";4000;4000 +16024;0;0;1000;230;0;2186;2186;"Normal";4222;2140 +16032;0;0;1000;1000;0;2500;2500;"Normal";4222;2140 +16040;0;0;1000;1000;0;2500;2500;"Normal";4222;116 +16048;0;0;1000;1000;0;2500;2500;"Normal";4222;116 +16056;0;0;233;1000;0;2811;2811;"Normal";4222;-222 +16064;0;0;0;1000;0;3000;3000;"Normal";4222;-222 +16072;0;0;0;1000;0;3000;3000;"Normal";4222;-222 +16080;0;0;0;1000;0;3000;3000;"Normal";4222;-222 +16088;0;0;0;1000;0;3000;3000;"Normal";4222;0 +16096;0;0;0;1000;0;3000;3000;"Normal";4222;0 +16104;0;0;0;1000;0;3000;3000;"Normal";4222;0 +16112;0;0;0;1000;0;3000;3000;"Normal";4222;0 +16120;0;0;0;1000;0;3000;3000;"Normal";4222;0 +16128;0;0;0;1000;0;3000;3000;"Normal";4222;0 +16136;0;0;0;1000;0;3000;3000;"Normal";4222;0 +16144;0;0;0;1000;0;3000;3000;"Normal";4222;0 +16152;0;0;0;1000;0;3000;3000;"Normal";4222;0 +16160;0;0;0;1000;0;3000;3000;"Normal";4222;0 +16168;0;0;238;1000;0;2807;2807;"Normal";4222;1930 +16176;0;0;238;1000;0;2807;2807;"Normal";4222;1930 +16184;0;0;1000;1000;0;2500;2500;"Normal";4158;3842 +16192;0;0;1000;1000;0;2500;2500;"Normal";4158;3842 +16200;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +16208;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +16216;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +16224;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +16232;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +16240;0;0;1000;218;0;2178;2178;"Normal";4222;2000 +16248;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +16256;0;0;1000;234;0;2189;2189;"Normal";4222;2000 +16264;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +16272;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +16280;0;0;1000;205;0;2170;2170;"Normal";2700;4222 +16288;0;0;1000;1000;0;2500;2500;"Normal";2700;4222 +16296;0;0;1000;1000;0;2500;2500;"Normal";4222;20 +16304;0;0;1000;1000;0;2500;2500;"Normal";4222;20 +16312;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +16320;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +16328;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +16336;0;0;226;1000;0;2815;2815;"Normal";4222;2000 +16344;0;0;231;1000;0;2812;2812;"Normal";4222;-222 +16352;0;0;236;1000;0;2809;2809;"Normal";4222;-222 +16360;0;0;232;1000;0;2811;2811;"Normal";4222;762 +16368;0;0;237;1000;0;2808;2808;"Normal";4222;762 +16376;0;0;237;1000;0;2808;2808;"Normal";4222;786 +16384;0;0;237;1000;0;2808;2808;"Normal";4222;786 +16392;0;0;1000;1000;0;2500;2500;"Normal";4152;3848 +16400;0;0;1000;1000;0;2500;2500;"Normal";4152;3848 +16408;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +16416;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +16424;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +16432;0;0;1000;223;0;2182;2182;"Normal";4222;2000 +16440;0;0;1000;0;0;2000;2000;"Normal";1000;4222 +16448;0;0;1000;0;0;2000;2000;"Normal";1000;4222 +16456;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16464;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16472;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16480;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16488;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16496;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16504;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16512;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16520;0;232;1000;0;0;1811;1811;"Normal";2110;4222 +16528;0;226;1000;0;0;1815;1815;"Normal";2110;4222 +16536;0;223;1000;0;0;1817;1817;"Normal";3304;4222 +16544;0;215;1000;0;0;1823;1823;"Normal";3304;4222 +16552;0;0;1000;0;0;2000;2000;"Normal";4222;2902 +16560;0;0;1000;0;0;2000;2000;"Normal";4222;2902 +16568;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16576;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16584;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16592;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16600;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16608;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16616;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16624;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16632;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16640;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16648;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16656;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16664;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16672;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16680;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16688;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16696;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16704;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16712;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16720;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16728;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16736;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16744;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16752;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16760;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16768;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16776;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16784;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16792;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16800;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16808;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16816;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16824;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16832;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16840;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16848;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16856;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16864;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16872;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16880;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16888;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16896;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16904;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16912;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16920;0;0;1000;230;0;2186;2186;"Normal";4222;2140 +16928;0;0;1000;225;0;2183;2183;"Normal";4222;2140 +16936;0;0;1000;230;0;2186;2186;"Normal";4222;3256 +16944;0;0;1000;0;0;2000;2000;"Normal";4222;3256 +16952;0;0;1000;0;0;2000;2000;"Normal";2884;4222 +16960;0;0;1000;0;0;2000;2000;"Normal";2884;4222 +16968;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16976;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16984;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +16992;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17000;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17008;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17016;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17024;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17032;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17040;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17048;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17056;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17064;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 +17072;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 +17080;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 +17088;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 +17096;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 +17104;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 +17112;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 +17120;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 +17128;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 +17136;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 +17144;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 +17152;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 +17160;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 +17168;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 +17176;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 +17184;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 +17192;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 +17200;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 +17208;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 +17216;0;0;0;0;0;4000;;"Track lost by gap";4000;4000 +17224;0;210;1000;0;0;1826;1826;"Normal";2260;4222 +17232;0;222;1000;0;0;1818;1818;"Normal";2260;4222 +17240;0;227;1000;0;0;1814;1814;"Normal";3184;4222 +17248;0;215;1000;0;0;1823;1823;"Normal";3184;4222 +17256;0;219;1000;0;0;1820;1820;"Normal";3316;4222 +17264;0;0;1000;0;0;2000;2000;"Normal";3316;4222 +17272;0;0;1000;0;0;2000;2000;"Normal";4222;2920 +17280;0;0;1000;0;0;2000;2000;"Normal";4222;2920 +17288;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17296;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17304;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17312;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17320;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17328;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17336;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17344;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17352;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17360;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17368;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17376;0;0;1000;189;0;2158;2158;"Normal";4000;4000 +17384;0;0;1000;215;0;2176;2176;"Normal";4222;2240 +17392;0;0;1000;230;0;2186;2186;"Normal";4222;2240 +17400;0;0;1000;226;0;2184;2184;"Normal";4222;3216 +17408;0;0;1000;225;0;2183;2183;"Normal";4222;3216 +17416;0;0;1000;187;0;2157;2157;"Normal";4222;3534 +17424;0;0;1000;0;0;2000;2000;"Normal";4222;3534 +17432;0;0;1000;0;0;2000;2000;"Normal";3058;4222 +17440;0;0;1000;0;0;2000;2000;"Normal";3058;4222 +17448;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17456;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17464;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17472;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17480;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17488;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17496;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17504;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17512;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17520;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17528;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17536;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17544;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17552;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17560;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17568;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17576;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17584;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17592;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17600;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17608;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17616;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17624;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17632;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17640;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17648;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17656;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17664;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17672;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17680;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17688;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17696;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17704;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17712;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17720;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17728;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17736;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17744;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17752;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +17760;0;0;1000;200;0;2166;2166;"Normal";4000;4000 +17768;0;0;1000;1000;0;2500;2500;"Normal";4222;-222 +17776;0;0;1000;1000;0;2500;2500;"Normal";4222;-222 +17784;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +17792;0;0;195;1000;0;2836;2836;"Normal";4222;2000 +17800;0;0;0;1000;0;3000;3000;"Normal";4222;-222 +17808;0;0;0;1000;0;3000;3000;"Normal";4222;-222 +17816;0;0;0;1000;0;3000;3000;"Normal";4222;0 +17824;0;0;0;1000;0;3000;3000;"Normal";4222;0 +17832;0;0;0;1000;0;3000;3000;"Normal";4222;0 +17840;0;0;0;1000;0;3000;3000;"Normal";4222;0 +17848;0;0;0;1000;0;3000;3000;"Normal";4222;0 +17856;0;0;0;1000;0;3000;3000;"Normal";4222;0 +17864;0;0;0;1000;0;3000;3000;"Normal";4222;0 +17872;0;0;0;1000;0;3000;3000;"Normal";4222;0 +17880;0;0;0;1000;0;3000;3000;"Normal";4222;0 +17888;0;0;0;1000;0;3000;3000;"Normal";4222;0 +17896;0;0;0;1000;0;3000;3000;"Normal";4222;0 +17904;0;0;0;1000;0;3000;3000;"Normal";4222;0 +17912;0;0;0;1000;0;3000;3000;"Normal";4222;0 +17920;0;0;213;1000;0;2824;2824;"Normal";4222;0 +17928;0;0;1000;1000;0;2500;2500;"Normal";3000;4222 +17936;0;0;1000;1000;0;2500;2500;"Normal";3000;4222 +17944;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +17952;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +17960;0;0;1000;209;0;2172;2172;"Normal";2720;4222 +17968;0;0;1000;1000;0;2500;2500;"Normal";2720;4222 +17976;0;0;1000;209;0;2172;2172;"Normal";4222;3312 +17984;0;0;1000;194;0;2162;2162;"Normal";4222;3312 +17992;0;0;1000;1000;0;2500;2500;"Normal";4222;32 +18000;0;0;1000;1000;0;2500;2500;"Normal";4222;32 +18008;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +18016;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +18024;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +18032;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +18040;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +18048;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +18056;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +18064;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +18072;0;0;231;1000;0;2812;2812;"Normal";4222;-222 +18080;0;0;236;1000;0;2809;2809;"Normal";4222;-222 +18088;0;0;236;1000;0;2809;2809;"Normal";4222;782 +18096;0;0;232;1000;0;2811;2811;"Normal";4222;782 +18104;0;0;235;1000;0;2809;2809;"Normal";4222;764 +18112;0;0;234;1000;0;2810;2810;"Normal";4222;764 +18120;0;0;235;1000;0;2809;2809;"Normal";4222;764 +18128;0;0;1000;1000;0;2500;2500;"Normal";4222;764 +18136;0;0;1000;1000;0;2500;2500;"Normal";4146;3854 +18144;0;0;1000;1000;0;2500;2500;"Normal";4146;3854 +18152;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +18160;0;0;1000;207;0;2171;2171;"Normal";4222;2000 +18168;0;0;1000;209;0;2172;2172;"Normal";2720;4222 +18176;0;0;1000;0;0;2000;2000;"Normal";2720;4222 +18184;0;0;1000;0;0;2000;2000;"Normal";2968;4222 +18192;0;0;1000;0;0;2000;2000;"Normal";2968;4222 +18200;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18208;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18216;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18224;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18232;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18240;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18248;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18256;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18264;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18272;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18280;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18288;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18296;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18304;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18312;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18320;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18328;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18336;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18344;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18352;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18360;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18368;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18376;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18384;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18392;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18400;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18408;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18416;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18424;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18432;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18440;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18448;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18456;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18464;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18472;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18480;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18488;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18496;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18504;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18512;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18520;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18528;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18536;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18544;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18552;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18560;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18568;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18576;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18584;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18592;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18600;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18608;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18616;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18624;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18632;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18640;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18648;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18656;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18664;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18672;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18680;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18688;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18696;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18704;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18712;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18720;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18728;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18736;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18744;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18752;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18760;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18768;0;0;1000;231;0;2187;2187;"Normal";4000;4000 +18776;0;0;1000;230;0;2186;2186;"Normal";4222;2140 +18784;0;0;1000;233;0;2188;2188;"Normal";4222;2140 +18792;0;0;1000;233;0;2188;2188;"Normal";4222;3236 +18800;0;0;1000;0;0;2000;2000;"Normal";4222;3236 +18808;0;0;1000;0;0;2000;2000;"Normal";2872;4222 +18816;0;0;1000;0;0;2000;2000;"Normal";2872;4222 +18824;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18832;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18840;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18848;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +18856;0;0;1000;231;0;2187;2187;"Normal";4222;2130 +18864;0;0;1000;233;0;2188;2188;"Normal";4222;2130 +18872;0;0;1000;1000;0;2500;2500;"Normal";4222;122 +18880;0;0;1000;1000;0;2500;2500;"Normal";4222;122 +18888;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +18896;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +18904;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +18912;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +18920;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +18928;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +18936;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +18944;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +18952;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +18960;0;0;0;1000;0;3000;3000;"Normal";4222;2000 +18968;0;0;224;1000;0;2816;2816;"Normal";4222;-222 +18976;0;0;0;1000;0;3000;3000;"Normal";4222;-222 +18984;0;0;0;1000;0;3000;3000;"Normal";4222;-222 +18992;0;0;0;1000;0;3000;3000;"Normal";4222;-222 +19000;0;0;0;1000;0;3000;3000;"Normal";4222;0 +19008;0;0;0;1000;0;3000;3000;"Normal";4222;0 +19016;0;0;232;1000;0;2811;2811;"Normal";4222;1890 +19024;0;0;0;1000;0;3000;3000;"Normal";4222;1890 +19032;0;0;187;1000;0;2842;2842;"Normal";4222;446 +19040;0;0;1000;1000;0;2500;2500;"Normal";4222;446 +19048;0;0;1000;1000;0;2500;2500;"Normal";3948;4052 +19056;0;0;1000;1000;0;2500;2500;"Normal";3948;4052 +19064;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +19072;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +19080;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +19088;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +19096;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +19104;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +19112;0;0;1000;205;0;2170;2170;"Normal";2700;4222 +19120;0;0;1000;1000;0;2500;2500;"Normal";2700;4222 +19128;0;0;1000;1000;0;2500;2500;"Normal";4222;20 +19136;0;0;1000;1000;0;2500;2500;"Normal";4222;20 +19144;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +19152;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +19160;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +19168;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +19176;0;0;227;1000;0;2814;2814;"Normal";4222;-222 +19184;0;0;236;1000;0;2809;2809;"Normal";4222;-222 +19192;0;0;231;1000;0;2812;2812;"Normal";4222;764 +19200;0;0;228;1000;0;2814;2814;"Normal";4222;764 +19208;0;0;206;1000;0;2829;2829;"Normal";4222;582 +19216;0;0;228;1000;0;2814;2814;"Normal";4222;582 +19224;0;0;1000;1000;0;2500;2500;"Normal";4026;3974 +19232;0;0;1000;1000;0;2500;2500;"Normal";4026;3974 +19240;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +19248;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +19256;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +19264;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +19272;0;0;1000;1000;0;2500;2500;"Normal";4222;2000 +19280;0;0;1000;233;0;2188;2188;"Normal";4222;2000 +19288;0;0;1000;154;0;2133;2133;"Normal";2330;4222 +19296;0;0;1000;0;0;2000;2000;"Normal";2330;4222 +19304;0;0;1000;0;0;2000;2000;"Normal";3202;4222 +19312;0;0;1000;0;0;2000;2000;"Normal";3202;4222 +19320;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19328;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19336;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19344;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19352;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19360;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19368;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19376;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19384;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19392;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19400;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19408;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19416;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19424;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19432;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19440;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19448;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19456;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19464;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19472;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19480;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19488;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19496;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19504;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19512;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19520;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19528;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19536;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19544;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19552;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19560;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19568;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19576;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19584;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19592;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19600;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19608;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19616;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19624;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19632;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19640;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19648;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19656;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19664;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19672;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19680;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19688;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19696;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19704;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19712;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19720;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19728;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19736;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19744;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19752;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19760;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19768;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19776;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19784;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19792;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19800;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19808;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19816;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19824;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19832;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19840;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19848;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19856;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19864;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19872;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19880;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19888;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19896;0;0;1000;0;0;2000;2000;"Normal";4000;4000 +19904;0;235;1000;0;0;1809;1809;"Normal";4000;4000 +19912;0;236;1000;0;0;1809;1809;"Normal";2090;4222 +19920;0;233;1000;0;0;1811;1811;"Normal";2090;4222 +19928;0;235;1000;0;0;1809;1809;"Normal";3236;4222 +19936;0;0;1000;0;0;2000;2000;"Normal";3236;4222 +19944;0;0;1000;0;0;2000;2000;"Normal";4222;2854 +19952;1000;0;1000;0;1000;2000;2000;"Track finished";4222;2854 diff --git a/doc/architecture/CONVOYLEADER.md b/doc/architecture/CONVOYLEADER.md index 992fd91d..5b8ffe4c 100644 --- a/doc/architecture/CONVOYLEADER.md +++ b/doc/architecture/CONVOYLEADER.md @@ -1,70 +1,70 @@ -# Radon Ulzer - Line Follower - -* [SW Architecture](#sw-architecture) - * [Logical View](#logical-view) - * [Application](#application) - * [Startup](#startup) - * [LineSensorsCalibration](#linesensorscalibration) - * [Ready](#ready) - * [Release Track](#release-track) - * [Driving](#driving) - * [Error](#error) - * [Process View](#process-view) -* [Abbreviations](#abbreviations) -* [Issues, Ideas And Bugs](#issues-ideas-and-bugs) -* [License](#license) -* [Contribution](#contribution) - -# SW Architecture -The following part contains the specific details of the ConvoyLeader application. - -## Logical View - -### Application - -![application](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/LineFollower/Application.plantuml) - -#### Startup - -![startup](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/LineFollower/StartupState.plantuml) - -#### LineSensorsCalibration - -![lineSensorsCalibration](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/LineFollower/LineSensorsCalibrationState.plantuml) - -#### Ready - -![ready](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/LineFollower/ReadyState.plantuml) - -#### Release Track - -![releaseTrack](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/LineFollower/ReleaseTrackState.plantuml) - -#### Driving - -![driving](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/LineFollower/DrivingState.plantuml) - -#### Error - -![error](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/LineFollower/ErrorState.plantuml) - -## Process View - -![processView](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/ProcessView/LineFollower/SystemStates2.plantuml) - -# Abbreviations - -| Abbreviation | Description | -| - | - | -| HAL | Hardware Abstraction Layer | - -# 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 - Line Follower + +* [SW Architecture](#sw-architecture) + * [Logical View](#logical-view) + * [Application](#application) + * [Startup](#startup) + * [LineSensorsCalibration](#linesensorscalibration) + * [Ready](#ready) + * [Release Track](#release-track) + * [Driving](#driving) + * [Error](#error) + * [Process View](#process-view) +* [Abbreviations](#abbreviations) +* [Issues, Ideas And Bugs](#issues-ideas-and-bugs) +* [License](#license) +* [Contribution](#contribution) + +# SW Architecture +The following part contains the specific details of the ConvoyLeader application. + +## Logical View + +### Application + +![application](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/LineFollower/Application.plantuml) + +#### Startup + +![startup](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/LineFollower/StartupState.plantuml) + +#### LineSensorsCalibration + +![lineSensorsCalibration](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/LineFollower/LineSensorsCalibrationState.plantuml) + +#### Ready + +![ready](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/LineFollower/ReadyState.plantuml) + +#### Release Track + +![releaseTrack](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/LineFollower/ReleaseTrackState.plantuml) + +#### Driving + +![driving](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/LineFollower/DrivingState.plantuml) + +#### Error + +![error](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/LineFollower/ErrorState.plantuml) + +## Process View + +![processView](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/ProcessView/LineFollower/SystemStates2.plantuml) + +# Abbreviations + +| Abbreviation | Description | +| - | - | +| HAL | Hardware Abstraction Layer | + +# 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/LINEFOLLOWER.md b/doc/architecture/LINEFOLLOWER.md index 1553cdef..3a739f5c 100644 --- a/doc/architecture/LINEFOLLOWER.md +++ b/doc/architecture/LINEFOLLOWER.md @@ -1,75 +1,75 @@ -# Radon Ulzer - Line Follower - -* [SW Architecture](#sw-architecture) - * [Logical View](#logical-view) - * [Application](#application) - * [Startup](#startup) - * [LineSensorsCalibration](#linesensorscalibration) - * [MotorSpeedCalibration](#motorspeedcalibration) - * [Ready](#ready) - * [Release Track](#release-track) - * [Driving](#driving) - * [Error](#error) - * [Process View](#process-view) -* [Abbreviations](#abbreviations) -* [Issues, Ideas And Bugs](#issues-ideas-and-bugs) -* [License](#license) -* [Contribution](#contribution) - -# SW Architecture -The following part contains the specific details of the LineFollower application. - -## Logical View - -### Application - -![application](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/LineFollower/Application.plantuml) - -#### Startup - -![startup](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/LineFollower/StartupState.plantuml) - -#### LineSensorsCalibration - -![lineSensorsCalibration](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/LineFollower/LineSensorsCalibrationState.plantuml) - -#### MotorSpeedCalibration - -![MotorSpeedCalibration](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/LineFollower/LineSensorsCalibrationState.plantuml) - -#### Ready - -![ready](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/LineFollower/ReadyState.plantuml) - -#### Release Track - -![releaseTrack](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/LineFollower/ReleaseTrackState.plantuml) - -#### Driving - -![driving](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/LineFollower/DrivingState.plantuml) - -#### Error - -![error](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/LineFollower/ErrorState.plantuml) - -## Process View - -![processView](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/ProcessView/LineFollower/SystemStates2.plantuml) - -# Abbreviations - -| Abbreviation | Description | -| - | - | -| HAL | Hardware Abstraction Layer | - -# 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 - Line Follower + +* [SW Architecture](#sw-architecture) + * [Logical View](#logical-view) + * [Application](#application) + * [Startup](#startup) + * [LineSensorsCalibration](#linesensorscalibration) + * [MotorSpeedCalibration](#motorspeedcalibration) + * [Ready](#ready) + * [Release Track](#release-track) + * [Driving](#driving) + * [Error](#error) + * [Process View](#process-view) +* [Abbreviations](#abbreviations) +* [Issues, Ideas And Bugs](#issues-ideas-and-bugs) +* [License](#license) +* [Contribution](#contribution) + +# SW Architecture +The following part contains the specific details of the LineFollower application. + +## Logical View + +### Application + +![application](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/LineFollower/Application.plantuml) + +#### Startup + +![startup](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/LineFollower/StartupState.plantuml) + +#### LineSensorsCalibration + +![lineSensorsCalibration](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/LineFollower/LineSensorsCalibrationState.plantuml) + +#### MotorSpeedCalibration + +![MotorSpeedCalibration](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/LineFollower/LineSensorsCalibrationState.plantuml) + +#### Ready + +![ready](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/LineFollower/ReadyState.plantuml) + +#### Release Track + +![releaseTrack](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/LineFollower/ReleaseTrackState.plantuml) + +#### Driving + +![driving](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/LineFollower/DrivingState.plantuml) + +#### Error + +![error](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/LineFollower/ErrorState.plantuml) + +## Process View + +![processView](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/ProcessView/LineFollower/SystemStates2.plantuml) + +# Abbreviations + +| Abbreviation | Description | +| - | - | +| HAL | Hardware Abstraction Layer | + +# 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/README.md b/doc/architecture/README.md index da98e963..7ef728fc 100644 --- a/doc/architecture/README.md +++ b/doc/architecture/README.md @@ -1,115 +1,115 @@ -# Radon Ulzer - -* [SW Architecture](#sw-architecture) -* [Abbreviations](#abbreviations) - * [Physical View](#physical-view) - * [Development View](#development-view) - * [Layers](#layers) - * [Logical View](#logical-view) - * [Application](#application) - * [Service](#service) - * [Differential Drive](#differential-drive) - * [Odometry](#odometry) - * [Speedometer](#speedometer) - * [HAL](#hal) - * [Process View](#process-view) - * [Differential Drive](#differential-drive-1) -* [Issues, Ideas And Bugs](#issues-ideas-and-bugs) -* [License](#license) -* [Contribution](#contribution) - -# SW Architecture -The Radon Ulzer repository contains several applications. Each application is described according to the 4+1 architectural view. - -![architecturalView](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/ViewModels.plantuml) - -# Abbreviations - -| Abbreviation | Description | -| - | - | -| HAL | Hardware Abstraction Layer | - -## Physical View -The physical view shows the deployment which is equal to all applications. But not every application may use all of the provided sensors and actors. - -![physicalView](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/PhysicalView/Deployment.plantuml) - -## Development View - -### Layers - -![deployment](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/DevelopmentView/Layers.plantuml) - -## Logical View - -### Application -The following applications are supported: - -* [ConvoyLeader application](CONVOYLEADER.md) -* [LineFollower application](LINEFOLLOWER.md) -* [RemoteControl application](REMOTECONTROL.md) -* [SensorFusion application](SENSORFUSION.md) - -### Service - -![service](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/Service.plantuml) - -#### Differential Drive - -![differentialDrive](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/DifferentialDrive.plantuml) - -#### Odometry - -![odometry](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/Odometry.plantuml) - -Base equations: -* $distanceLeft [mm] = \frac{encoderStepsLeft [steps]}{encoderStepsPerMM [\frac{steps}{mm}]}$ -* $distanceRight [mm] = \frac{encoderStepsRight [steps]}{encoderStepsPerMM [\frac{steps}{mm}]}$ -* $stepsCenter [steps] = \frac{encoderStepsLeft - encoderStepsRight}{2}$ -* $distanceCenter [mm] = \frac{stepsCenter [steps]}{encoderStepsPerMM [\frac{steps}{mm}]}$ - -Orientation: -* $alpha [rad] = \frac{distanceRight [mm] - distanceLeft [mm]}{wheelBase [mm]}$ -* $orientation' [rad] = orientation [rad] + alpha [rad]$ -* $orientation' [rad] = orientation [rad]~\%~2\pi$ -* $-2\pi < Orientation < 2\pi$ -* After wrapping on the positive limit $2\pi$, the orientation remains positive and starts from 0 again. -* After wrapping on the negative limit $2\pi$, the orientation remains negative and starts from 0 again. - -Position: -* $dX [mm] = -distanceCenter [mm] \cdot sin(orientation' [rad])$ <- Approximation for performance reason -* $dY [mm] = distanceCenter [mm] \cdot cos(orientation' [rad])$ <- Approximation for performance reason -* $x' [mm] = x [mm] + dX [mm]$ -* $y' [mm] = y [mm] + dY [mm]$ - -Improvement for better accuracy: -* $alpha [mrad] = \frac{1000 \cdot (encoderStepsRight [steps] - encoderStepsLeft [steps])}{encoderStepsPerMM [\frac{steps}{mm}] \cdot wheelBase [mm]}$ -* $orientation' [mrad] = orientation [mrad] + alpha [mrad]$ -* $dX [mm] = -distanceCenter [mm] \cdot sin(\frac{orientation' [mrad]}{1000})$ -* $dY [mm] = distanceCenter [mm] \cdot cos(\frac{orientation' [mrad]}{1000})$ - -#### Speedometer - -![speedometer](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/Speedometer.plantuml) - -### HAL -The hardware abstraction layer (HAL) for the target is shown. For the simulation and the test only the realization part is different and may base on other 3rd party components. - -![hal](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/HAL.plantuml) - -## Process View - -### Differential Drive - -![differentialDrive](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/ProcessView/DifferentialDrive.plantuml) - -# 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 + +* [SW Architecture](#sw-architecture) +* [Abbreviations](#abbreviations) + * [Physical View](#physical-view) + * [Development View](#development-view) + * [Layers](#layers) + * [Logical View](#logical-view) + * [Application](#application) + * [Service](#service) + * [Differential Drive](#differential-drive) + * [Odometry](#odometry) + * [Speedometer](#speedometer) + * [HAL](#hal) + * [Process View](#process-view) + * [Differential Drive](#differential-drive-1) +* [Issues, Ideas And Bugs](#issues-ideas-and-bugs) +* [License](#license) +* [Contribution](#contribution) + +# SW Architecture +The Radon Ulzer repository contains several applications. Each application is described according to the 4+1 architectural view. + +![architecturalView](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/ViewModels.plantuml) + +# Abbreviations + +| Abbreviation | Description | +| - | - | +| HAL | Hardware Abstraction Layer | + +## Physical View +The physical view shows the deployment which is equal to all applications. But not every application may use all of the provided sensors and actors. + +![physicalView](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/PhysicalView/Deployment.plantuml) + +## Development View + +### Layers + +![deployment](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/DevelopmentView/Layers.plantuml) + +## Logical View + +### Application +The following applications are supported: + +* [ConvoyLeader application](CONVOYLEADER.md) +* [LineFollower application](LINEFOLLOWER.md) +* [RemoteControl application](REMOTECONTROL.md) +* [SensorFusion application](SENSORFUSION.md) + +### Service + +![service](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/Service.plantuml) + +#### Differential Drive + +![differentialDrive](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/DifferentialDrive.plantuml) + +#### Odometry + +![odometry](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/Odometry.plantuml) + +Base equations: +* $distanceLeft [mm] = \frac{encoderStepsLeft [steps]}{encoderStepsPerMM [\frac{steps}{mm}]}$ +* $distanceRight [mm] = \frac{encoderStepsRight [steps]}{encoderStepsPerMM [\frac{steps}{mm}]}$ +* $stepsCenter [steps] = \frac{encoderStepsLeft - encoderStepsRight}{2}$ +* $distanceCenter [mm] = \frac{stepsCenter [steps]}{encoderStepsPerMM [\frac{steps}{mm}]}$ + +Orientation: +* $alpha [rad] = \frac{distanceRight [mm] - distanceLeft [mm]}{wheelBase [mm]}$ +* $orientation' [rad] = orientation [rad] + alpha [rad]$ +* $orientation' [rad] = orientation [rad]~\%~2\pi$ +* $-2\pi < Orientation < 2\pi$ +* After wrapping on the positive limit $2\pi$, the orientation remains positive and starts from 0 again. +* After wrapping on the negative limit $2\pi$, the orientation remains negative and starts from 0 again. + +Position: +* $dX [mm] = -distanceCenter [mm] \cdot sin(orientation' [rad])$ <- Approximation for performance reason +* $dY [mm] = distanceCenter [mm] \cdot cos(orientation' [rad])$ <- Approximation for performance reason +* $x' [mm] = x [mm] + dX [mm]$ +* $y' [mm] = y [mm] + dY [mm]$ + +Improvement for better accuracy: +* $alpha [mrad] = \frac{1000 \cdot (encoderStepsRight [steps] - encoderStepsLeft [steps])}{encoderStepsPerMM [\frac{steps}{mm}] \cdot wheelBase [mm]}$ +* $orientation' [mrad] = orientation [mrad] + alpha [mrad]$ +* $dX [mm] = -distanceCenter [mm] \cdot sin(\frac{orientation' [mrad]}{1000})$ +* $dY [mm] = distanceCenter [mm] \cdot cos(\frac{orientation' [mrad]}{1000})$ + +#### Speedometer + +![speedometer](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/Speedometer.plantuml) + +### HAL +The hardware abstraction layer (HAL) for the target is shown. For the simulation and the test only the realization part is different and may base on other 3rd party components. + +![hal](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/HAL.plantuml) + +## Process View + +### Differential Drive + +![differentialDrive](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/ProcessView/DifferentialDrive.plantuml) + +# 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/REMOTECONTROL.md b/doc/architecture/REMOTECONTROL.md index 49297f13..8f2dc8ef 100644 --- a/doc/architecture/REMOTECONTROL.md +++ b/doc/architecture/REMOTECONTROL.md @@ -1,115 +1,115 @@ -# Radon Ulzer - Remote Control - -* [General](#general) - * [SerialMuxProt Channels](#serialmuxprot-channels) - * [Rx channel "REMOTE\_CMD"](#rx-channel-remote_cmd) - * [Tx channel "REMOTE\_RSP"](#tx-channel-remote_rsp) - * [Rx channel "MOT\_SPEEDS"](#rx-channel-mot_speeds) - * [Tx channel "LINE\_SENS"](#tx-channel-line_sens) -* [SW Architecture](#sw-architecture) - * [Logical View](#logical-view) - * [Application](#application) - * [Startup](#startup) - * [LineSensorsCalibration](#linesensorscalibration) - * [MotorSpeedCalibration](#motorspeedcalibration) - * [RemoteCtrl](#remotectrl) - * [Error](#error) - * [Process View](#process-view) -* [Abbreviations](#abbreviations) -* [Issues, Ideas And Bugs](#issues-ideas-and-bugs) -* [License](#license) -* [Contribution](#contribution) - -# General - -The robot can be controlled remotely by using the SerialMuxProt protocol. - -On target the physical communication uses the serial. - -On simulation the physical communication uses a socket connection. - -## SerialMuxProt Channels - -### Rx channel "REMOTE_CMD" -This channel is used to receive commands, which will be executed by the application in RemoteCtrl state. A command related response will be sent via the "REMOTE_RSP" channel. - -Possible commands: -* NO_ACTION (0) - Nothing will happend and no response will be sent. -* START_LINE_SENSOR_CALIB (1) - Start line sensor calibration. -* START_MOTOR_SPEED_CALIB (2) - Start motor speed calibration. -* REINIT_BOARD (3) - Re-initialized the board. Required for webots simulation. - -### Tx channel "REMOTE_RSP" -This channel is used to send command related responses. A response will be sent only once and not periodically. - -Possible responses: -* OK (0) - Command successful executed. -* PENDING (1) - Command execution is pending. -* ERROR (2) - Command execution failed. - -### Rx channel "MOT_SPEEDS" -This channel is used to receive linear motor speed information for the left and right motor in [mm/s]. - -* The dataypes int16_t is used. -* Order: - * The left motor speed. - * The right motor speed. -* Endianess: Big endian - -### Tx channel "LINE_SENS" -This channel is used to send line sensor informations in digits. - -* The dataypes uint16_t is used. -* Order: - * From most left to right line sensor. -* Endianess: Big endian - -# SW Architecture -The following part contains the specific details of the RemoteControl application. - -## Logical View - -### Application - -![application](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/RemoteControl/Application.plantuml) - -#### Startup - -![startup](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/RemoteControl/StartupState.plantuml) - -#### LineSensorsCalibration - -![lineSensorsCalibration](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/RemoteControl/LineSensorsCalibrationState.plantuml) - -#### MotorSpeedCalibration - -![MotorSpeedCalibration](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/RemoteControl/LineSensorsCalibrationState.plantuml) - -#### RemoteCtrl - -![driving](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/RemoteControl/RemoteCtrlState.plantuml) - -#### Error - -![error](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/RemoteControl/ErrorState.plantuml) - -## Process View - -![processView](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/ProcessView/RemoteControl/SystemStates.plantuml) - -# Abbreviations - -| Abbreviation | Description | -| - | - | -| HAL | Hardware Abstraction Layer | - -# 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 - Remote Control + +* [General](#general) + * [SerialMuxProt Channels](#serialmuxprot-channels) + * [Rx channel "REMOTE\_CMD"](#rx-channel-remote_cmd) + * [Tx channel "REMOTE\_RSP"](#tx-channel-remote_rsp) + * [Rx channel "MOT\_SPEEDS"](#rx-channel-mot_speeds) + * [Tx channel "LINE\_SENS"](#tx-channel-line_sens) +* [SW Architecture](#sw-architecture) + * [Logical View](#logical-view) + * [Application](#application) + * [Startup](#startup) + * [LineSensorsCalibration](#linesensorscalibration) + * [MotorSpeedCalibration](#motorspeedcalibration) + * [RemoteCtrl](#remotectrl) + * [Error](#error) + * [Process View](#process-view) +* [Abbreviations](#abbreviations) +* [Issues, Ideas And Bugs](#issues-ideas-and-bugs) +* [License](#license) +* [Contribution](#contribution) + +# General + +The robot can be controlled remotely by using the SerialMuxProt protocol. + +On target the physical communication uses the serial. + +On simulation the physical communication uses a socket connection. + +## SerialMuxProt Channels + +### Rx channel "REMOTE_CMD" +This channel is used to receive commands, which will be executed by the application in RemoteCtrl state. A command related response will be sent via the "REMOTE_RSP" channel. + +Possible commands: +* NO_ACTION (0) - Nothing will happend and no response will be sent. +* START_LINE_SENSOR_CALIB (1) - Start line sensor calibration. +* START_MOTOR_SPEED_CALIB (2) - Start motor speed calibration. +* REINIT_BOARD (3) - Re-initialized the board. Required for webots simulation. + +### Tx channel "REMOTE_RSP" +This channel is used to send command related responses. A response will be sent only once and not periodically. + +Possible responses: +* OK (0) - Command successful executed. +* PENDING (1) - Command execution is pending. +* ERROR (2) - Command execution failed. + +### Rx channel "MOT_SPEEDS" +This channel is used to receive linear motor speed information for the left and right motor in [mm/s]. + +* The dataypes int16_t is used. +* Order: + * The left motor speed. + * The right motor speed. +* Endianess: Big endian + +### Tx channel "LINE_SENS" +This channel is used to send line sensor informations in digits. + +* The dataypes uint16_t is used. +* Order: + * From most left to right line sensor. +* Endianess: Big endian + +# SW Architecture +The following part contains the specific details of the RemoteControl application. + +## Logical View + +### Application + +![application](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/RemoteControl/Application.plantuml) + +#### Startup + +![startup](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/RemoteControl/StartupState.plantuml) + +#### LineSensorsCalibration + +![lineSensorsCalibration](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/RemoteControl/LineSensorsCalibrationState.plantuml) + +#### MotorSpeedCalibration + +![MotorSpeedCalibration](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/RemoteControl/LineSensorsCalibrationState.plantuml) + +#### RemoteCtrl + +![driving](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/RemoteControl/RemoteCtrlState.plantuml) + +#### Error + +![error](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/RemoteControl/ErrorState.plantuml) + +## Process View + +![processView](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/ProcessView/RemoteControl/SystemStates.plantuml) + +# Abbreviations + +| Abbreviation | Description | +| - | - | +| HAL | Hardware Abstraction Layer | + +# 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/SENSORFUSION.md b/doc/architecture/SENSORFUSION.md index 993e4906..fc0c3ecf 100644 --- a/doc/architecture/SENSORFUSION.md +++ b/doc/architecture/SENSORFUSION.md @@ -1,85 +1,85 @@ -# Radon Ulzer - SENSORFUSION - -* [General](#general) - * [SerialMuxProt Channels](#serialmuxprot-channels) - * [Tx channel "SENSOR\_DATA"](#tx-channel-sensor_data) - * [Tx channel "END\_LINE"](#tx-channel-end_line) -* [SW Architecture](#sw-architecture) - * [Logical View](#logical-view) - * [Application](#application) - * [HAL](#hal) - * [Process View](#process-view) -* [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 -All channels are defined in the file [SerialMuxChannels.h](https://github.com/BlueAndi/RadonUlzer/blob/main/lib/APPSensorFusion/SerialMuxChannels.h) - -### 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) - * Turnrate 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) - * Time passed since the last sensor value (in ms) -* Endianess: Big endian - -### Tx channel "END_LINE" -This channel is used to send a flag to signal that a new End Line has been detected. This is used for testing purposes. -The only content that is sent is a boolean variable with the value true. - -# SW Architecture -The following part contains the specific details of the SensorFusion application. - -## Logical View - -### Application -The application uses the same [States](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 compared to the HAL of the other Applications. -ButtonB, ButtonC, LedYellow, LedRed, the Display and the ProximitySensors have been removed in the App specific HAL. -The Buzzer Component can not be removed since it is used inside the Service Layer. Instead, the Buzzer is replaced with a Dummy Buzzer without any Functionality. -![HALSensorFusion](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/SensorFusion/HAL_SensorFusion.puml) - -An IMU has been added: -![HALIMU](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/SensorFusion/HAL_IMU.puml) - -## Process View -Compared to the System States of the [Linefollower Application](https://github.com/BlueAndi/RadonUlzer/blob/main/doc/architecture/LINEFOLLOWER.md), the only change that has been made is that the detection of an end line does not trigger a change of state from DrivingState to ReadyState. This is done for testing purposes. -![processView](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/ProcessView/SensorFusion/SystemStates.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 "SENSOR\_DATA"](#tx-channel-sensor_data) + * [Tx channel "END\_LINE"](#tx-channel-end_line) +* [SW Architecture](#sw-architecture) + * [Logical View](#logical-view) + * [Application](#application) + * [HAL](#hal) + * [Process View](#process-view) +* [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 +All channels are defined in the file [SerialMuxChannels.h](https://github.com/BlueAndi/RadonUlzer/blob/main/lib/APPSensorFusion/SerialMuxChannels.h) + +### 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) + * Turnrate 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) + * Time passed since the last sensor value (in ms) +* Endianess: Big endian + +### Tx channel "END_LINE" +This channel is used to send a flag to signal that a new End Line has been detected. This is used for testing purposes. +The only content that is sent is a boolean variable with the value true. + +# SW Architecture +The following part contains the specific details of the SensorFusion application. + +## Logical View + +### Application +The application uses the same [States](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 compared to the HAL of the other Applications. +ButtonB, ButtonC, LedYellow, LedRed, the Display and the ProximitySensors have been removed in the App specific HAL. +The Buzzer Component can not be removed since it is used inside the Service Layer. Instead, the Buzzer is replaced with a Dummy Buzzer without any Functionality. +![HALSensorFusion](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/SensorFusion/HAL_SensorFusion.puml) + +An IMU has been added: +![HALIMU](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/SensorFusion/HAL_IMU.puml) + +## Process View +Compared to the System States of the [Linefollower Application](https://github.com/BlueAndi/RadonUlzer/blob/main/doc/architecture/LINEFOLLOWER.md), the only change that has been made is that the detection of an end line does not trigger a change of state from DrivingState to ReadyState. This is done for testing purposes. +![processView](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/ProcessView/SensorFusion/SystemStates.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/DevelopmentView/Layers.plantuml b/doc/architecture/uml/DevelopmentView/Layers.plantuml index d16ac4e4..4e07ed04 100644 --- a/doc/architecture/uml/DevelopmentView/Layers.plantuml +++ b/doc/architecture/uml/DevelopmentView/Layers.plantuml @@ -1,31 +1,31 @@ -@startuml - -title Layers - -package Application -package Service -package HAL - -Application ..> Service: <> -Application ...> HAL: <> - -Service ..> HAL: <> - -package "HAL" as halDetail { - package HALInterfaces - package HALTarget - package HALSim - package HALTest - - HALInterfaces <|.. HALTarget: <> - HALInterfaces <|.. HALSim: <> - HALInterfaces <|.. HALTest: <> -} - -HAL -[hidden]-- halDetail - -note top of halDetail - HAL white box view -end note - +@startuml + +title Layers + +package Application +package Service +package HAL + +Application ..> Service: <> +Application ...> HAL: <> + +Service ..> HAL: <> + +package "HAL" as halDetail { + package HALInterfaces + package HALTarget + package HALSim + package HALTest + + HALInterfaces <|.. HALTarget: <> + HALInterfaces <|.. HALSim: <> + HALInterfaces <|.. HALTest: <> +} + +HAL -[hidden]-- halDetail + +note top of halDetail + HAL white box view +end note + @enduml \ No newline at end of file diff --git a/doc/architecture/uml/LogicalView/DifferentialDrive.plantuml b/doc/architecture/uml/LogicalView/DifferentialDrive.plantuml index b944766c..682b0cf7 100644 --- a/doc/architecture/uml/LogicalView/DifferentialDrive.plantuml +++ b/doc/architecture/uml/LogicalView/DifferentialDrive.plantuml @@ -1,29 +1,29 @@ -@startuml - -package "Service" { - class "DifferentialDrive" as differentialDrive <> - class "SimpleTimer" as simpleTimer <> - class "PIDController" as pidController <> - class "Speedometer" as speedometer <> - - note left of differentialDrive - Steering the robot with linear (steps/s) and - angular speed [mrad/s] by controlling the - speed with PID controllers. - end note - - differentialDrive --> simpleTimer - differentialDrive *--> "2" pidController - differentialDrive ..> speedometer: <> -} - -package "HAL" { - class "IMotors" as iMotors - class "Motors" as motors - - iMotors <|.. motors: <> -} - -differentialDrive ...> iMotors: <> - -@enduml +@startuml + +package "Service" { + class "DifferentialDrive" as differentialDrive <> + class "SimpleTimer" as simpleTimer <> + class "PIDController" as pidController <> + class "Speedometer" as speedometer <> + + note left of differentialDrive + Steering the robot with linear (steps/s) and + angular speed [mrad/s] by controlling the + speed with PID controllers. + end note + + differentialDrive --> simpleTimer + differentialDrive *--> "2" pidController + differentialDrive ..> speedometer: <> +} + +package "HAL" { + class "IMotors" as iMotors + class "Motors" as motors + + iMotors <|.. motors: <> +} + +differentialDrive ...> iMotors: <> + +@enduml diff --git a/doc/architecture/uml/LogicalView/HAL.plantuml b/doc/architecture/uml/LogicalView/HAL.plantuml index b2cb53f7..88559bbe 100644 --- a/doc/architecture/uml/LogicalView/HAL.plantuml +++ b/doc/architecture/uml/LogicalView/HAL.plantuml @@ -1,182 +1,182 @@ -@startuml - -title Hardware Abstraction Layer - -package "HAL" as hal { - - package "Interfaces" as halInterfaces { - - interface "IButton" as iButton { - + {abstract} isPressed() : bool - + {abstract} waitForRelease() : void - } - - interface "IBuzzer" as iBuzzer { - + {abstract} playFrequency(freq : uint16_t, duration : uin16_t, volume : uint8_t) : void - + {abstract} playMelody(sequence : const char*) : void - + {abstract} playMelodyPGM(sequence : const char*) : void - + {abstract} isPlaying() : bool - + {abstract} process() : 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 "IDisplay" as iDisplay { - + {abstract} clear() : void - + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void - + {abstract} print(str : const String&) : size_t - + {abstract} print(str : const char[]) : size_t - + {abstract} print(value : uint8_t) : size_t - + {abstract} print(value : uint16_t) : size_t - + {abstract} print(value : uint32_t) : size_t - + {abstract} print(value : int8_t) : size_t - + {abstract} print(value : int16_t) : size_t - + {abstract} print(value : int32_t) : size_t - } - - interface "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 "IProximitySensors" as iProximitySensors { - + {abstract} initFrontSensor() : void - + {abstract} getNumSensors() const : uint8_t - + {abstract} read() : void - + {abstract} countsFrontWithLeftLeds() : uint8_t - + {abstract} countsFrontWithRightLeds() : uint8_t - } - - interface "ISettings" as iSettings { - + {abstract} init() : void - + {abstract} getMaxSpeed() : int16_t - + {abstract} setMaxSpeed(maxSpeed : int16_t) : void - } - } - - class Board << namespace >> { - + getButtonA() : IButton& - + getButtonB() : IButton& - + getButtonC() : IButton& - + getDisplay() : IDisplay& - + getEncoders() : IEncoders& - + getLedRed() : ILed& - + getLedYellow() : ILed& - + getLedGreen() : iLed& - + getLineSensors() : ILineSensors& - + getMotors() : IMotors& - + getProximitySensors() : IProximitySensors& - + getSettings() : ISettings& - } - - 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 - - package "Target" as target { - class LineSensors - class ButtonA - class ButtonB - class ButtonC - class Motors - class Buzzer - class Display - class Encoders - class LedRed - class LedYellow - class LedGreen - class ProximitySensors - class Settings - } - - iLineSensors <|... LineSensors: <> - iButton <|... ButtonA: <> - iButton <|... ButtonB: <> - iButton <|... ButtonC: <> - iMotors <|... Motors: <> - iBuzzer <|... Buzzer: <> - iDisplay <|... Display: <> - iEncoders <|... Encoders: <> - iLed <|... LedRed: <> - iLed <|... LedYellow: <> - iLed <|... LedGreen: <> - iProximitySensors <|... ProximitySensors: <> - iSettings <|... Settings: <> -} - -package "Zumo32U4 library" as zumo32u4Lib { - class Zumo32U4LineSensors - class Zumo32U4ButtonA - class Zumo32U4ButtonB - class Zumo32U4ButtonC - class Zumo32U4Motors - class Zumo32U4Buzzer - class Zumo32U4LCD - class Zumo32U4Encoders - class Zumo32U4 <> - class Zumo32U4ProximitySensors -} - -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 -ButtonB *--> Zumo32U4ButtonB -ButtonC *--> Zumo32U4ButtonC -Motors *--> Zumo32U4Motors -Buzzer *--> Zumo32U4Buzzer -Display *--> Zumo32U4LCD -Encoders *--> Zumo32U4Encoders -LedRed ..> Zumo32U4: <> -LedYellow ..> Zumo32U4: <> -LedGreen ..> Zumo32U4: <> -ProximitySensors ..> Zumo32U4ProximitySensors: <> - -note bottom of hal - The hardware abstraction layer contains - all drivers, provided by the Pololu Library. -end note - +@startuml + +title Hardware Abstraction Layer + +package "HAL" as hal { + + package "Interfaces" as halInterfaces { + + interface "IButton" as iButton { + + {abstract} isPressed() : bool + + {abstract} waitForRelease() : void + } + + interface "IBuzzer" as iBuzzer { + + {abstract} playFrequency(freq : uint16_t, duration : uin16_t, volume : uint8_t) : void + + {abstract} playMelody(sequence : const char*) : void + + {abstract} playMelodyPGM(sequence : const char*) : void + + {abstract} isPlaying() : bool + + {abstract} process() : 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 "IDisplay" as iDisplay { + + {abstract} clear() : void + + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void + + {abstract} print(str : const String&) : size_t + + {abstract} print(str : const char[]) : size_t + + {abstract} print(value : uint8_t) : size_t + + {abstract} print(value : uint16_t) : size_t + + {abstract} print(value : uint32_t) : size_t + + {abstract} print(value : int8_t) : size_t + + {abstract} print(value : int16_t) : size_t + + {abstract} print(value : int32_t) : size_t + } + + interface "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 "IProximitySensors" as iProximitySensors { + + {abstract} initFrontSensor() : void + + {abstract} getNumSensors() const : uint8_t + + {abstract} read() : void + + {abstract} countsFrontWithLeftLeds() : uint8_t + + {abstract} countsFrontWithRightLeds() : uint8_t + } + + interface "ISettings" as iSettings { + + {abstract} init() : void + + {abstract} getMaxSpeed() : int16_t + + {abstract} setMaxSpeed(maxSpeed : int16_t) : void + } + } + + class Board << namespace >> { + + getButtonA() : IButton& + + getButtonB() : IButton& + + getButtonC() : IButton& + + getDisplay() : IDisplay& + + getEncoders() : IEncoders& + + getLedRed() : ILed& + + getLedYellow() : ILed& + + getLedGreen() : iLed& + + getLineSensors() : ILineSensors& + + getMotors() : IMotors& + + getProximitySensors() : IProximitySensors& + + getSettings() : ISettings& + } + + 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 + + package "Target" as target { + class LineSensors + class ButtonA + class ButtonB + class ButtonC + class Motors + class Buzzer + class Display + class Encoders + class LedRed + class LedYellow + class LedGreen + class ProximitySensors + class Settings + } + + iLineSensors <|... LineSensors: <> + iButton <|... ButtonA: <> + iButton <|... ButtonB: <> + iButton <|... ButtonC: <> + iMotors <|... Motors: <> + iBuzzer <|... Buzzer: <> + iDisplay <|... Display: <> + iEncoders <|... Encoders: <> + iLed <|... LedRed: <> + iLed <|... LedYellow: <> + iLed <|... LedGreen: <> + iProximitySensors <|... ProximitySensors: <> + iSettings <|... Settings: <> +} + +package "Zumo32U4 library" as zumo32u4Lib { + class Zumo32U4LineSensors + class Zumo32U4ButtonA + class Zumo32U4ButtonB + class Zumo32U4ButtonC + class Zumo32U4Motors + class Zumo32U4Buzzer + class Zumo32U4LCD + class Zumo32U4Encoders + class Zumo32U4 <> + class Zumo32U4ProximitySensors +} + +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 +ButtonB *--> Zumo32U4ButtonB +ButtonC *--> Zumo32U4ButtonC +Motors *--> Zumo32U4Motors +Buzzer *--> Zumo32U4Buzzer +Display *--> Zumo32U4LCD +Encoders *--> Zumo32U4Encoders +LedRed ..> Zumo32U4: <> +LedYellow ..> Zumo32U4: <> +LedGreen ..> Zumo32U4: <> +ProximitySensors ..> Zumo32U4ProximitySensors: <> + +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/doc/architecture/uml/LogicalView/LineFollower/Application.plantuml b/doc/architecture/uml/LogicalView/LineFollower/Application.plantuml index 29070ffb..a3d32090 100644 --- a/doc/architecture/uml/LogicalView/LineFollower/Application.plantuml +++ b/doc/architecture/uml/LogicalView/LineFollower/Application.plantuml @@ -1,120 +1,120 @@ -@startuml - -title Application - -package "Application" as appLayer { - - class App <
> { - + setup() : void - + loop() : void - } - - note left of App - The program entry point. - end note - - class StateMachine <> { - + setState(state : IState*) : void - + getState() : IState* - + process() : void - } - - note left of StateMachine - The state machine executes always one - state exclusive. It is responsible to - enter/leave/process a state. - end note - - interface IState { - + {abstract} entry() : void - + {abstract} process(sm : StateMachine&) : void - + {abstract} exit() : void - } - - note left of IState - Defines the abstract state interface, - which forces every inherited class - to realize it. - end note - - class StartupState <> - class MotorSpeedCalibrationState <> - class LineSensorsCalibrationState <> - class ErrorState <> - class DrivingState <> - class ReadyState <> - class ReleaseTrackState <> - - class ParameterSets <> - - note bottom of ParameterSets - One of several provided parameter sets - can be selected. The selected one is - used during the competiton. - end note - - note bottom of StartupState - The system starts up and shows - the team name on the display. - end note - - note bottom of MotorSpeedCalibrationState - The robot drives with full speed forward - and with full speed backwar to determine - the max speed in steps/s. The slowest - motor is considered! - end note - - note bottom of LineSensorsCalibrationState - The robot turns several times the - line sensors over the track for - calibration. - end note - - note bottom of ErrorState - Error information is shown on display. - Confirmation from operator is requested. - end note - - note bottom of DrivingState - The robot follows the line and handles - any detected gap. - end note - - note bottom of ReadyState - The robot is stopped and waits for - operator input. - end note - - note bottom of ReleaseTrackState - Operator selects parameter set and - starts the competition after a - defined time. - end note -} - -note top of appLayer - Hint: See the application state behaviour - in the corresponding state diagram. - - The ParameterSets supports several parameters, - e.g. to initialize the PID controller and the - top speed. -end note - -App *--> StateMachine - -StateMachine o--> "0..1" IState - -IState <|.. StartupState: <> -IState <|.... MotorSpeedCalibrationState: <> -IState <|.. LineSensorsCalibrationState: <> -IState <|.... ErrorState: <> -IState <|.. ReadyState: <> -IState <|.... ReleaseTrackState: <> -IState <|.... DrivingState: <> - -ReleaseTrackState ..> ParameterSets: <> -DrivingState ..> ParameterSets: <> - +@startuml + +title Application + +package "Application" as appLayer { + + class App <
> { + + setup() : void + + loop() : void + } + + note left of App + The program entry point. + end note + + class StateMachine <> { + + setState(state : IState*) : void + + getState() : IState* + + process() : void + } + + note left of StateMachine + The state machine executes always one + state exclusive. It is responsible to + enter/leave/process a state. + end note + + interface IState { + + {abstract} entry() : void + + {abstract} process(sm : StateMachine&) : void + + {abstract} exit() : void + } + + note left of IState + Defines the abstract state interface, + which forces every inherited class + to realize it. + end note + + class StartupState <> + class MotorSpeedCalibrationState <> + class LineSensorsCalibrationState <> + class ErrorState <> + class DrivingState <> + class ReadyState <> + class ReleaseTrackState <> + + class ParameterSets <> + + note bottom of ParameterSets + One of several provided parameter sets + can be selected. The selected one is + used during the competiton. + end note + + note bottom of StartupState + The system starts up and shows + the team name on the display. + end note + + note bottom of MotorSpeedCalibrationState + The robot drives with full speed forward + and with full speed backwar to determine + the max speed in steps/s. The slowest + motor is considered! + end note + + note bottom of LineSensorsCalibrationState + The robot turns several times the + line sensors over the track for + calibration. + end note + + note bottom of ErrorState + Error information is shown on display. + Confirmation from operator is requested. + end note + + note bottom of DrivingState + The robot follows the line and handles + any detected gap. + end note + + note bottom of ReadyState + The robot is stopped and waits for + operator input. + end note + + note bottom of ReleaseTrackState + Operator selects parameter set and + starts the competition after a + defined time. + end note +} + +note top of appLayer + Hint: See the application state behaviour + in the corresponding state diagram. + + The ParameterSets supports several parameters, + e.g. to initialize the PID controller and the + top speed. +end note + +App *--> StateMachine + +StateMachine o--> "0..1" IState + +IState <|.. StartupState: <> +IState <|.... MotorSpeedCalibrationState: <> +IState <|.. LineSensorsCalibrationState: <> +IState <|.... ErrorState: <> +IState <|.. ReadyState: <> +IState <|.... ReleaseTrackState: <> +IState <|.... DrivingState: <> + +ReleaseTrackState ..> ParameterSets: <> +DrivingState ..> ParameterSets: <> + @enduml \ No newline at end of file diff --git a/doc/architecture/uml/LogicalView/LineFollower/DrivingState.plantuml b/doc/architecture/uml/LogicalView/LineFollower/DrivingState.plantuml index d8ab48d6..571fe35f 100644 --- a/doc/architecture/uml/LogicalView/LineFollower/DrivingState.plantuml +++ b/doc/architecture/uml/LogicalView/LineFollower/DrivingState.plantuml @@ -1,129 +1,129 @@ -@startuml - -title Driving State - -package "Application" as appLayer { - - class DrivingState <> { - + {static} getInstance() : DrivingState - + entry() : void - + process(sm : StateMachine&) : void - + exit() : void - } - - class ReadyState <> - - class ParameterSet <> - - class ParameterSets <> { - + {static} getInstance() : ParameterSets& - + choose(setId : uint8_t) : void - + next() : void - + getCurrentSetId() const : uint8_t - + getParameterSet() : ParameterSet& - } - - ParameterSet --+ ParameterSets - ParameterSet "3" <--* ParameterSets - - DrivingState .r.> ReadyState: <> - DrivingState .u.> ParameterSets: <> -} - -package "Service" as serviceLayer { - - class SimpleTimer <> { - + start(duration : uint32_t) : void - + restart() : void - + stop() : void - + isTimeout() : bool - } - - class Sound <> { - + playAlarm() : void - + playBeep() : void - } - - class Odometry <> { - + {static} getInstance() : Odometry& - + process() : void - + getMileageCenter() : uint32_t - + clearMileage() : void - } - - class PIDController < T > <> { - + calculate(setPoint : T, processValue : T) : T - + getPFactor(numerator : T&, denominator : T&) : void - + setPFactor(numerator : T, denominator : T) : void - + getIFactor(numerator : T&, denominator : T&) : void - + setIFactor(numerator : T, denominator : T) : void - + getDFactor(numerator : T&, denominator : T&) : void - + setDFactor(numerator : T, denominator : T) : void - + setLimits(min : T, max : T) : void - + clear() : void - + getSampleTime() : uint32_t - + setSampleTime(sampleTime : uint32_t) : void - + enforceCalculationOnce() : void - + resync() : void - } - - class MovAvg < T, length > <> { - + clear() : void - + write(value : T) : T - + getResult() : T - } - - class DifferentialDrive <> -} - -package "HAL" as hal { - - package "Interfaces" as halInterfaces { - interface IDisplay { - + {abstract} clear() : void - + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void - + {abstract} print(str : const String&) : size_t - + {abstract} print(str : const char[]) : size_t - + {abstract} print(value : uint8_t) : size_t - + {abstract} print(value : uint16_t) : size_t - + {abstract} print(value : uint32_t) : size_t - + {abstract} print(value : int8_t) : size_t - + {abstract} print(value : int16_t) : size_t - + {abstract} print(value : int32_t) : size_t - } - - interface ILineSensors { - + {abstract} init() : void - + {abstract} calibrate() : void - + {abstract} readLine() : int16_t - + {abstract} getSensorValues() : const uint16_t* - + {abstract} isCalibrationSuccessful() : bool - + {abstract} getCalibErrorInfo() const : uint8_t - + {abstract} getNumLineSensors() const : uint8_t - + {abstract} getSensorValueMax() const : uint16_t - } - - interface ILed { - + {abstract} enable(enableIt : bool) : void - } - } - - class Board << namespace >> { - + getDisplay() : IDisplay& - + getLineSensors() : ILineSensors& - + getLedYellow() : ILed& - } -} - -DrivingState *-> SimpleTimer -DrivingState ..> Sound: <> -DrivingState ..> Odometry: <> -DrivingState *-> PIDController -DrivingState *-> MovAvg -DrivingState ..> DifferentialDrive: <> -DrivingState ...> IDisplay: <> -DrivingState ...> ILineSensors: <> -DrivingState ...> ILed: <> -DrivingState ...> Board:: <> - +@startuml + +title Driving State + +package "Application" as appLayer { + + class DrivingState <> { + + {static} getInstance() : DrivingState + + entry() : void + + process(sm : StateMachine&) : void + + exit() : void + } + + class ReadyState <> + + class ParameterSet <> + + class ParameterSets <> { + + {static} getInstance() : ParameterSets& + + choose(setId : uint8_t) : void + + next() : void + + getCurrentSetId() const : uint8_t + + getParameterSet() : ParameterSet& + } + + ParameterSet --+ ParameterSets + ParameterSet "3" <--* ParameterSets + + DrivingState .r.> ReadyState: <> + DrivingState .u.> ParameterSets: <> +} + +package "Service" as serviceLayer { + + class SimpleTimer <> { + + start(duration : uint32_t) : void + + restart() : void + + stop() : void + + isTimeout() : bool + } + + class Sound <> { + + playAlarm() : void + + playBeep() : void + } + + class Odometry <> { + + {static} getInstance() : Odometry& + + process() : void + + getMileageCenter() : uint32_t + + clearMileage() : void + } + + class PIDController < T > <> { + + calculate(setPoint : T, processValue : T) : T + + getPFactor(numerator : T&, denominator : T&) : void + + setPFactor(numerator : T, denominator : T) : void + + getIFactor(numerator : T&, denominator : T&) : void + + setIFactor(numerator : T, denominator : T) : void + + getDFactor(numerator : T&, denominator : T&) : void + + setDFactor(numerator : T, denominator : T) : void + + setLimits(min : T, max : T) : void + + clear() : void + + getSampleTime() : uint32_t + + setSampleTime(sampleTime : uint32_t) : void + + enforceCalculationOnce() : void + + resync() : void + } + + class MovAvg < T, length > <> { + + clear() : void + + write(value : T) : T + + getResult() : T + } + + class DifferentialDrive <> +} + +package "HAL" as hal { + + package "Interfaces" as halInterfaces { + interface IDisplay { + + {abstract} clear() : void + + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void + + {abstract} print(str : const String&) : size_t + + {abstract} print(str : const char[]) : size_t + + {abstract} print(value : uint8_t) : size_t + + {abstract} print(value : uint16_t) : size_t + + {abstract} print(value : uint32_t) : size_t + + {abstract} print(value : int8_t) : size_t + + {abstract} print(value : int16_t) : size_t + + {abstract} print(value : int32_t) : size_t + } + + interface ILineSensors { + + {abstract} init() : void + + {abstract} calibrate() : void + + {abstract} readLine() : int16_t + + {abstract} getSensorValues() : const uint16_t* + + {abstract} isCalibrationSuccessful() : bool + + {abstract} getCalibErrorInfo() const : uint8_t + + {abstract} getNumLineSensors() const : uint8_t + + {abstract} getSensorValueMax() const : uint16_t + } + + interface ILed { + + {abstract} enable(enableIt : bool) : void + } + } + + class Board << namespace >> { + + getDisplay() : IDisplay& + + getLineSensors() : ILineSensors& + + getLedYellow() : ILed& + } +} + +DrivingState *-> SimpleTimer +DrivingState ..> Sound: <> +DrivingState ..> Odometry: <> +DrivingState *-> PIDController +DrivingState *-> MovAvg +DrivingState ..> DifferentialDrive: <> +DrivingState ...> IDisplay: <> +DrivingState ...> ILineSensors: <> +DrivingState ...> ILed: <> +DrivingState ...> Board:: <> + @enduml \ No newline at end of file diff --git a/doc/architecture/uml/LogicalView/LineFollower/ErrorState.plantuml b/doc/architecture/uml/LogicalView/LineFollower/ErrorState.plantuml index 49c0ce7c..810526a6 100644 --- a/doc/architecture/uml/LogicalView/LineFollower/ErrorState.plantuml +++ b/doc/architecture/uml/LogicalView/LineFollower/ErrorState.plantuml @@ -1,52 +1,52 @@ -@startuml - -title Error State - -package "Application" as appLayer { - - class ErrorState <> { - + {static} getInstance() : ErrorState - + entry() : void - + process(sm : StateMachine&) : void - + exit() : void - + setErrorMsg(msg : const String&) : void - } - - class StartupState <> - - ErrorState .r.> StartupState: <> -} - -package "HAL" as hal { - - package "Interfaces" as halInterfaces { - interface IDisplay { - + {abstract} clear() : void - + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void - + {abstract} print(str : const String&) : size_t - + {abstract} print(str : const char[]) : size_t - + {abstract} print(value : uint8_t) : size_t - + {abstract} print(value : uint16_t) : size_t - + {abstract} print(value : uint32_t) : size_t - + {abstract} print(value : int8_t) : size_t - + {abstract} print(value : int16_t) : size_t - + {abstract} print(value : int32_t) : size_t - } - - interface IButton { - + {abstract} isPressed() : bool - + {abstract} waitForRelease() : void - } - } - - class Board << namespace >> { - + getDisplay() : IDisplay& - + getButtonA() : IButton& - } -} - -ErrorState ..> Board: <> -ErrorState ..> IDisplay: <> -ErrorState ..> IButton: <> - +@startuml + +title Error State + +package "Application" as appLayer { + + class ErrorState <> { + + {static} getInstance() : ErrorState + + entry() : void + + process(sm : StateMachine&) : void + + exit() : void + + setErrorMsg(msg : const String&) : void + } + + class StartupState <> + + ErrorState .r.> StartupState: <> +} + +package "HAL" as hal { + + package "Interfaces" as halInterfaces { + interface IDisplay { + + {abstract} clear() : void + + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void + + {abstract} print(str : const String&) : size_t + + {abstract} print(str : const char[]) : size_t + + {abstract} print(value : uint8_t) : size_t + + {abstract} print(value : uint16_t) : size_t + + {abstract} print(value : uint32_t) : size_t + + {abstract} print(value : int8_t) : size_t + + {abstract} print(value : int16_t) : size_t + + {abstract} print(value : int32_t) : size_t + } + + interface IButton { + + {abstract} isPressed() : bool + + {abstract} waitForRelease() : void + } + } + + class Board << namespace >> { + + getDisplay() : IDisplay& + + getButtonA() : IButton& + } +} + +ErrorState ..> Board: <> +ErrorState ..> IDisplay: <> +ErrorState ..> IButton: <> + @enduml \ No newline at end of file diff --git a/doc/architecture/uml/LogicalView/LineFollower/LineSensorsCalibrationState.plantuml b/doc/architecture/uml/LogicalView/LineFollower/LineSensorsCalibrationState.plantuml index 20a50abe..6b3d5cba 100644 --- a/doc/architecture/uml/LogicalView/LineFollower/LineSensorsCalibrationState.plantuml +++ b/doc/architecture/uml/LogicalView/LineFollower/LineSensorsCalibrationState.plantuml @@ -1,91 +1,91 @@ -@startuml - -title Line Sensors Calibration State - -package "Application" as appLayer { - - class LineSensorsCalibrationState <> { - + {static} getInstance() : LineSensorsCalibrationState - + entry() : void - + process(sm : StateMachine&) : void - + exit() : void - } - - class MotorSpeedCalibrationState <> - class ErrorState <> - - LineSensorsCalibrationState ..> MotorSpeedCalibrationState: <> - LineSensorsCalibrationState ..> ErrorState: <> -} - -package "Service" as serviceLayer { - - class SimpleTimer <> - class RelativeEncoder <> - class DifferentialDrve <><> - class Speedometer <><> - - DifferentialDrve ..> Speedometer: <> -} - -package "HAL" as hal { - - package "Interfaces" as halInterfaces { - interface IDisplay { - + {abstract} clear() : void - + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void - + {abstract} print(str : const String&) : size_t - + {abstract} print(str : const char[]) : size_t - + {abstract} print(value : uint8_t) : size_t - + {abstract} print(value : uint16_t) : size_t - + {abstract} print(value : uint32_t) : size_t - + {abstract} print(value : int8_t) : size_t - + {abstract} print(value : int16_t) : size_t - + {abstract} print(value : int32_t) : size_t - } - - interface IMotors { - + {abstract} setSpeeds(leftSpeed : int16_t, rightSpeed : int16_t) : void - + {abstract} getMaxSpeed() : int16_t - } - - interface ILineSensors { - + {abstract} init() : void - + {abstract} calibrate() : void - + {abstract} readLine() : int16_t - + {abstract} getSensorValues() : const uint16_t* - + {abstract} getNumLineSensors() const : uint8_t - + {abstract} getSensorValueMax() const : uint16_t - } - - interface IEncoders { - + {abstract} getCountsLeft() : int16_t - + {abstract} getCountsRight() : int16_t - + {abstract} getCountsAndResetLeft() : int16_t - + {abstract} getCountsAndResetRight() : int16_t - + {abstract} getResolution() const : uint16_t - } - } - - class Board << namespace >> { - + getDisplay() : IDisplay& - + getMotors() : IMotors& - + getLineSensors() : ILineSensors& - } -} - -appLayer -[hidden]-- serviceLayer -serviceLayer -[hidden]-- hal - -LineSensorsCalibrationState ....> IDisplay: <> -LineSensorsCalibrationState ....> ILineSensors: <> -LineSensorsCalibrationState ....> Board: <> -LineSensorsCalibrationState *--> SimpleTimer -LineSensorsCalibrationState *--> RelativeEncoder -LineSensorsCalibrationState ...> DifferentialDrve: <> - -DifferentialDrve ...> IMotors: <> -Speedometer ..> IEncoders: <> -Speedometer ..> IMotors: <> - +@startuml + +title Line Sensors Calibration State + +package "Application" as appLayer { + + class LineSensorsCalibrationState <> { + + {static} getInstance() : LineSensorsCalibrationState + + entry() : void + + process(sm : StateMachine&) : void + + exit() : void + } + + class MotorSpeedCalibrationState <> + class ErrorState <> + + LineSensorsCalibrationState ..> MotorSpeedCalibrationState: <> + LineSensorsCalibrationState ..> ErrorState: <> +} + +package "Service" as serviceLayer { + + class SimpleTimer <> + class RelativeEncoder <> + class DifferentialDrve <><> + class Speedometer <><> + + DifferentialDrve ..> Speedometer: <> +} + +package "HAL" as hal { + + package "Interfaces" as halInterfaces { + interface IDisplay { + + {abstract} clear() : void + + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void + + {abstract} print(str : const String&) : size_t + + {abstract} print(str : const char[]) : size_t + + {abstract} print(value : uint8_t) : size_t + + {abstract} print(value : uint16_t) : size_t + + {abstract} print(value : uint32_t) : size_t + + {abstract} print(value : int8_t) : size_t + + {abstract} print(value : int16_t) : size_t + + {abstract} print(value : int32_t) : size_t + } + + interface IMotors { + + {abstract} setSpeeds(leftSpeed : int16_t, rightSpeed : int16_t) : void + + {abstract} getMaxSpeed() : int16_t + } + + interface ILineSensors { + + {abstract} init() : void + + {abstract} calibrate() : void + + {abstract} readLine() : int16_t + + {abstract} getSensorValues() : const uint16_t* + + {abstract} getNumLineSensors() const : uint8_t + + {abstract} getSensorValueMax() const : uint16_t + } + + interface IEncoders { + + {abstract} getCountsLeft() : int16_t + + {abstract} getCountsRight() : int16_t + + {abstract} getCountsAndResetLeft() : int16_t + + {abstract} getCountsAndResetRight() : int16_t + + {abstract} getResolution() const : uint16_t + } + } + + class Board << namespace >> { + + getDisplay() : IDisplay& + + getMotors() : IMotors& + + getLineSensors() : ILineSensors& + } +} + +appLayer -[hidden]-- serviceLayer +serviceLayer -[hidden]-- hal + +LineSensorsCalibrationState ....> IDisplay: <> +LineSensorsCalibrationState ....> ILineSensors: <> +LineSensorsCalibrationState ....> Board: <> +LineSensorsCalibrationState *--> SimpleTimer +LineSensorsCalibrationState *--> RelativeEncoder +LineSensorsCalibrationState ...> DifferentialDrve: <> + +DifferentialDrve ...> IMotors: <> +Speedometer ..> IEncoders: <> +Speedometer ..> IMotors: <> + @enduml \ No newline at end of file diff --git a/doc/architecture/uml/LogicalView/LineFollower/MotorSpeedCalibrationState.plantuml b/doc/architecture/uml/LogicalView/LineFollower/MotorSpeedCalibrationState.plantuml index 7b9da16e..b9ae03c4 100644 --- a/doc/architecture/uml/LogicalView/LineFollower/MotorSpeedCalibrationState.plantuml +++ b/doc/architecture/uml/LogicalView/LineFollower/MotorSpeedCalibrationState.plantuml @@ -1,85 +1,85 @@ -@startuml - -title Motor Speed Calibration State - -package "Application" as appLayer { - - class MotorSpeedCalibrationState <> { - + {static} getInstance() : MotorSpeedCalibrationState - + entry() : void - + process(sm : StateMachine&) : void - + exit() : void - } - - class ReadyState <> - class ErrorState <> - - MotorSpeedCalibrationState ..> ReadyState: <> - MotorSpeedCalibrationState ..> ErrorState: <> -} - -package "Service" as serviceLayer { - - class SimpleTimer <> - class RelativeEncoder <> - class DifferentialDrve <> -} - -package "HAL" as hal { - - package "Interfaces" as halInterfaces { - interface IDisplay { - + {abstract} clear() : void - + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void - + {abstract} print(str : const String&) : size_t - + {abstract} print(str : const char[]) : size_t - + {abstract} print(value : uint8_t) : size_t - + {abstract} print(value : uint16_t) : size_t - + {abstract} print(value : uint32_t) : size_t - + {abstract} print(value : int8_t) : size_t - + {abstract} print(value : int16_t) : size_t - + {abstract} print(value : int32_t) : size_t - } - - interface IMotors { - + {abstract} setSpeeds(leftSpeed : int16_t, rightSpeed : int16_t) : void - + {abstract} getMaxSpeed() : int16_t - } - - interface ILineSensors { - + {abstract} init() : void - + {abstract} calibrate() : void - + {abstract} readLine() : int16_t - + {abstract} getSensorValues() : const uint16_t* - + {abstract} getNumLineSensors() const : uint8_t - + {abstract} getSensorValueMax() const : uint16_t - } - - interface ISettings { - + {abstract} init() : void - + {abstract} getMaxSpeed() : int16_t - + {abstract} setMaxSpeed(maxSpeed : int16_t) : void - } - } - - class Board << namespace >> { - + getDisplay() : IDisplay& - + getMotors() : IMotors& - + getLineSensors() : ILineSensors& - + getSettings() : ISettings& - } -} - -appLayer -[hidden]-- serviceLayer -serviceLayer -[hidden]-- hal - -MotorSpeedCalibrationState ....> IDisplay: <> -MotorSpeedCalibrationState ....> IMotors: <> -MotorSpeedCalibrationState ....> ILineSensors: <> -MotorSpeedCalibrationState ....> ISettings: <> -MotorSpeedCalibrationState ....> Board: <> -MotorSpeedCalibrationState *--> SimpleTimer -MotorSpeedCalibrationState *--> RelativeEncoder -MotorSpeedCalibrationState ...> DifferentialDrve: <> - +@startuml + +title Motor Speed Calibration State + +package "Application" as appLayer { + + class MotorSpeedCalibrationState <> { + + {static} getInstance() : MotorSpeedCalibrationState + + entry() : void + + process(sm : StateMachine&) : void + + exit() : void + } + + class ReadyState <> + class ErrorState <> + + MotorSpeedCalibrationState ..> ReadyState: <> + MotorSpeedCalibrationState ..> ErrorState: <> +} + +package "Service" as serviceLayer { + + class SimpleTimer <> + class RelativeEncoder <> + class DifferentialDrve <> +} + +package "HAL" as hal { + + package "Interfaces" as halInterfaces { + interface IDisplay { + + {abstract} clear() : void + + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void + + {abstract} print(str : const String&) : size_t + + {abstract} print(str : const char[]) : size_t + + {abstract} print(value : uint8_t) : size_t + + {abstract} print(value : uint16_t) : size_t + + {abstract} print(value : uint32_t) : size_t + + {abstract} print(value : int8_t) : size_t + + {abstract} print(value : int16_t) : size_t + + {abstract} print(value : int32_t) : size_t + } + + interface IMotors { + + {abstract} setSpeeds(leftSpeed : int16_t, rightSpeed : int16_t) : void + + {abstract} getMaxSpeed() : int16_t + } + + interface ILineSensors { + + {abstract} init() : void + + {abstract} calibrate() : void + + {abstract} readLine() : int16_t + + {abstract} getSensorValues() : const uint16_t* + + {abstract} getNumLineSensors() const : uint8_t + + {abstract} getSensorValueMax() const : uint16_t + } + + interface ISettings { + + {abstract} init() : void + + {abstract} getMaxSpeed() : int16_t + + {abstract} setMaxSpeed(maxSpeed : int16_t) : void + } + } + + class Board << namespace >> { + + getDisplay() : IDisplay& + + getMotors() : IMotors& + + getLineSensors() : ILineSensors& + + getSettings() : ISettings& + } +} + +appLayer -[hidden]-- serviceLayer +serviceLayer -[hidden]-- hal + +MotorSpeedCalibrationState ....> IDisplay: <> +MotorSpeedCalibrationState ....> IMotors: <> +MotorSpeedCalibrationState ....> ILineSensors: <> +MotorSpeedCalibrationState ....> ISettings: <> +MotorSpeedCalibrationState ....> Board: <> +MotorSpeedCalibrationState *--> SimpleTimer +MotorSpeedCalibrationState *--> RelativeEncoder +MotorSpeedCalibrationState ...> DifferentialDrve: <> + @enduml \ No newline at end of file diff --git a/doc/architecture/uml/LogicalView/LineFollower/ReadyState.plantuml b/doc/architecture/uml/LogicalView/LineFollower/ReadyState.plantuml index cdda8369..4ead8912 100644 --- a/doc/architecture/uml/LogicalView/LineFollower/ReadyState.plantuml +++ b/doc/architecture/uml/LogicalView/LineFollower/ReadyState.plantuml @@ -1,63 +1,63 @@ -@startuml - -title Ready State - -package "Application" as appLayer { - - class ReadyState <> { - + {static} getInstance() : ReadyState - + entry() : void - + process(sm : StateMachine&) : void - + exit() : void - + setLapTIme(lapTime : uint32_t) void - } - - class ReleaseTrackState <> - - ReadyState .r.> ReleaseTrackState: <> -} - -package "HAL" as hal { - - package "Interfaces" as halInterfaces { - interface IDisplay { - + {abstract} clear() : void - + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void - + {abstract} print(str : const String&) : size_t - + {abstract} print(str : const char[]) : size_t - + {abstract} print(value : uint8_t) : size_t - + {abstract} print(value : uint16_t) : size_t - + {abstract} print(value : uint32_t) : size_t - + {abstract} print(value : int8_t) : size_t - + {abstract} print(value : int16_t) : size_t - + {abstract} print(value : int32_t) : size_t - } - - interface IButton { - + {abstract} isPressed() : bool - + {abstract} waitForRelease() : void - } - - interface ILineSensors { - + {abstract} init() : void - + {abstract} calibrate() : void - + {abstract} readLine() : int16_t - + {abstract} getSensorValues() : const uint16_t* - + {abstract} getNumLineSensors() const : uint8_t - + {abstract} getSensorValueMax() const : uint16_t - } - } - - class Board << namespace >> { - + getDisplay() : IDisplay& - + getButtonA() : IButton& - + getLineSensors() : ILineSensors& - } -} - -ReadyState ..> IDisplay: <> -ReadyState ..> IButton: <> -ReadyState ..> ILineSensors: <> -ReadyState ..> Board:: <> - +@startuml + +title Ready State + +package "Application" as appLayer { + + class ReadyState <> { + + {static} getInstance() : ReadyState + + entry() : void + + process(sm : StateMachine&) : void + + exit() : void + + setLapTIme(lapTime : uint32_t) void + } + + class ReleaseTrackState <> + + ReadyState .r.> ReleaseTrackState: <> +} + +package "HAL" as hal { + + package "Interfaces" as halInterfaces { + interface IDisplay { + + {abstract} clear() : void + + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void + + {abstract} print(str : const String&) : size_t + + {abstract} print(str : const char[]) : size_t + + {abstract} print(value : uint8_t) : size_t + + {abstract} print(value : uint16_t) : size_t + + {abstract} print(value : uint32_t) : size_t + + {abstract} print(value : int8_t) : size_t + + {abstract} print(value : int16_t) : size_t + + {abstract} print(value : int32_t) : size_t + } + + interface IButton { + + {abstract} isPressed() : bool + + {abstract} waitForRelease() : void + } + + interface ILineSensors { + + {abstract} init() : void + + {abstract} calibrate() : void + + {abstract} readLine() : int16_t + + {abstract} getSensorValues() : const uint16_t* + + {abstract} getNumLineSensors() const : uint8_t + + {abstract} getSensorValueMax() const : uint16_t + } + } + + class Board << namespace >> { + + getDisplay() : IDisplay& + + getButtonA() : IButton& + + getLineSensors() : ILineSensors& + } +} + +ReadyState ..> IDisplay: <> +ReadyState ..> IButton: <> +ReadyState ..> ILineSensors: <> +ReadyState ..> Board:: <> + @enduml \ No newline at end of file diff --git a/doc/architecture/uml/LogicalView/LineFollower/ReleaseTrackState.plantuml b/doc/architecture/uml/LogicalView/LineFollower/ReleaseTrackState.plantuml index d51f67a5..162f9868 100644 --- a/doc/architecture/uml/LogicalView/LineFollower/ReleaseTrackState.plantuml +++ b/doc/architecture/uml/LogicalView/LineFollower/ReleaseTrackState.plantuml @@ -1,76 +1,76 @@ -@startuml - -title Release Track State - -package "Application" as appLayer { - - class ReleaseTrackState <> { - + {static} getInstance() : ReleaseTrackState - + entry() : void - + process(sm : StateMachine&) : void - + exit() : void - } - - class DrivingState <> - - class ParameterSet <> - - class ParameterSets <> { - + {static} getInstance() : ParameterSets& - + choose(setId : uint8_t) : void - + next() : void - + getCurrentSetId() const : uint8_t - + getParameterSet() : ParameterSet& - } - - ParameterSet --+ ParameterSets - ParameterSet "3" <--* ParameterSets - - ReleaseTrackState .r.> DrivingState: <> - ReleaseTrackState .u.> ParameterSets: <> -} - -package "Service" as serviceLayer { - - class SimpleTimer <> { - + start(duration : uint32_t) : void - + restart() : void - + stop() : void - + isTimeout() : bool - } -} - -package "HAL" as hal { - - package "Interfaces" as halInterfaces { - interface IDisplay { - + {abstract} clear() : void - + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void - + {abstract} print(str : const String&) : size_t - + {abstract} print(str : const char[]) : size_t - + {abstract} print(value : uint8_t) : size_t - + {abstract} print(value : uint16_t) : size_t - + {abstract} print(value : uint32_t) : size_t - + {abstract} print(value : int8_t) : size_t - + {abstract} print(value : int16_t) : size_t - + {abstract} print(value : int32_t) : size_t - } - - interface IButton { - + {abstract} isPressed() : bool - + {abstract} waitForRelease() : void - } - } - - class Board << namespace >> { - + getDisplay() : IDisplay& - + getButtonA() : IButton& - } -} - -ReleaseTrackState *--> SimpleTimer -ReleaseTrackState ...> IDisplay: <> -ReleaseTrackState ...> IButton: <> -ReleaseTrackState ...> Board: <> - +@startuml + +title Release Track State + +package "Application" as appLayer { + + class ReleaseTrackState <> { + + {static} getInstance() : ReleaseTrackState + + entry() : void + + process(sm : StateMachine&) : void + + exit() : void + } + + class DrivingState <> + + class ParameterSet <> + + class ParameterSets <> { + + {static} getInstance() : ParameterSets& + + choose(setId : uint8_t) : void + + next() : void + + getCurrentSetId() const : uint8_t + + getParameterSet() : ParameterSet& + } + + ParameterSet --+ ParameterSets + ParameterSet "3" <--* ParameterSets + + ReleaseTrackState .r.> DrivingState: <> + ReleaseTrackState .u.> ParameterSets: <> +} + +package "Service" as serviceLayer { + + class SimpleTimer <> { + + start(duration : uint32_t) : void + + restart() : void + + stop() : void + + isTimeout() : bool + } +} + +package "HAL" as hal { + + package "Interfaces" as halInterfaces { + interface IDisplay { + + {abstract} clear() : void + + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void + + {abstract} print(str : const String&) : size_t + + {abstract} print(str : const char[]) : size_t + + {abstract} print(value : uint8_t) : size_t + + {abstract} print(value : uint16_t) : size_t + + {abstract} print(value : uint32_t) : size_t + + {abstract} print(value : int8_t) : size_t + + {abstract} print(value : int16_t) : size_t + + {abstract} print(value : int32_t) : size_t + } + + interface IButton { + + {abstract} isPressed() : bool + + {abstract} waitForRelease() : void + } + } + + class Board << namespace >> { + + getDisplay() : IDisplay& + + getButtonA() : IButton& + } +} + +ReleaseTrackState *--> SimpleTimer +ReleaseTrackState ...> IDisplay: <> +ReleaseTrackState ...> IButton: <> +ReleaseTrackState ...> Board: <> + @enduml \ No newline at end of file diff --git a/doc/architecture/uml/LogicalView/LineFollower/StartupState.plantuml b/doc/architecture/uml/LogicalView/LineFollower/StartupState.plantuml index c1d547ab..ded4686e 100644 --- a/doc/architecture/uml/LogicalView/LineFollower/StartupState.plantuml +++ b/doc/architecture/uml/LogicalView/LineFollower/StartupState.plantuml @@ -1,68 +1,68 @@ -@startuml - -title Startup State - -package "Application" as appLayer { - - class StartupState <> { - + {static} getInstance() : StartupState - + entry() : void - + process(sm : StateMachine&) : void - + exit() : void - } - - class MotorSpeedCalibrationState <> - class LineSensorsCalibrationState <> - - StartupState .l.> MotorSpeedCalibrationState: <> - StartupState .r.> LineSensorsCalibrationState: <> -} - -package "Service" as serviceLayer { - - class DifferentialDrve <> -} - -package "HAL" as hal { - - package "Interfaces" as halInterfaces { - interface IDisplay { - + {abstract} clear() : void - + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void - + {abstract} print(str : const String&) : size_t - + {abstract} print(str : const char[]) : size_t - + {abstract} print(value : uint8_t) : size_t - + {abstract} print(value : uint16_t) : size_t - + {abstract} print(value : uint32_t) : size_t - + {abstract} print(value : int8_t) : size_t - + {abstract} print(value : int16_t) : size_t - + {abstract} print(value : int32_t) : size_t - } - - interface IButton { - + {abstract} isPressed() : bool - + {abstract} waitForRelease() : void - } - - interface ISettings { - + {abstract} init() : void - + {abstract} getMaxSpeed() : int16_t - + {abstract} setMaxSpeed(maxSpeed : int16_t) : void - } - } - - class Board << namespace >> { - + getDisplay() : IDisplay& - + getButtonA() : IButton& - + getButtonB() : IButton& - + getSettings() : ISettings& - } -} - -StartupState ..> DifferentialDrve: <> -StartupState ...> IDisplay: <> -StartupState ...> IButton: <> -StartupState ...> ISettings: <> -StartupState ...> Board:: <> - +@startuml + +title Startup State + +package "Application" as appLayer { + + class StartupState <> { + + {static} getInstance() : StartupState + + entry() : void + + process(sm : StateMachine&) : void + + exit() : void + } + + class MotorSpeedCalibrationState <> + class LineSensorsCalibrationState <> + + StartupState .l.> MotorSpeedCalibrationState: <> + StartupState .r.> LineSensorsCalibrationState: <> +} + +package "Service" as serviceLayer { + + class DifferentialDrve <> +} + +package "HAL" as hal { + + package "Interfaces" as halInterfaces { + interface IDisplay { + + {abstract} clear() : void + + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void + + {abstract} print(str : const String&) : size_t + + {abstract} print(str : const char[]) : size_t + + {abstract} print(value : uint8_t) : size_t + + {abstract} print(value : uint16_t) : size_t + + {abstract} print(value : uint32_t) : size_t + + {abstract} print(value : int8_t) : size_t + + {abstract} print(value : int16_t) : size_t + + {abstract} print(value : int32_t) : size_t + } + + interface IButton { + + {abstract} isPressed() : bool + + {abstract} waitForRelease() : void + } + + interface ISettings { + + {abstract} init() : void + + {abstract} getMaxSpeed() : int16_t + + {abstract} setMaxSpeed(maxSpeed : int16_t) : void + } + } + + class Board << namespace >> { + + getDisplay() : IDisplay& + + getButtonA() : IButton& + + getButtonB() : IButton& + + getSettings() : ISettings& + } +} + +StartupState ..> DifferentialDrve: <> +StartupState ...> IDisplay: <> +StartupState ...> IButton: <> +StartupState ...> ISettings: <> +StartupState ...> Board:: <> + @enduml \ No newline at end of file diff --git a/doc/architecture/uml/LogicalView/Odometry.plantuml b/doc/architecture/uml/LogicalView/Odometry.plantuml index 45cd4b37..e6322afb 100644 --- a/doc/architecture/uml/LogicalView/Odometry.plantuml +++ b/doc/architecture/uml/LogicalView/Odometry.plantuml @@ -1,27 +1,27 @@ -@startuml - -package "Service" { - class "Odometry" as odometry <> - class "RelativeEncoders" as relativeEncoders <> - - note left of odometry - Provides the: - * position in mm - * orientation in mrad - * mileage in mm - end note - - odometry --> relativeEncoders -} - -package "HAL" { - class "IEncoders" as iEncoders - class "Encoders" as encoders - - iEncoders <|.. encoders: <> -} - -odometry ...> iEncoders: <> -relativeEncoders ..> iEncoders: <> - -@enduml +@startuml + +package "Service" { + class "Odometry" as odometry <> + class "RelativeEncoders" as relativeEncoders <> + + note left of odometry + Provides the: + * position in mm + * orientation in mrad + * mileage in mm + end note + + odometry --> relativeEncoders +} + +package "HAL" { + class "IEncoders" as iEncoders + class "Encoders" as encoders + + iEncoders <|.. encoders: <> +} + +odometry ...> iEncoders: <> +relativeEncoders ..> iEncoders: <> + +@enduml diff --git a/doc/architecture/uml/LogicalView/RemoteControl/Application.plantuml b/doc/architecture/uml/LogicalView/RemoteControl/Application.plantuml index 69b50af2..c70036fd 100644 --- a/doc/architecture/uml/LogicalView/RemoteControl/Application.plantuml +++ b/doc/architecture/uml/LogicalView/RemoteControl/Application.plantuml @@ -1,91 +1,91 @@ -@startuml RemoteControl Application - -title Application - -package "Application" as appLayer { - - class App <
> { - + setup() : void - + loop() : void - } - - note left of App - The program entry point. - end note - - class StateMachine <> { - + setState(state : IState*) : void - + getState() : IState* - + process() : void - } - - note left of StateMachine - The state machine executes always one - state exclusive. It is responsible to - enter/leave/process a state. - end note - - interface IState { - + {abstract} entry() : void - + {abstract} process(sm : StateMachine&) : void - + {abstract} exit() : void - } - - note left of IState - Defines the abstract state interface, - which forces every inherited class - to realize it. - end note - - class StartupState <> - class MotorSpeedCalibrationState <> - class LineSensorsCalibrationState <> - class ErrorState <> - class RemoteCtrlState <> - - note bottom of StartupState - The system starts up and shows - the application name on the display. - end note - - note bottom of MotorSpeedCalibrationState - The robot drives with full speed forward - and with full speed backwar to determine - the max speed in steps/s. The slowest - motor is considered! - end note - - note bottom of LineSensorsCalibrationState - The robot turns several times the - line sensors over the track for - calibration. - end note - - note bottom of ErrorState - Error information is shown on display. - Confirmation from operator is requested. - end note - - note bottom of RemoteCtrlState - The system drives according to - the commands sent by the operator. - Every calibration can be triggered via remote. - end note -} - -note top of appLayer - Hint: See the application state behaviour - in the corresponding state diagram. -end note - -App *--> StateMachine - -StateMachine o--> "0..1" IState - -IState <|.. StartupState: <> -IState <|.... MotorSpeedCalibrationState: <> -IState <|.. LineSensorsCalibrationState: <> -IState <|.... ErrorState: <> -IState <|.... RemoteCtrlState: <> - +@startuml RemoteControl Application + +title Application + +package "Application" as appLayer { + + class App <
> { + + setup() : void + + loop() : void + } + + note left of App + The program entry point. + end note + + class StateMachine <> { + + setState(state : IState*) : void + + getState() : IState* + + process() : void + } + + note left of StateMachine + The state machine executes always one + state exclusive. It is responsible to + enter/leave/process a state. + end note + + interface IState { + + {abstract} entry() : void + + {abstract} process(sm : StateMachine&) : void + + {abstract} exit() : void + } + + note left of IState + Defines the abstract state interface, + which forces every inherited class + to realize it. + end note + + class StartupState <> + class MotorSpeedCalibrationState <> + class LineSensorsCalibrationState <> + class ErrorState <> + class RemoteCtrlState <> + + note bottom of StartupState + The system starts up and shows + the application name on the display. + end note + + note bottom of MotorSpeedCalibrationState + The robot drives with full speed forward + and with full speed backwar to determine + the max speed in steps/s. The slowest + motor is considered! + end note + + note bottom of LineSensorsCalibrationState + The robot turns several times the + line sensors over the track for + calibration. + end note + + note bottom of ErrorState + Error information is shown on display. + Confirmation from operator is requested. + end note + + note bottom of RemoteCtrlState + The system drives according to + the commands sent by the operator. + Every calibration can be triggered via remote. + end note +} + +note top of appLayer + Hint: See the application state behaviour + in the corresponding state diagram. +end note + +App *--> StateMachine + +StateMachine o--> "0..1" IState + +IState <|.. StartupState: <> +IState <|.... MotorSpeedCalibrationState: <> +IState <|.. LineSensorsCalibrationState: <> +IState <|.... ErrorState: <> +IState <|.... RemoteCtrlState: <> + @enduml \ No newline at end of file diff --git a/doc/architecture/uml/LogicalView/RemoteControl/ErrorState.plantuml b/doc/architecture/uml/LogicalView/RemoteControl/ErrorState.plantuml index 17a56d68..0bb537ad 100644 --- a/doc/architecture/uml/LogicalView/RemoteControl/ErrorState.plantuml +++ b/doc/architecture/uml/LogicalView/RemoteControl/ErrorState.plantuml @@ -1,56 +1,56 @@ -@startuml RemoteControl ErrorState - -title Error State - -package "Application" as appLayer { - - class ErrorState <> { - + {static} getInstance() : ErrorState - + entry() : void - + process(sm : StateMachine&) : void - + exit() : void - + setErrorMsg(msg : const String&) : void - } - - class MotorSpeedCalibrationState <> - class RemoteCtrlState <> - class ReadyState <> - - ErrorState .r.> MotorSpeedCalibrationState: <> - ErrorState .r.> RemoteCtrlState: <> - ErrorState .r.> ReadyState: <> -} - -package "HAL" as hal { - - package "Interfaces" as halInterfaces { - interface IDisplay { - + {abstract} clear() : void - + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void - + {abstract} print(str : const String&) : size_t - + {abstract} print(str : const char[]) : size_t - + {abstract} print(value : uint8_t) : size_t - + {abstract} print(value : uint16_t) : size_t - + {abstract} print(value : uint32_t) : size_t - + {abstract} print(value : int8_t) : size_t - + {abstract} print(value : int16_t) : size_t - + {abstract} print(value : int32_t) : size_t - } - - interface IButton { - + {abstract} isPressed() : bool - + {abstract} waitForRelease() : void - } - } - - class Board << namespace >> { - + getDisplay() : IDisplay& - + getButtonA() : IButton& - } -} - -ErrorState ..> Board: <> -ErrorState ..> IDisplay: <> -ErrorState ..> IButton: <> - +@startuml RemoteControl ErrorState + +title Error State + +package "Application" as appLayer { + + class ErrorState <> { + + {static} getInstance() : ErrorState + + entry() : void + + process(sm : StateMachine&) : void + + exit() : void + + setErrorMsg(msg : const String&) : void + } + + class MotorSpeedCalibrationState <> + class RemoteCtrlState <> + class ReadyState <> + + ErrorState .r.> MotorSpeedCalibrationState: <> + ErrorState .r.> RemoteCtrlState: <> + ErrorState .r.> ReadyState: <> +} + +package "HAL" as hal { + + package "Interfaces" as halInterfaces { + interface IDisplay { + + {abstract} clear() : void + + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void + + {abstract} print(str : const String&) : size_t + + {abstract} print(str : const char[]) : size_t + + {abstract} print(value : uint8_t) : size_t + + {abstract} print(value : uint16_t) : size_t + + {abstract} print(value : uint32_t) : size_t + + {abstract} print(value : int8_t) : size_t + + {abstract} print(value : int16_t) : size_t + + {abstract} print(value : int32_t) : size_t + } + + interface IButton { + + {abstract} isPressed() : bool + + {abstract} waitForRelease() : void + } + } + + class Board << namespace >> { + + getDisplay() : IDisplay& + + getButtonA() : IButton& + } +} + +ErrorState ..> Board: <> +ErrorState ..> IDisplay: <> +ErrorState ..> IButton: <> + @enduml \ No newline at end of file diff --git a/doc/architecture/uml/LogicalView/RemoteControl/LineSensorsCalibrationState.plantuml b/doc/architecture/uml/LogicalView/RemoteControl/LineSensorsCalibrationState.plantuml index 895bff76..462f0299 100644 --- a/doc/architecture/uml/LogicalView/RemoteControl/LineSensorsCalibrationState.plantuml +++ b/doc/architecture/uml/LogicalView/RemoteControl/LineSensorsCalibrationState.plantuml @@ -1,91 +1,91 @@ -@startuml RemoteControl LineSensorsCalibrationState - -title Line Sensors Calibration State - -package "Application" as appLayer { - - class LineSensorsCalibrationState <> { - + {static} getInstance() : LineSensorsCalibrationState - + entry() : void - + process(sm : StateMachine&) : void - + exit() : void - } - - class RemoteCtrlState <> - class ErrorState <> - - LineSensorsCalibrationState ..> RemoteCtrlState: <> - LineSensorsCalibrationState ..> ErrorState: <> -} - -package "Service" as serviceLayer { - - class SimpleTimer <> - class RelativeEncoder <> - class DifferentialDrve <><> - class Speedometer <><> - - DifferentialDrve ..> Speedometer: <> -} - -package "HAL" as hal { - - package "Interfaces" as halInterfaces { - interface IDisplay { - + {abstract} clear() : void - + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void - + {abstract} print(str : const String&) : size_t - + {abstract} print(str : const char[]) : size_t - + {abstract} print(value : uint8_t) : size_t - + {abstract} print(value : uint16_t) : size_t - + {abstract} print(value : uint32_t) : size_t - + {abstract} print(value : int8_t) : size_t - + {abstract} print(value : int16_t) : size_t - + {abstract} print(value : int32_t) : size_t - } - - interface IMotors { - + {abstract} setSpeeds(leftSpeed : int16_t, rightSpeed : int16_t) : void - + {abstract} getMaxSpeed() : int16_t - } - - interface ILineSensors { - + {abstract} init() : void - + {abstract} calibrate() : void - + {abstract} readLine() : int16_t - + {abstract} getSensorValues() : const uint16_t* - + {abstract} getNumLineSensors() const : uint8_t - + {abstract} getSensorValueMax() const : uint16_t - } - - interface IEncoders { - + {abstract} getCountsLeft() : int16_t - + {abstract} getCountsRight() : int16_t - + {abstract} getCountsAndResetLeft() : int16_t - + {abstract} getCountsAndResetRight() : int16_t - + {abstract} getResolution() const : uint16_t - } - } - - class Board << namespace >> { - + getDisplay() : IDisplay& - + getMotors() : IMotors& - + getLineSensors() : ILineSensors& - } -} - -appLayer -[hidden]-- serviceLayer -serviceLayer -[hidden]-- hal - -LineSensorsCalibrationState ....> IDisplay: <> -LineSensorsCalibrationState ....> ILineSensors: <> -LineSensorsCalibrationState ....> Board: <> -LineSensorsCalibrationState *--> SimpleTimer -LineSensorsCalibrationState *--> RelativeEncoder -LineSensorsCalibrationState ...> DifferentialDrve: <> - -DifferentialDrve ...> IMotors: <> -Speedometer ..> IEncoders: <> -Speedometer ..> IMotors: <> - +@startuml RemoteControl LineSensorsCalibrationState + +title Line Sensors Calibration State + +package "Application" as appLayer { + + class LineSensorsCalibrationState <> { + + {static} getInstance() : LineSensorsCalibrationState + + entry() : void + + process(sm : StateMachine&) : void + + exit() : void + } + + class RemoteCtrlState <> + class ErrorState <> + + LineSensorsCalibrationState ..> RemoteCtrlState: <> + LineSensorsCalibrationState ..> ErrorState: <> +} + +package "Service" as serviceLayer { + + class SimpleTimer <> + class RelativeEncoder <> + class DifferentialDrve <><> + class Speedometer <><> + + DifferentialDrve ..> Speedometer: <> +} + +package "HAL" as hal { + + package "Interfaces" as halInterfaces { + interface IDisplay { + + {abstract} clear() : void + + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void + + {abstract} print(str : const String&) : size_t + + {abstract} print(str : const char[]) : size_t + + {abstract} print(value : uint8_t) : size_t + + {abstract} print(value : uint16_t) : size_t + + {abstract} print(value : uint32_t) : size_t + + {abstract} print(value : int8_t) : size_t + + {abstract} print(value : int16_t) : size_t + + {abstract} print(value : int32_t) : size_t + } + + interface IMotors { + + {abstract} setSpeeds(leftSpeed : int16_t, rightSpeed : int16_t) : void + + {abstract} getMaxSpeed() : int16_t + } + + interface ILineSensors { + + {abstract} init() : void + + {abstract} calibrate() : void + + {abstract} readLine() : int16_t + + {abstract} getSensorValues() : const uint16_t* + + {abstract} getNumLineSensors() const : uint8_t + + {abstract} getSensorValueMax() const : uint16_t + } + + interface IEncoders { + + {abstract} getCountsLeft() : int16_t + + {abstract} getCountsRight() : int16_t + + {abstract} getCountsAndResetLeft() : int16_t + + {abstract} getCountsAndResetRight() : int16_t + + {abstract} getResolution() const : uint16_t + } + } + + class Board << namespace >> { + + getDisplay() : IDisplay& + + getMotors() : IMotors& + + getLineSensors() : ILineSensors& + } +} + +appLayer -[hidden]-- serviceLayer +serviceLayer -[hidden]-- hal + +LineSensorsCalibrationState ....> IDisplay: <> +LineSensorsCalibrationState ....> ILineSensors: <> +LineSensorsCalibrationState ....> Board: <> +LineSensorsCalibrationState *--> SimpleTimer +LineSensorsCalibrationState *--> RelativeEncoder +LineSensorsCalibrationState ...> DifferentialDrve: <> + +DifferentialDrve ...> IMotors: <> +Speedometer ..> IEncoders: <> +Speedometer ..> IMotors: <> + @enduml \ No newline at end of file diff --git a/doc/architecture/uml/LogicalView/RemoteControl/MotorSpeedCalibrationState.plantuml b/doc/architecture/uml/LogicalView/RemoteControl/MotorSpeedCalibrationState.plantuml index ea76552f..243fe54f 100644 --- a/doc/architecture/uml/LogicalView/RemoteControl/MotorSpeedCalibrationState.plantuml +++ b/doc/architecture/uml/LogicalView/RemoteControl/MotorSpeedCalibrationState.plantuml @@ -1,67 +1,67 @@ -@startuml RemoteControl MotorSpeedCalibrationState - -title Motor Speed Calibration State - -package "Application" as appLayer { - - class MotorSpeedCalibrationState <> { - + {static} getInstance() : MotorSpeedCalibrationState - + entry() : void - + process(sm : StateMachine&) : void - + exit() : void - } - - class RemoteCtrlState <> - class ErrorState <> - - MotorSpeedCalibrationState ..> RemoteCtrlState: <> - MotorSpeedCalibrationState ..> ErrorState: <> -} - -package "Service" as serviceLayer { - - class SimpleTimer <> - class RelativeEncoder <> - class DifferentialDrve <> -} - -package "HAL" as hal { - - package "Interfaces" as halInterfaces { - interface IDisplay { - + {abstract} clear() : void - + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void - + {abstract} print(str : const String&) : size_t - + {abstract} print(str : const char[]) : size_t - + {abstract} print(value : uint8_t) : size_t - + {abstract} print(value : uint16_t) : size_t - + {abstract} print(value : uint32_t) : size_t - + {abstract} print(value : int8_t) : size_t - + {abstract} print(value : int16_t) : size_t - + {abstract} print(value : int32_t) : size_t - } - - interface IMotors { - + {abstract} setSpeeds(leftSpeed : int16_t, rightSpeed : int16_t) : void - + {abstract} getMaxSpeed() : int16_t - } - - } - - class Board << namespace >> { - + getDisplay() : IDisplay& - + getMotors() : IMotors& - } -} - -appLayer -[hidden]-- serviceLayer -serviceLayer -[hidden]-- hal - -MotorSpeedCalibrationState ....> IDisplay: <> -MotorSpeedCalibrationState ....> IMotors: <> -MotorSpeedCalibrationState ....> Board: <> -MotorSpeedCalibrationState *--> SimpleTimer -MotorSpeedCalibrationState *--> RelativeEncoder -MotorSpeedCalibrationState ...> DifferentialDrve: <> - +@startuml RemoteControl MotorSpeedCalibrationState + +title Motor Speed Calibration State + +package "Application" as appLayer { + + class MotorSpeedCalibrationState <> { + + {static} getInstance() : MotorSpeedCalibrationState + + entry() : void + + process(sm : StateMachine&) : void + + exit() : void + } + + class RemoteCtrlState <> + class ErrorState <> + + MotorSpeedCalibrationState ..> RemoteCtrlState: <> + MotorSpeedCalibrationState ..> ErrorState: <> +} + +package "Service" as serviceLayer { + + class SimpleTimer <> + class RelativeEncoder <> + class DifferentialDrve <> +} + +package "HAL" as hal { + + package "Interfaces" as halInterfaces { + interface IDisplay { + + {abstract} clear() : void + + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void + + {abstract} print(str : const String&) : size_t + + {abstract} print(str : const char[]) : size_t + + {abstract} print(value : uint8_t) : size_t + + {abstract} print(value : uint16_t) : size_t + + {abstract} print(value : uint32_t) : size_t + + {abstract} print(value : int8_t) : size_t + + {abstract} print(value : int16_t) : size_t + + {abstract} print(value : int32_t) : size_t + } + + interface IMotors { + + {abstract} setSpeeds(leftSpeed : int16_t, rightSpeed : int16_t) : void + + {abstract} getMaxSpeed() : int16_t + } + + } + + class Board << namespace >> { + + getDisplay() : IDisplay& + + getMotors() : IMotors& + } +} + +appLayer -[hidden]-- serviceLayer +serviceLayer -[hidden]-- hal + +MotorSpeedCalibrationState ....> IDisplay: <> +MotorSpeedCalibrationState ....> IMotors: <> +MotorSpeedCalibrationState ....> Board: <> +MotorSpeedCalibrationState *--> SimpleTimer +MotorSpeedCalibrationState *--> RelativeEncoder +MotorSpeedCalibrationState ...> DifferentialDrve: <> + @enduml \ No newline at end of file diff --git a/doc/architecture/uml/LogicalView/RemoteControl/RemoteCtrlState.plantuml b/doc/architecture/uml/LogicalView/RemoteControl/RemoteCtrlState.plantuml index 88c09644..4e7c2c28 100644 --- a/doc/architecture/uml/LogicalView/RemoteControl/RemoteCtrlState.plantuml +++ b/doc/architecture/uml/LogicalView/RemoteControl/RemoteCtrlState.plantuml @@ -1,93 +1,93 @@ -@startuml RemoteControl RemoteCtrlState - -title Remote Control State - -package "Application" as appLayer { - - class RemoteCtrlState <> { - + {static} getInstance() : RemoteCtrlState - + entry() : void - + process(sm : StateMachine&) : void - + exit() : void - } - - class MotorSpeedCalibrationState <> - class LineSensorsCalibrationState <> - - RemoteCtrlState .l.> MotorSpeedCalibrationState: <> - RemoteCtrlState .r.> LineSensorsCalibrationState: <> -} - -package "Service" as serviceLayer { - - class SimpleTimer <> { - + start(duration : uint32_t) : void - + restart() : void - + stop() : void - + isTimeout() : bool - } -} - -package "HAL" as hal { - - package "Interfaces" as halInterfaces { - interface IDisplay { - + {abstract} clear() : void - + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void - + {abstract} print(str : const String&) : size_t - + {abstract} print(str : const char[]) : size_t - + {abstract} print(value : uint8_t) : size_t - + {abstract} print(value : uint16_t) : size_t - + {abstract} print(value : uint32_t) : size_t - + {abstract} print(value : int8_t) : size_t - + {abstract} print(value : int16_t) : size_t - + {abstract} print(value : int32_t) : size_t - } - - interface IMotors { - + {abstract} setSpeeds(leftSpeed : int16_t, rightSpeed : int16_t) : void - + {abstract} getMaxSpeed() : int16_t - } - - interface ILineSensors { - + {abstract} init() : void - + {abstract} calibrate() : void - + {abstract} readLine() : int16_t - + {abstract} getSensorValues() : const uint16_t* - + {abstract} isCalibrationSuccessful() : bool - + {abstract} getCalibErrorInfo() const : uint8_t - + {abstract} getNumLineSensors() const : uint8_t - + {abstract} getSensorValueMax() const : uint16_t - } - - interface IProximitySensors { - + {abstract} initFrontSensor() : void - + {abstract} getNumSensors() : const : uint8_t - + {abstract} read() : void - + {abstract} countsFrontWithLeftLeds() const : uint8_t - + {abstract} countsFrontWithRightLeds() const : uint8_t - } - - interface ILed { - + {abstract} enable(enableIt : bool) : void - } - } - - class Board << namespace >> { - + getDisplay() : IDisplay& - + getMotors() : IMotors& - + getLineSensors() : ILineSensors& - + getProximitySensors() : IProximitySensors& - + getLedYellow() : ILed& - } -} - -RemoteCtrlState *--> SimpleTimer -RemoteCtrlState ...> IDisplay: <> -RemoteCtrlState ...> IMotors: <> -RemoteCtrlState ...> ILineSensors: <> -RemoteCtrlState ...> IProximitySensors: <> -RemoteCtrlState ...> ILed: <> -RemoteCtrlState ...> Board:: <> - +@startuml RemoteControl RemoteCtrlState + +title Remote Control State + +package "Application" as appLayer { + + class RemoteCtrlState <> { + + {static} getInstance() : RemoteCtrlState + + entry() : void + + process(sm : StateMachine&) : void + + exit() : void + } + + class MotorSpeedCalibrationState <> + class LineSensorsCalibrationState <> + + RemoteCtrlState .l.> MotorSpeedCalibrationState: <> + RemoteCtrlState .r.> LineSensorsCalibrationState: <> +} + +package "Service" as serviceLayer { + + class SimpleTimer <> { + + start(duration : uint32_t) : void + + restart() : void + + stop() : void + + isTimeout() : bool + } +} + +package "HAL" as hal { + + package "Interfaces" as halInterfaces { + interface IDisplay { + + {abstract} clear() : void + + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void + + {abstract} print(str : const String&) : size_t + + {abstract} print(str : const char[]) : size_t + + {abstract} print(value : uint8_t) : size_t + + {abstract} print(value : uint16_t) : size_t + + {abstract} print(value : uint32_t) : size_t + + {abstract} print(value : int8_t) : size_t + + {abstract} print(value : int16_t) : size_t + + {abstract} print(value : int32_t) : size_t + } + + interface IMotors { + + {abstract} setSpeeds(leftSpeed : int16_t, rightSpeed : int16_t) : void + + {abstract} getMaxSpeed() : int16_t + } + + interface ILineSensors { + + {abstract} init() : void + + {abstract} calibrate() : void + + {abstract} readLine() : int16_t + + {abstract} getSensorValues() : const uint16_t* + + {abstract} isCalibrationSuccessful() : bool + + {abstract} getCalibErrorInfo() const : uint8_t + + {abstract} getNumLineSensors() const : uint8_t + + {abstract} getSensorValueMax() const : uint16_t + } + + interface IProximitySensors { + + {abstract} initFrontSensor() : void + + {abstract} getNumSensors() : const : uint8_t + + {abstract} read() : void + + {abstract} countsFrontWithLeftLeds() const : uint8_t + + {abstract} countsFrontWithRightLeds() const : uint8_t + } + + interface ILed { + + {abstract} enable(enableIt : bool) : void + } + } + + class Board << namespace >> { + + getDisplay() : IDisplay& + + getMotors() : IMotors& + + getLineSensors() : ILineSensors& + + getProximitySensors() : IProximitySensors& + + getLedYellow() : ILed& + } +} + +RemoteCtrlState *--> SimpleTimer +RemoteCtrlState ...> IDisplay: <> +RemoteCtrlState ...> IMotors: <> +RemoteCtrlState ...> ILineSensors: <> +RemoteCtrlState ...> IProximitySensors: <> +RemoteCtrlState ...> ILed: <> +RemoteCtrlState ...> Board:: <> + @enduml \ No newline at end of file diff --git a/doc/architecture/uml/LogicalView/RemoteControl/StartupState.plantuml b/doc/architecture/uml/LogicalView/RemoteControl/StartupState.plantuml index f556a5fe..01acf648 100644 --- a/doc/architecture/uml/LogicalView/RemoteControl/StartupState.plantuml +++ b/doc/architecture/uml/LogicalView/RemoteControl/StartupState.plantuml @@ -1,44 +1,44 @@ -@startuml RemoteControl StartupState - -title Startup State - -package "Application" as appLayer { - - class StartupState <> { - + {static} getInstance() : StartupState - + entry() : void - + process(sm : StateMachine&) : void - + exit() : void - } - - class RemoteCtrlState <> - - StartupState .r.> RemoteCtrlState: <> -} - -package "HAL" as hal { - - package "Interfaces" as halInterfaces { - interface IDisplay { - + {abstract} clear() : void - + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void - + {abstract} print(str : const String&) : size_t - + {abstract} print(str : const char[]) : size_t - + {abstract} print(value : uint8_t) : size_t - + {abstract} print(value : uint16_t) : size_t - + {abstract} print(value : uint32_t) : size_t - + {abstract} print(value : int8_t) : size_t - + {abstract} print(value : int16_t) : size_t - + {abstract} print(value : int32_t) : size_t - } - } - - class Board << namespace >> { - + getDisplay() : IDisplay& - } -} - -StartupState ..> IDisplay: <> -StartupState ..> Board:: <> - +@startuml RemoteControl StartupState + +title Startup State + +package "Application" as appLayer { + + class StartupState <> { + + {static} getInstance() : StartupState + + entry() : void + + process(sm : StateMachine&) : void + + exit() : void + } + + class RemoteCtrlState <> + + StartupState .r.> RemoteCtrlState: <> +} + +package "HAL" as hal { + + package "Interfaces" as halInterfaces { + interface IDisplay { + + {abstract} clear() : void + + {abstract} gotoXY(xCoord : uint8_t, yCoord : uint8_t) : void + + {abstract} print(str : const String&) : size_t + + {abstract} print(str : const char[]) : size_t + + {abstract} print(value : uint8_t) : size_t + + {abstract} print(value : uint16_t) : size_t + + {abstract} print(value : uint32_t) : size_t + + {abstract} print(value : int8_t) : size_t + + {abstract} print(value : int16_t) : size_t + + {abstract} print(value : int32_t) : size_t + } + } + + class Board << namespace >> { + + getDisplay() : IDisplay& + } +} + +StartupState ..> IDisplay: <> +StartupState ..> Board:: <> + @enduml \ No newline at end of file diff --git a/doc/architecture/uml/LogicalView/SensorFusion/HAL_IMU.puml b/doc/architecture/uml/LogicalView/SensorFusion/HAL_IMU.puml index a0f520ab..f1163ca6 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 7d04a66d..e4732fdf 100644 --- a/doc/architecture/uml/LogicalView/SensorFusion/HAL_SensorFusion.puml +++ b/doc/architecture/uml/LogicalView/SensorFusion/HAL_SensorFusion.puml @@ -1,150 +1,150 @@ -@startuml HAL SensorFusion - -title Hardware Abstraction Layer - SensorFusion App - -package "HAL" as hal { - - package "Interfaces" as halInterfaces { - - interface "IBuzzer" as iBuzzer { - + {abstract} playFrequency(freq : uint16_t, duration : uin16_t, volume : uint8_t) : void - + {abstract} playMelody(sequence : const char*) : void - + {abstract} playMelodyPGM(sequence : const char*) : void - + {abstract} isPlaying() : bool - + {abstract} process() : void - } - 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 NoBuzzer - class Encoders - class LedYellow - class IMU - } - - note bottom of NoBuzzer - This is a Dummy Buzzer Component - to save Flash Space. - end note - - iLineSensors <|... LineSensors: <> - iButton <|... ButtonA: <> - iMotors <|... Motors: <> - iBuzzer <|... NoBuzzer: <> - iEncoders <|... Encoders: <> - iLed <|... LedYellow: <> - iIMU <|... IMU: <> -} - -package "Zumo32U4 library" as zumo32u4Lib { - class Zumo32U4LineSensors - class Zumo32U4ButtonA - class Zumo32U4Motors - 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 "IBuzzer" as iBuzzer { + + {abstract} playFrequency(freq : uint16_t, duration : uin16_t, volume : uint8_t) : void + + {abstract} playMelody(sequence : const char*) : void + + {abstract} playMelodyPGM(sequence : const char*) : void + + {abstract} isPlaying() : bool + + {abstract} process() : void + } + 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 NoBuzzer + class Encoders + class LedYellow + class IMU + } + + note bottom of NoBuzzer + This is a Dummy Buzzer Component + to save Flash Space. + end note + + iLineSensors <|... LineSensors: <> + iButton <|... ButtonA: <> + iMotors <|... Motors: <> + iBuzzer <|... NoBuzzer: <> + iEncoders <|... Encoders: <> + iLed <|... LedYellow: <> + iIMU <|... IMU: <> +} + +package "Zumo32U4 library" as zumo32u4Lib { + class Zumo32U4LineSensors + class Zumo32U4ButtonA + class Zumo32U4Motors + 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/doc/architecture/uml/LogicalView/Service.plantuml b/doc/architecture/uml/LogicalView/Service.plantuml index 3b84dd4b..d2528ec8 100644 --- a/doc/architecture/uml/LogicalView/Service.plantuml +++ b/doc/architecture/uml/LogicalView/Service.plantuml @@ -1,82 +1,82 @@ -@startuml - -title Service - -package "Service" as serviceLayer { - - class Sound <> - - note top of Sound - Dedicated functions supporting the - standard beep, the alarm tone and - some melodies. - end note - - class SimpleTimer <> - - note top of SimpleTimer - Provides an easy interface for - timer handling on the base of - the Arduino millis(). - end note - - class Odometry <> - - note top of Odometry - Provides odometry information: - * Absolute position - * Orientation - * Mileage - end note - - class PIDController <> - - note top of PIDController - A PID controller used for driving - on the track. - end note - - class MovAvg < T, length > <> - - note top of MovAvg - Moving average filter which can be - configured at compile time. - end note - - class RelativeEncoder <> - - note top of RelativeEncoder - Calculates the relative encoder - steps from a reference point. - end note - - class Speedometer <> - - note top of Speedometer - Determines the linear speed left/right/center - in [steps/s]. - end note - - class DifferentialDrive <> - - note top of DifferentialDrive - Steering the robot with linear (steps/s) and - angular speed [mrad/s] by controlling the - speed with PID controllers. - end note - - class SerialMuxProt <> - - note top of SerialMuxProt - Communication Protocol for transmitting - and receiving information to/from a remote - client using data channels - end note - - DifferentialDrive -[hidden]-- MovAvg - Speedometer -[hidden]-- Odometry - RelativeEncoder -[hidden]-- PIDController - SimpleTimer -[hidden]-- Sound -} - +@startuml + +title Service + +package "Service" as serviceLayer { + + class Sound <> + + note top of Sound + Dedicated functions supporting the + standard beep, the alarm tone and + some melodies. + end note + + class SimpleTimer <> + + note top of SimpleTimer + Provides an easy interface for + timer handling on the base of + the Arduino millis(). + end note + + class Odometry <> + + note top of Odometry + Provides odometry information: + * Absolute position + * Orientation + * Mileage + end note + + class PIDController <> + + note top of PIDController + A PID controller used for driving + on the track. + end note + + class MovAvg < T, length > <> + + note top of MovAvg + Moving average filter which can be + configured at compile time. + end note + + class RelativeEncoder <> + + note top of RelativeEncoder + Calculates the relative encoder + steps from a reference point. + end note + + class Speedometer <> + + note top of Speedometer + Determines the linear speed left/right/center + in [steps/s]. + end note + + class DifferentialDrive <> + + note top of DifferentialDrive + Steering the robot with linear (steps/s) and + angular speed [mrad/s] by controlling the + speed with PID controllers. + end note + + class SerialMuxProt <> + + note top of SerialMuxProt + Communication Protocol for transmitting + and receiving information to/from a remote + client using data channels + end note + + DifferentialDrive -[hidden]-- MovAvg + Speedometer -[hidden]-- Odometry + RelativeEncoder -[hidden]-- PIDController + SimpleTimer -[hidden]-- Sound +} + @enduml \ No newline at end of file diff --git a/doc/architecture/uml/LogicalView/Speedometer.plantuml b/doc/architecture/uml/LogicalView/Speedometer.plantuml index 7cbb1d39..2d1ffe2a 100644 --- a/doc/architecture/uml/LogicalView/Speedometer.plantuml +++ b/doc/architecture/uml/LogicalView/Speedometer.plantuml @@ -1,29 +1,29 @@ -@startuml - -package "Service" { - class "Speedometer" as speedometer <> - class "RelativeEncoders" as relativeEncoders <> - - note left of speedometer - Determines the linear speed left/right/center - in [steps/s]. - end note - - speedometer --> relativeEncoders -} - -package "HAL" { - class "IEncoders" as iEncoders - class "IMotors" as iMotors - class "Encoders" as encoders - class "Motors" as motors - - iEncoders <|.. encoders: <> - iMotors <|.. motors: <> -} - -speedometer ...> iEncoders: <> -speedometer ...> iMotors: <> -relativeEncoders ..> iEncoders: <> - -@enduml +@startuml + +package "Service" { + class "Speedometer" as speedometer <> + class "RelativeEncoders" as relativeEncoders <> + + note left of speedometer + Determines the linear speed left/right/center + in [steps/s]. + end note + + speedometer --> relativeEncoders +} + +package "HAL" { + class "IEncoders" as iEncoders + class "IMotors" as iMotors + class "Encoders" as encoders + class "Motors" as motors + + iEncoders <|.. encoders: <> + iMotors <|.. motors: <> +} + +speedometer ...> iEncoders: <> +speedometer ...> iMotors: <> +relativeEncoders ..> iEncoders: <> + +@enduml diff --git a/doc/architecture/uml/PhysicalView/Deployment.plantuml b/doc/architecture/uml/PhysicalView/Deployment.plantuml index 21275df3..608fff55 100644 --- a/doc/architecture/uml/PhysicalView/Deployment.plantuml +++ b/doc/architecture/uml/PhysicalView/Deployment.plantuml @@ -1,56 +1,56 @@ -@startuml - -title Deployment - -node "PC" as pc - -node "Pololu Zumo32U4" as zumo32u4 { - - node "Main Board" as mainBoard { - - component "MCU\nAVR32U4" as mcu { - component "Software" as software - } - - component "Buttons" as buttons - - component "Buzzer" as buzzer - component "Motors" as motors - component "Encoders" as encoders - component "LEDs" as leds - component "ProximitySensors" as proximitySensors - component "Accelerometer" as accelerometer - component "Gyroscope" as gyroscope - - software -u- buttons - software -u- leds - software -u- proximitySensors - software -u- accelerometer - software -- buzzer - software -- motors - software -- encoders - software -- gyroscope - } - - node "Front Sensor Array" { - component "Line sensors" as lineSensors - } - - node "LCD" { - component "LCD" as lcd - } - - software - lcd - software -- lineSensors -} - -interface "USB" as usb - -zumo32u4 -l- usb -usb )-l- pc - -note top of zumo32u4 - Only the used actors and sensors are shown. -end note - +@startuml + +title Deployment + +node "PC" as pc + +node "Pololu Zumo32U4" as zumo32u4 { + + node "Main Board" as mainBoard { + + component "MCU\nAVR32U4" as mcu { + component "Software" as software + } + + component "Buttons" as buttons + + component "Buzzer" as buzzer + component "Motors" as motors + component "Encoders" as encoders + component "LEDs" as leds + component "ProximitySensors" as proximitySensors + component "Accelerometer" as accelerometer + component "Gyroscope" as gyroscope + + software -u- buttons + software -u- leds + software -u- proximitySensors + software -u- accelerometer + software -- buzzer + software -- motors + software -- encoders + software -- gyroscope + } + + node "Front Sensor Array" { + component "Line sensors" as lineSensors + } + + node "LCD" { + component "LCD" as lcd + } + + software - lcd + software -- lineSensors +} + +interface "USB" as usb + +zumo32u4 -l- usb +usb )-l- pc + +note top of zumo32u4 + Only the used actors and sensors are shown. +end note + @enduml \ No newline at end of file diff --git a/doc/architecture/uml/ProcessView/DifferentialDrive.plantuml b/doc/architecture/uml/ProcessView/DifferentialDrive.plantuml index 42780254..58a10e8b 100644 --- a/doc/architecture/uml/ProcessView/DifferentialDrive.plantuml +++ b/doc/architecture/uml/ProcessView/DifferentialDrive.plantuml @@ -1,39 +1,39 @@ -@startuml - -start - -if (Is differential drive enabled?) then (yes) - - if (Is max. motor speed available?) then (yes) - - :Get measured motor speeds [steps/s] left and right.; - - if (Is linear speed set point left 0 steps/s?) then (yes) - :Clear left PID conroller; - else - :Calculate next motor speed [steps/s] by last motor speed [steps/s]\n+ PID with speed set point [steps/s] and measured speed [steps/s].; - note right: Velocity PID controller - :Limit calculated motor speed by max. motor speed.; - :Store calculated motor speed for next calculation.; - :Convert calculated motor speed [steps/s] to motor speed PWM [digits].; - endif - - if (Is linear speed set point right 0 steps/s?) then (yes) - :Clear right PID conroller; - else - :Calculate next motor speed [steps/s] by last motor speed [steps/s]\n+ PID with speed set point [steps/s] and measured speed [steps/s].; - note right: Velocity PID controller - :Limit calculated motor speed by max. motor speed.; - :Store calculated motor speed for next calculation.; - :Convert calculated motor speed [steps/s] to motor speed PWM [digits].; - endif - - :Update motor speed PWM [digits] of left and right motor.; - - endif - -endif - -stop - +@startuml + +start + +if (Is differential drive enabled?) then (yes) + + if (Is max. motor speed available?) then (yes) + + :Get measured motor speeds [steps/s] left and right.; + + if (Is linear speed set point left 0 steps/s?) then (yes) + :Clear left PID conroller; + else + :Calculate next motor speed [steps/s] by last motor speed [steps/s]\n+ PID with speed set point [steps/s] and measured speed [steps/s].; + note right: Velocity PID controller + :Limit calculated motor speed by max. motor speed.; + :Store calculated motor speed for next calculation.; + :Convert calculated motor speed [steps/s] to motor speed PWM [digits].; + endif + + if (Is linear speed set point right 0 steps/s?) then (yes) + :Clear right PID conroller; + else + :Calculate next motor speed [steps/s] by last motor speed [steps/s]\n+ PID with speed set point [steps/s] and measured speed [steps/s].; + note right: Velocity PID controller + :Limit calculated motor speed by max. motor speed.; + :Store calculated motor speed for next calculation.; + :Convert calculated motor speed [steps/s] to motor speed PWM [digits].; + endif + + :Update motor speed PWM [digits] of left and right motor.; + + endif + +endif + +stop + @enduml \ No newline at end of file diff --git a/doc/architecture/uml/ProcessView/LineFollower/SystemStates.plantuml b/doc/architecture/uml/ProcessView/LineFollower/SystemStates.plantuml index cea78c95..423f042b 100644 --- a/doc/architecture/uml/ProcessView/LineFollower/SystemStates.plantuml +++ b/doc/architecture/uml/ProcessView/LineFollower/SystemStates.plantuml @@ -1,61 +1,61 @@ -@startuml - -title System States - -state StartupState -state MotorSpeedCalibrationState -state LineSensorsCalibrationState -state ErrorState -state DrivingState -state ReadyState -state ReleaseTrackState - -[*] --> StartupState: Power up -StartupState --> MotorSpeedCalibrationState: [Pushbutton A triggered] -StartupState --> LineSensorsCalibrationState: [Pushbutton B triggered] and\n[Max. motor speed calib. is available in settings] -MotorSpeedCalibrationState --> LineSensorsCalibrationState: [Calibration finished] -LineSensorsCalibrationState --> ReadyState: [Calibration finished] -LineSensorsCalibrationState --> ErrorState: [Calibration failed] -ReadyState --> ReleaseTrackState: [Pushbutton A triggered] -ReleaseTrackState --> DrivingState: [After 5s] -ReleaseTrackState --> ReleaseTrackState: [Pushbutton A triggered] -DrivingState --> ReadyState: [End line detected] or\n[Track lost] or\n[End line not found after 5 min.] -ErrorState --> StartupState: [Pushbutton A triggered] - -note right of StartupState - Initialize the system and wait till - the operator is ready for calibration. -end note - -note right of MotorSpeedCalibrationState - Measures the max. left/right encoder steps / s with - 100% motor speed. Required for the motor speed PID. - - Stores the calibration value in the settings persistent. -end note - -note left of LineSensorsCalibrationState - Calibrates the line sensors on the playfield by - measuring the the darkness and brightness values. -end note - -note bottom of ErrorState - Shows error to the operator. -end note - -note left of ReadyState - Waits for the operator to start the competition. -end note - -note left of ReleaseTrackState - The operator can select the parameter set - and releases the track after a certain time. -end note - -note right of DrivingState - The system drives autonomous on track, detects - start and end line. Additional it handels a - detected gap as well. -end note - +@startuml + +title System States + +state StartupState +state MotorSpeedCalibrationState +state LineSensorsCalibrationState +state ErrorState +state DrivingState +state ReadyState +state ReleaseTrackState + +[*] --> StartupState: Power up +StartupState --> MotorSpeedCalibrationState: [Pushbutton A triggered] +StartupState --> LineSensorsCalibrationState: [Pushbutton B triggered] and\n[Max. motor speed calib. is available in settings] +MotorSpeedCalibrationState --> LineSensorsCalibrationState: [Calibration finished] +LineSensorsCalibrationState --> ReadyState: [Calibration finished] +LineSensorsCalibrationState --> ErrorState: [Calibration failed] +ReadyState --> ReleaseTrackState: [Pushbutton A triggered] +ReleaseTrackState --> DrivingState: [After 5s] +ReleaseTrackState --> ReleaseTrackState: [Pushbutton A triggered] +DrivingState --> ReadyState: [End line detected] or\n[Track lost] or\n[End line not found after 5 min.] +ErrorState --> StartupState: [Pushbutton A triggered] + +note right of StartupState + Initialize the system and wait till + the operator is ready for calibration. +end note + +note right of MotorSpeedCalibrationState + Measures the max. left/right encoder steps / s with + 100% motor speed. Required for the motor speed PID. + + Stores the calibration value in the settings persistent. +end note + +note left of LineSensorsCalibrationState + Calibrates the line sensors on the playfield by + measuring the the darkness and brightness values. +end note + +note bottom of ErrorState + Shows error to the operator. +end note + +note left of ReadyState + Waits for the operator to start the competition. +end note + +note left of ReleaseTrackState + The operator can select the parameter set + and releases the track after a certain time. +end note + +note right of DrivingState + The system drives autonomous on track, detects + start and end line. Additional it handels a + detected gap as well. +end note + @enduml \ No newline at end of file diff --git a/doc/architecture/uml/ProcessView/LineFollower/SystemStates2.plantuml b/doc/architecture/uml/ProcessView/LineFollower/SystemStates2.plantuml index 1400ceac..367e7b77 100644 --- a/doc/architecture/uml/ProcessView/LineFollower/SystemStates2.plantuml +++ b/doc/architecture/uml/ProcessView/LineFollower/SystemStates2.plantuml @@ -1,44 +1,44 @@ -@startuml - -title System States - -state StartupState: /entry Initialize HAL. -state StartupState: /entry Show operator info on LCD. -state StartupState: /do Wait for pushbutton A or B is triggered. - -state MotorSpeedCalibrationState: /entry Show operator info on LCD. -state MotorSpeedCalibrationState: /do Perform calibration. - -state LineSensorsCalibrationState: /entry Show operator info on LCD. -state LineSensorsCalibrationState: /do Perform calibration. - -state ErrorState: /entry Show error info on LCD. -state ErrorState: /do Wait for pushbutton A is triggered. - -state ReadyState: /entry Show operator info on LCD. -state ReadyState: /do Wait for pushbutton A is triggered. - -state ReleaseTrackState: /entry Choose parameter set 1 -state ReleaseTrackState: /entry Show parameter set on LCD. -state ReleaseTrackState: /entry Start release timer. -state ReleaseTrackState: /do If pushbutton A is triggered, choose next parameter set and update LCD. -state ReleaseTrackState: /do If pushbutton A is triggered, restart release timer. -state ReleaseTrackState: /exit Stop release timer. - -state DrivingState: /entry Start observation timer. -state DrivingState: /do Perform driving. -state DrivingState: /exit Stop observation timer. - -[*] --> StartupState: Power up -StartupState --> MotorSpeedCalibrationState: [Pushbutton A triggered] -StartupState --> LineSensorsCalibrationState: [Pushbutton B triggered] and\n[Max. motor speed calib. is available in settings] -MotorSpeedCalibrationState --> LineSensorsCalibrationState: [Calibration finished] -LineSensorsCalibrationState --> ReadyState: [Calibration finished] -LineSensorsCalibrationState --> ErrorState: [Calibration failed] -ReadyState --> ReleaseTrackState: [Pushbutton A triggered] -ReleaseTrackState --> DrivingState: [Release timer timeout] -ReleaseTrackState --> ReleaseTrackState: [Pushbutton A triggered] -DrivingState --> ReadyState: [End line detected] or\n[Track lost] or\n[Observation timer timeout] -ErrorState --> StartupState: [Pushbutton A triggered] - +@startuml + +title System States + +state StartupState: /entry Initialize HAL. +state StartupState: /entry Show operator info on LCD. +state StartupState: /do Wait for pushbutton A or B is triggered. + +state MotorSpeedCalibrationState: /entry Show operator info on LCD. +state MotorSpeedCalibrationState: /do Perform calibration. + +state LineSensorsCalibrationState: /entry Show operator info on LCD. +state LineSensorsCalibrationState: /do Perform calibration. + +state ErrorState: /entry Show error info on LCD. +state ErrorState: /do Wait for pushbutton A is triggered. + +state ReadyState: /entry Show operator info on LCD. +state ReadyState: /do Wait for pushbutton A is triggered. + +state ReleaseTrackState: /entry Choose parameter set 1 +state ReleaseTrackState: /entry Show parameter set on LCD. +state ReleaseTrackState: /entry Start release timer. +state ReleaseTrackState: /do If pushbutton A is triggered, choose next parameter set and update LCD. +state ReleaseTrackState: /do If pushbutton A is triggered, restart release timer. +state ReleaseTrackState: /exit Stop release timer. + +state DrivingState: /entry Start observation timer. +state DrivingState: /do Perform driving. +state DrivingState: /exit Stop observation timer. + +[*] --> StartupState: Power up +StartupState --> MotorSpeedCalibrationState: [Pushbutton A triggered] +StartupState --> LineSensorsCalibrationState: [Pushbutton B triggered] and\n[Max. motor speed calib. is available in settings] +MotorSpeedCalibrationState --> LineSensorsCalibrationState: [Calibration finished] +LineSensorsCalibrationState --> ReadyState: [Calibration finished] +LineSensorsCalibrationState --> ErrorState: [Calibration failed] +ReadyState --> ReleaseTrackState: [Pushbutton A triggered] +ReleaseTrackState --> DrivingState: [Release timer timeout] +ReleaseTrackState --> ReleaseTrackState: [Pushbutton A triggered] +DrivingState --> ReadyState: [End line detected] or\n[Track lost] or\n[Observation timer timeout] +ErrorState --> StartupState: [Pushbutton A triggered] + @enduml \ No newline at end of file diff --git a/doc/architecture/uml/ProcessView/RemoteControl/SystemStates.plantuml b/doc/architecture/uml/ProcessView/RemoteControl/SystemStates.plantuml index ac2658fd..a2dcf3c0 100644 --- a/doc/architecture/uml/ProcessView/RemoteControl/SystemStates.plantuml +++ b/doc/architecture/uml/ProcessView/RemoteControl/SystemStates.plantuml @@ -1,48 +1,48 @@ -@startuml RemoteControl System States - -title System States - -state StartupState -state MotorSpeedCalibrationState -state LineSensorsCalibrationState -state ErrorState -state RemoteCtrlState - -[*] --> StartupState: Power up -StartupState --> RemoteCtrlState - -RemoteCtrlState --> MotorSpeedCalibrationState: [on remote request] -RemoteCtrlState --> LineSensorsCalibrationState: [on remote request] - -MotorSpeedCalibrationState --> RemoteCtrlState: [Calibration finished] -MotorSpeedCalibrationState --> ErrorState: [Calibration failed] - -LineSensorsCalibrationState --> RemoteCtrlState: [Calibration finished] -LineSensorsCalibrationState --> ErrorState: [Calibration failed] - -ErrorState --> RemoteCtrlState: [Pushbutton A triggered] - -note right of StartupState - Initialize the system and show application name. -end note - -note right of MotorSpeedCalibrationState - Measures the max. left/right encoder steps / s with - 100% motor speed. Required for the motor speed PID. -end note - -note right of LineSensorsCalibrationState - Calibrates the line sensors on the playfield by - measuring the the darkness and brightness values. -end note - -note left of ErrorState - Shows error to the operator. -end note - -note right of RemoteCtrlState - The system drives according to the commands sent by the operator. - Additionally, it prevents crashes using the proximity sensors. -end note - +@startuml RemoteControl System States + +title System States + +state StartupState +state MotorSpeedCalibrationState +state LineSensorsCalibrationState +state ErrorState +state RemoteCtrlState + +[*] --> StartupState: Power up +StartupState --> RemoteCtrlState + +RemoteCtrlState --> MotorSpeedCalibrationState: [on remote request] +RemoteCtrlState --> LineSensorsCalibrationState: [on remote request] + +MotorSpeedCalibrationState --> RemoteCtrlState: [Calibration finished] +MotorSpeedCalibrationState --> ErrorState: [Calibration failed] + +LineSensorsCalibrationState --> RemoteCtrlState: [Calibration finished] +LineSensorsCalibrationState --> ErrorState: [Calibration failed] + +ErrorState --> RemoteCtrlState: [Pushbutton A triggered] + +note right of StartupState + Initialize the system and show application name. +end note + +note right of MotorSpeedCalibrationState + Measures the max. left/right encoder steps / s with + 100% motor speed. Required for the motor speed PID. +end note + +note right of LineSensorsCalibrationState + Calibrates the line sensors on the playfield by + measuring the the darkness and brightness values. +end note + +note left of ErrorState + Shows error to the operator. +end note + +note right of RemoteCtrlState + The system drives according to the commands sent by the operator. + Additionally, it prevents crashes using the proximity sensors. +end note + @enduml \ No newline at end of file diff --git a/doc/architecture/uml/ViewModels.plantuml b/doc/architecture/uml/ViewModels.plantuml index 9e5888f0..68167a87 100644 --- a/doc/architecture/uml/ViewModels.plantuml +++ b/doc/architecture/uml/ViewModels.plantuml @@ -1,59 +1,59 @@ -@startuml - -title View Models of Software Architecture - -legend top - [[https://en.wikipedia.org/wiki/4%2B1_architectural_view_model 4+1 architectural view model]] -endlegend - -component "Logical View" as logicalView -component "Development View" as developmentView -component "Physical View" as physicalView -component "Process View" as processView -component "Scenarios" as scenarios - -logicalView -> developmentView -logicalView --> processView -developmentView --> physicalView -processView -> physicalView - -note left of logicalView - **The Object-Oriented Decomposition** - The logical architecture primarily supports - the functional requirements—what the system - should provide in terms of services to its users. -end note - -note right of developmentView - **Subsystem decomposition** - The development architecture focuses on the - actual software module organization on the - software development environment. -end note - -note right of physicalView - **Mapping the software to the hardware** - The physical architecture takes into account - primarily the non-functional requirements of - the system such as availability, reliability - (fault-tolerance), performance (throughput) - and scalability. -end note - -note left of processView - **The Process Decomposition** - The process architecture takes into account - some non-functional requirements, such as - performance and availability. -end note - -processView -[hidden]-- scenarios - -note right of scenarios - **Putting it all together** - The elements in the four views are shown to - work together seamlessly by the use of a small - set of important scenarios. -end note - +@startuml + +title View Models of Software Architecture + +legend top + [[https://en.wikipedia.org/wiki/4%2B1_architectural_view_model 4+1 architectural view model]] +endlegend + +component "Logical View" as logicalView +component "Development View" as developmentView +component "Physical View" as physicalView +component "Process View" as processView +component "Scenarios" as scenarios + +logicalView -> developmentView +logicalView --> processView +developmentView --> physicalView +processView -> physicalView + +note left of logicalView + **The Object-Oriented Decomposition** + The logical architecture primarily supports + the functional requirements—what the system + should provide in terms of services to its users. +end note + +note right of developmentView + **Subsystem decomposition** + The development architecture focuses on the + actual software module organization on the + software development environment. +end note + +note right of physicalView + **Mapping the software to the hardware** + The physical architecture takes into account + primarily the non-functional requirements of + the system such as availability, reliability + (fault-tolerance), performance (throughput) + and scalability. +end note + +note left of processView + **The Process Decomposition** + The process architecture takes into account + some non-functional requirements, such as + performance and availability. +end note + +processView -[hidden]-- scenarios + +note right of scenarios + **Putting it all together** + The elements in the four views are shown to + work together seamlessly by the use of a small + set of important scenarios. +end note + @enduml \ No newline at end of file diff --git a/doc/configuration/README.md b/doc/configuration/README.md index 24d037a1..80dbeda2 100644 --- a/doc/configuration/README.md +++ b/doc/configuration/README.md @@ -1,34 +1,34 @@ -# Radon Ulzer - Line Follower - -- [Configuration Management](#configuration-management) -- [ArduinoNative](#arduinonative) -- [Issues, Ideas And Bugs](#issues-ideas-and-bugs) -- [License](#license) -- [Contribution](#contribution) - -# Configuration Management - -![config](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/configuration/uml/configuration.plantuml) - -# PlatformIO Environments -As seen in the diagram before, the working environments are defined in the platformio.ini file, and these are used by PlatformIO to build the respective applications. To build an specific application, there are three possible methods: - -- On the left task bar of VS Code, choose the PlatformIO extension symbol, and open de drop-down menu of the desired application. -- Choose the desired application on the bottom task bar using the "Switch PlatformIO Project Environment" button. -- Use the command line: ```pio run -e ``` - -# ArduinoNative -In order to maintain compatibility between target and simulation, some interfaces from the Arduino Core have been adapted and/or stubbed into the ArduinoNative library. Code stubs for Stream, Serial, millis(), delay() and other similar Arduino functionalities have been implemented here. - -It is important to note that in the case of the simulation, the main entry point of the program is found in Arduino.cpp. - -# 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 - Line Follower + +- [Configuration Management](#configuration-management) +- [ArduinoNative](#arduinonative) +- [Issues, Ideas And Bugs](#issues-ideas-and-bugs) +- [License](#license) +- [Contribution](#contribution) + +# Configuration Management + +![config](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/configuration/uml/configuration.plantuml) + +# PlatformIO Environments +As seen in the diagram before, the working environments are defined in the platformio.ini file, and these are used by PlatformIO to build the respective applications. To build an specific application, there are three possible methods: + +- On the left task bar of VS Code, choose the PlatformIO extension symbol, and open de drop-down menu of the desired application. +- Choose the desired application on the bottom task bar using the "Switch PlatformIO Project Environment" button. +- Use the command line: ```pio run -e ``` + +# ArduinoNative +In order to maintain compatibility between target and simulation, some interfaces from the Arduino Core have been adapted and/or stubbed into the ArduinoNative library. Code stubs for Stream, Serial, millis(), delay() and other similar Arduino functionalities have been implemented here. + +It is important to note that in the case of the simulation, the main entry point of the program is found in Arduino.cpp. + +# 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/configuration/uml/RemoteControlSim.puml b/doc/configuration/uml/RemoteControlSim.puml index 8607dbde..09abcdaa 100644 --- a/doc/configuration/uml/RemoteControlSim.puml +++ b/doc/configuration/uml/RemoteControlSim.puml @@ -1,39 +1,39 @@ -@startuml Simulation_Deployment - -Title RemoteControl in a Simulation Environment - -node "PC" { - node "RemoteControl" <> as remoteControlExe{ - - component "RemoteControl" <> as app - - component "Odometry" <> as odometry - component "Speedometer" <> as speedometer - component "DiffDrive" <> as diffdrive - component "SocketServer" <> as socketServer - - component "SerialMuxProt" <> as smp - component "HAL" <> as hal - - app ..> smp: <> - app <.. odometry: <> - app <.. speedometer: <> - app ..> diffdrive: <> - - smp <..> socketServer: <> - - odometry ..> hal: <> - speedometer ..> hal: <> - diffdrive ..> hal: <> - } - - node "Webots" <> as webots { - PortIn "ipc://1234/Zumo" as webotsPort - component "Robot: Zumo" as robot - webotsPort <..> robot: <> - } - - hal <..> webotsPort : <> -} - +@startuml Simulation_Deployment + +Title RemoteControl in a Simulation Environment + +node "PC" { + node "RemoteControl" <> as remoteControlExe{ + + component "RemoteControl" <> as app + + component "Odometry" <> as odometry + component "Speedometer" <> as speedometer + component "DiffDrive" <> as diffdrive + component "SocketServer" <> as socketServer + + component "SerialMuxProt" <> as smp + component "HAL" <> as hal + + app ..> smp: <> + app <.. odometry: <> + app <.. speedometer: <> + app ..> diffdrive: <> + + smp <..> socketServer: <> + + odometry ..> hal: <> + speedometer ..> hal: <> + diffdrive ..> hal: <> + } + + node "Webots" <> as webots { + PortIn "ipc://1234/Zumo" as webotsPort + component "Robot: Zumo" as robot + webotsPort <..> robot: <> + } + + hal <..> webotsPort : <> +} + @enduml \ No newline at end of file diff --git a/doc/configuration/uml/configuration.plantuml b/doc/configuration/uml/configuration.plantuml index d8475777..ef01f283 100644 --- a/doc/configuration/uml/configuration.plantuml +++ b/doc/configuration/uml/configuration.plantuml @@ -1,112 +1,112 @@ -@startuml - -title Configuration - -package "platformio.ini" { - - package "working environments" { - component "env:Target" <> as pioEnvAppTarget - component "env:Sim" <> as pioEnvAppSim - } - - package "configuration environments" as cfgEnv { - - component "hal_app:Target" <> as pioHalAppTarget - component "app:" <> as pioAppApp - component "hal_app:Sim" <> as pioHalAppSim - - component "hal:Target" <> as pioHalTarget - component "hal:Sim" <> as pioHalSim - } -} - -package "/lib" as libFolder { - - component "HALInterfaces" <> as halInterfacesLib - - component "APP" <> as appLib - component "Service" <> as serviceLib - - component "HALTarget" <> as halAppTarget - component "HALSim" <> as halAppSim - - component "HALTarget" <> as halTargetLib - component "HALSim" <> as halSimLib - - component "Webots" <> as webotsLib - component "ArduinoNative" <> as arduinoNativeLib - - note bottom of halAppTarget - Application specific target HAL - instantiation via Board class. - May use less peripherals, than - available. - end note - - note bottom of halAppSim - Application specific simulation HAL - instantiation via Board class. - May use less peripherals, than - available. - end note - - note bottom of halTargetLib - Contains the target HAL - peripheral classes. - end note - - note bottom of halSimLib - Contains the simulation HAL - peripheral classes. - end note - - note bottom of halInterfacesLib - Defines the interfaces - of the HAL peripherals. - end note - - note bottom of appLib - Application specific functionality. - end note - - note bottom of serviceLib - General services, used by - the applications. - end note - - note bottom of webotsLib - Webots simulation glue code. - end note - - note bottom of arduinoNativeLib - Some stubbed Arduino interfaces - for simulation. - end note -} - -pioEnvAppTarget ...> pioAppApp: <> -pioEnvAppTarget ...> pioHalAppTarget: <> - -pioHalAppTarget ..> pioHalTarget: <> -pioHalAppTarget ...> halAppTarget: <> - -pioHalTarget ..> halTargetLib: <> -pioHalTarget ..> halInterfacesLib: <> - - -pioEnvAppSim ..> pioAppApp: <> -pioEnvAppSim ..> pioHalAppSim: <> - -pioHalAppSim ..> pioHalSim: <> -pioHalAppSim ...> halAppSim: <> - -pioHalSim ..> halSimLib: <> -pioHalSim ..> webotsLib: <> -pioHalSim ..> halInterfacesLib: <> -pioHalSim ..> arduinoNativeLib: <> - - -pioAppApp ...> appLib: <> -pioAppApp ...> serviceLib: <> - +@startuml + +title Configuration + +package "platformio.ini" { + + package "working environments" { + component "env:Target" <> as pioEnvAppTarget + component "env:Sim" <> as pioEnvAppSim + } + + package "configuration environments" as cfgEnv { + + component "hal_app:Target" <> as pioHalAppTarget + component "app:" <> as pioAppApp + component "hal_app:Sim" <> as pioHalAppSim + + component "hal:Target" <> as pioHalTarget + component "hal:Sim" <> as pioHalSim + } +} + +package "/lib" as libFolder { + + component "HALInterfaces" <> as halInterfacesLib + + component "APP" <> as appLib + component "Service" <> as serviceLib + + component "HALTarget" <> as halAppTarget + component "HALSim" <> as halAppSim + + component "HALTarget" <> as halTargetLib + component "HALSim" <> as halSimLib + + component "Webots" <> as webotsLib + component "ArduinoNative" <> as arduinoNativeLib + + note bottom of halAppTarget + Application specific target HAL + instantiation via Board class. + May use less peripherals, than + available. + end note + + note bottom of halAppSim + Application specific simulation HAL + instantiation via Board class. + May use less peripherals, than + available. + end note + + note bottom of halTargetLib + Contains the target HAL + peripheral classes. + end note + + note bottom of halSimLib + Contains the simulation HAL + peripheral classes. + end note + + note bottom of halInterfacesLib + Defines the interfaces + of the HAL peripherals. + end note + + note bottom of appLib + Application specific functionality. + end note + + note bottom of serviceLib + General services, used by + the applications. + end note + + note bottom of webotsLib + Webots simulation glue code. + end note + + note bottom of arduinoNativeLib + Some stubbed Arduino interfaces + for simulation. + end note +} + +pioEnvAppTarget ...> pioAppApp: <> +pioEnvAppTarget ...> pioHalAppTarget: <> + +pioHalAppTarget ..> pioHalTarget: <> +pioHalAppTarget ...> halAppTarget: <> + +pioHalTarget ..> halTargetLib: <> +pioHalTarget ..> halInterfacesLib: <> + + +pioEnvAppSim ..> pioAppApp: <> +pioEnvAppSim ..> pioHalAppSim: <> + +pioHalAppSim ..> pioHalSim: <> +pioHalAppSim ...> halAppSim: <> + +pioHalSim ..> halSimLib: <> +pioHalSim ..> webotsLib: <> +pioHalSim ..> halInterfacesLib: <> +pioHalSim ..> arduinoNativeLib: <> + + +pioAppApp ...> appLib: <> +pioAppApp ...> serviceLib: <> + @enduml \ No newline at end of file diff --git a/doc/doxygen/mainpage.dox b/doc/doxygen/mainpage.dox index 3b1bf2d1..2bc98966 100644 --- a/doc/doxygen/mainpage.dox +++ b/doc/doxygen/mainpage.dox @@ -1,26 +1,26 @@ -@mainpage Manual of Line Follower - -@defgroup Application Application -The main application. -@{ -@} - -@defgroup HALInterfaces Hardware abstraction interfaces -The HAL interface abstraction, which every kind of HAL needs to provide. -@{ -@} - -@defgroup HALTarget Hardware abstraction on the Zumo32U4 -Abstracts the the physical hardware of the Zumo32U4. -@{ -@} - -@defgroup HALSim Hardware abstraction in Webots simulation -Abstracts the the hardware in the Webots simulation. -@{ -@} - -@defgroup Service Service -The services which are widely used by the application. -@{ -@} +@mainpage Manual of Line Follower + +@defgroup Application Application +The main application. +@{ +@} + +@defgroup HALInterfaces Hardware abstraction interfaces +The HAL interface abstraction, which every kind of HAL needs to provide. +@{ +@} + +@defgroup HALTarget Hardware abstraction on the Zumo32U4 +Abstracts the the physical hardware of the Zumo32U4. +@{ +@} + +@defgroup HALSim Hardware abstraction in Webots simulation +Abstracts the the hardware in the Webots simulation. +@{ +@} + +@defgroup Service Service +The services which are widely used by the application. +@{ +@} diff --git a/lib/APPCalib/App.cpp b/lib/APPCalib/App.cpp index 4ab51d9b..84013f0b 100644 --- a/lib/APPCalib/App.cpp +++ b/lib/APPCalib/App.cpp @@ -1,120 +1,120 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Calibration 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); - - /* Initialize HAL */ - Board::getInstance().init(); - - /* Setup the state machine with the first state. */ - m_systemStateMachine.setState(&StartupState::getInstance()); - - /* Setup the periodically processing of robot control. */ - m_controlInterval.start(DIFFERENTIAL_DRIVE_CONTROL_PERIOD); -} - -void App::loop() -{ - Board::getInstance().process(); - Speedometer::getInstance().process(); - - if (true == m_controlInterval.isTimeout()) - { - /* The differential drive control needs the measured speed of the - * left and right wheel. Therefore it shall be processed after - * the speedometer. - */ - DifferentialDrive::getInstance().process(DIFFERENTIAL_DRIVE_CONTROL_PERIOD); - - /* The odometry unit needs to detect motor speed changes to be able to - * calculate correct values. Therefore it shall be processed right after - * the differential drive control. - */ - Odometry::getInstance().process(); - - m_controlInterval.restart(); - } - - m_systemStateMachine.process(); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Calibration 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); + + /* Initialize HAL */ + Board::getInstance().init(); + + /* Setup the state machine with the first state. */ + m_systemStateMachine.setState(&StartupState::getInstance()); + + /* Setup the periodically processing of robot control. */ + m_controlInterval.start(DIFFERENTIAL_DRIVE_CONTROL_PERIOD); +} + +void App::loop() +{ + Board::getInstance().process(); + Speedometer::getInstance().process(); + + if (true == m_controlInterval.isTimeout()) + { + /* The differential drive control needs the measured speed of the + * left and right wheel. Therefore it shall be processed after + * the speedometer. + */ + DifferentialDrive::getInstance().process(DIFFERENTIAL_DRIVE_CONTROL_PERIOD); + + /* The odometry unit needs to detect motor speed changes to be able to + * calculate correct values. Therefore it shall be processed right after + * the differential drive control. + */ + Odometry::getInstance().process(); + + m_controlInterval.restart(); + } + + m_systemStateMachine.process(); +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions *****************************************************************************/ \ No newline at end of file diff --git a/lib/APPCalib/App.h b/lib/APPCalib/App.h index 8d43f353..e2ffbae0 100644 --- a/lib/APPCalib/App.h +++ b/lib/APPCalib/App.h @@ -1,109 +1,109 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Calibration application - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef APP_H -#define APP_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The calibration application. */ -class App -{ -public: - /** - * Construct the calibration application. - */ - App() : m_systemStateMachine(), m_controlInterval() - { - } - - /** - * Destroy the calibration 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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Calibration application + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef APP_H +#define APP_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The calibration application. */ +class App +{ +public: + /** + * Construct the calibration application. + */ + App() : m_systemStateMachine(), m_controlInterval() + { + } + + /** + * Destroy the calibration 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/APPCalib/DrivingState.cpp b/lib/APPCalib/DrivingState.cpp index 280980ba..8532c501 100644 --- a/lib/APPCalib/DrivingState.cpp +++ b/lib/APPCalib/DrivingState.cpp @@ -1,177 +1,177 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Driving state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "DrivingState.h" -#include -#include -#include -#include -#include "ReadyState.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void DrivingState::entry() -{ - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - Odometry& odometry = Odometry::getInstance(); - int32_t angle = odometry.getOrientation(); - int16_t speed = diffDrive.getMaxMotorSpeed() / 3; - - switch (m_cmd) - { - case DRIVING_CMD_FORWARD: - odometry.clearMileage(); - diffDrive.setLinearSpeed(speed); - break; - - case DRIVING_CMD_TURN_LEFT: - m_dstAngle = angle + TURN_ANGLE; - - if (FP_2PI() <= m_dstAngle) - { - m_dstAngle -= FP_2PI(); - m_wrapAroundCnt = 1; - } - - diffDrive.setLinearSpeed(-speed, speed); - break; - - case DRIVING_CMD_TURN_RIGHT: - m_dstAngle = angle - TURN_ANGLE; - - if (-FP_2PI() >= m_dstAngle) - { - m_dstAngle += FP_2PI(); - m_wrapAroundCnt = 1; - } - - diffDrive.setLinearSpeed(speed, -speed); - break; - - default: - break; - } -} - -void DrivingState::process(StateMachine& sm) -{ - Odometry& odometry = Odometry::getInstance(); - - switch (m_cmd) - { - case DRIVING_CMD_FORWARD: - if (DRIVE_FORWARD_DISTANCE <= odometry.getMileageCenter()) - { - sm.setState(&ReadyState::getInstance()); - } - break; - - case DRIVING_CMD_TURN_LEFT: - if (0 < m_wrapAroundCnt) - { - if (FP_PI() > odometry.getOrientation()) - { - --m_wrapAroundCnt; - } - } - - if ((m_dstAngle <= odometry.getOrientation()) && (0 == m_wrapAroundCnt)) - { - sm.setState(&ReadyState::getInstance()); - } - break; - - case DRIVING_CMD_TURN_RIGHT: - if (0 < m_wrapAroundCnt) - { - if (-FP_PI() < odometry.getOrientation()) - { - --m_wrapAroundCnt; - } - } - - if ((m_dstAngle >= odometry.getOrientation()) && (0 == m_wrapAroundCnt)) - { - sm.setState(&ReadyState::getInstance()); - } - break; - - default: - break; - } -} - -void DrivingState::exit() -{ - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - diffDrive.setLinearSpeed(0, 0); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Driving state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "DrivingState.h" +#include +#include +#include +#include +#include "ReadyState.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void DrivingState::entry() +{ + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + Odometry& odometry = Odometry::getInstance(); + int32_t angle = odometry.getOrientation(); + int16_t speed = diffDrive.getMaxMotorSpeed() / 3; + + switch (m_cmd) + { + case DRIVING_CMD_FORWARD: + odometry.clearMileage(); + diffDrive.setLinearSpeed(speed); + break; + + case DRIVING_CMD_TURN_LEFT: + m_dstAngle = angle + TURN_ANGLE; + + if (FP_2PI() <= m_dstAngle) + { + m_dstAngle -= FP_2PI(); + m_wrapAroundCnt = 1; + } + + diffDrive.setLinearSpeed(-speed, speed); + break; + + case DRIVING_CMD_TURN_RIGHT: + m_dstAngle = angle - TURN_ANGLE; + + if (-FP_2PI() >= m_dstAngle) + { + m_dstAngle += FP_2PI(); + m_wrapAroundCnt = 1; + } + + diffDrive.setLinearSpeed(speed, -speed); + break; + + default: + break; + } +} + +void DrivingState::process(StateMachine& sm) +{ + Odometry& odometry = Odometry::getInstance(); + + switch (m_cmd) + { + case DRIVING_CMD_FORWARD: + if (DRIVE_FORWARD_DISTANCE <= odometry.getMileageCenter()) + { + sm.setState(&ReadyState::getInstance()); + } + break; + + case DRIVING_CMD_TURN_LEFT: + if (0 < m_wrapAroundCnt) + { + if (FP_PI() > odometry.getOrientation()) + { + --m_wrapAroundCnt; + } + } + + if ((m_dstAngle <= odometry.getOrientation()) && (0 == m_wrapAroundCnt)) + { + sm.setState(&ReadyState::getInstance()); + } + break; + + case DRIVING_CMD_TURN_RIGHT: + if (0 < m_wrapAroundCnt) + { + if (-FP_PI() < odometry.getOrientation()) + { + --m_wrapAroundCnt; + } + } + + if ((m_dstAngle >= odometry.getOrientation()) && (0 == m_wrapAroundCnt)) + { + sm.setState(&ReadyState::getInstance()); + } + break; + + default: + break; + } +} + +void DrivingState::exit() +{ + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + diffDrive.setLinearSpeed(0, 0); +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPCalib/DrivingState.h b/lib/APPCalib/DrivingState.h index 7b864ca3..3c593561 100644 --- a/lib/APPCalib/DrivingState.h +++ b/lib/APPCalib/DrivingState.h @@ -1,150 +1,150 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Driving state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef DRIVING_STATE_H -#define DRIVING_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The system driving state. */ -class DrivingState : public IState -{ -public: - /** - * Supported driving commands. - */ - enum DrivingCmd - { - DRIVING_CMD_FORWARD = 0, /**< Drive forward. */ - DRIVING_CMD_TURN_LEFT, /**< Turn left 90°. */ - DRIVING_CMD_TURN_RIGHT /**< Turn right 90°. */ - }; - - /** - * Get state instance. - * - * @return State instance. - */ - static DrivingState& getInstance() - { - static DrivingState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - - /** - * Set driving command. - * - * @param[in] cmd Command which to execute. - */ - void setCmd(DrivingCmd cmd) - { - m_cmd = cmd; - } - -private: - /** Forward distance to drive in mm. */ - static const uint32_t DRIVE_FORWARD_DISTANCE = 100; - - /** Angle to turn in mrad. */ - static const int32_t TURN_ANGLE = FP_PI() / 2; - - DrivingCmd m_cmd; /**< Driving command */ - int32_t m_dstAngle; /**< Destination angle in mrad. */ - uint8_t m_wrapAroundCnt; /**< Counts the angle wrap around. */ - - /** - * Default constructor. - */ - DrivingState() : m_cmd(DRIVING_CMD_FORWARD), m_dstAngle(0), m_wrapAroundCnt(0) - { - } - - /** - * Default destructor. - */ - ~DrivingState() - { - } - - /* Not allowed. */ - DrivingState(const DrivingState& state); /**< Copy construction of an instance. */ - DrivingState& operator=(const DrivingState& state); /**< Assignment of an instance. */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* DRIVING_STATE_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Driving state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef DRIVING_STATE_H +#define DRIVING_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The system driving state. */ +class DrivingState : public IState +{ +public: + /** + * Supported driving commands. + */ + enum DrivingCmd + { + DRIVING_CMD_FORWARD = 0, /**< Drive forward. */ + DRIVING_CMD_TURN_LEFT, /**< Turn left 90°. */ + DRIVING_CMD_TURN_RIGHT /**< Turn right 90°. */ + }; + + /** + * Get state instance. + * + * @return State instance. + */ + static DrivingState& getInstance() + { + static DrivingState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + + /** + * Set driving command. + * + * @param[in] cmd Command which to execute. + */ + void setCmd(DrivingCmd cmd) + { + m_cmd = cmd; + } + +private: + /** Forward distance to drive in mm. */ + static const uint32_t DRIVE_FORWARD_DISTANCE = 100; + + /** Angle to turn in mrad. */ + static const int32_t TURN_ANGLE = FP_PI() / 2; + + DrivingCmd m_cmd; /**< Driving command */ + int32_t m_dstAngle; /**< Destination angle in mrad. */ + uint8_t m_wrapAroundCnt; /**< Counts the angle wrap around. */ + + /** + * Default constructor. + */ + DrivingState() : m_cmd(DRIVING_CMD_FORWARD), m_dstAngle(0), m_wrapAroundCnt(0) + { + } + + /** + * Default destructor. + */ + ~DrivingState() + { + } + + /* Not allowed. */ + DrivingState(const DrivingState& state); /**< Copy construction of an instance. */ + DrivingState& operator=(const DrivingState& state); /**< Assignment of an instance. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* DRIVING_STATE_H */ +/** @} */ diff --git a/lib/APPCalib/ErrorState.cpp b/lib/APPCalib/ErrorState.cpp index b5ca6714..12bef91a 100644 --- a/lib/APPCalib/ErrorState.cpp +++ b/lib/APPCalib/ErrorState.cpp @@ -1,137 +1,137 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Error state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ErrorState.h" -#include -#include -#include "StartupState.h" -#include -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/** - * Error logging tag. - */ -LOG_TAG("EState"); - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void ErrorState::entry() -{ - IDisplay& display = Board::getInstance().getDisplay(); - - DifferentialDrive::getInstance().disable(); - - display.clear(); - display.print("A: CONT"); - display.gotoXY(0, 1); - - if ('\0' == m_errorMsg[0]) - { - display.print("ERR"); - } - else - { - display.print(m_errorMsg); - } - - LOG_ERROR_VAL("Error: ", m_errorMsg); -} - -void ErrorState::process(StateMachine& sm) -{ - IButton& buttonA = Board::getInstance().getButtonA(); - - /* Restart calibration? */ - if (true == buttonA.isPressed()) - { - buttonA.waitForRelease(); - sm.setState(&StartupState::getInstance()); - } -} - -void ErrorState::exit() -{ - /* Nothing to do. */ -} - -void ErrorState::setErrorMsg(const char* msg) -{ - if (nullptr == msg) - { - m_errorMsg[0] = '\0'; - } - else - { - strncpy(m_errorMsg, msg, ERROR_MSG_SIZE - 1); - m_errorMsg[ERROR_MSG_SIZE - 1] = '\0'; - } -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Error state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ErrorState.h" +#include +#include +#include "StartupState.h" +#include +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/** + * Error logging tag. + */ +LOG_TAG("EState"); + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void ErrorState::entry() +{ + IDisplay& display = Board::getInstance().getDisplay(); + + DifferentialDrive::getInstance().disable(); + + display.clear(); + display.print("A: CONT"); + display.gotoXY(0, 1); + + if ('\0' == m_errorMsg[0]) + { + display.print("ERR"); + } + else + { + display.print(m_errorMsg); + } + + LOG_ERROR_VAL("Error: ", m_errorMsg); +} + +void ErrorState::process(StateMachine& sm) +{ + IButton& buttonA = Board::getInstance().getButtonA(); + + /* Restart calibration? */ + if (true == buttonA.isPressed()) + { + buttonA.waitForRelease(); + sm.setState(&StartupState::getInstance()); + } +} + +void ErrorState::exit() +{ + /* Nothing to do. */ +} + +void ErrorState::setErrorMsg(const char* msg) +{ + if (nullptr == msg) + { + m_errorMsg[0] = '\0'; + } + else + { + strncpy(m_errorMsg, msg, ERROR_MSG_SIZE - 1); + m_errorMsg[ERROR_MSG_SIZE - 1] = '\0'; + } +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPCalib/ErrorState.h b/lib/APPCalib/ErrorState.h index 74095203..0679e9a3 100644 --- a/lib/APPCalib/ErrorState.h +++ b/lib/APPCalib/ErrorState.h @@ -1,134 +1,134 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Error state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef ERROR_STATE_H -#define ERROR_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The system error state. */ -class ErrorState : public IState -{ -public: - /** - * Get state instance. - * - * @return State instance. - */ - static ErrorState& getInstance() - { - static ErrorState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - - /** - * Set error message, which to show on the display. - * - * @param[in] msg Error message - */ - void setErrorMsg(const char* msg); - -protected: -private: - /** - * The error message string size in bytes, which - * includes the terminating character. - */ - static const size_t ERROR_MSG_SIZE = 20; - - char m_errorMsg[ERROR_MSG_SIZE]; /**< Error message, which to show. */ - - /** - * Default constructor. - */ - ErrorState() : m_errorMsg() - { - m_errorMsg[0] = '\0'; - } - - /** - * Default destructor. - */ - ~ErrorState() - { - } - - /* Not allowed. */ - ErrorState(const ErrorState& state); /**< Copy construction of an instance. */ - ErrorState& operator=(const ErrorState& state); /**< Assignment of an instance. */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* ERROR_STATE_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Error state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef ERROR_STATE_H +#define ERROR_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The system error state. */ +class ErrorState : public IState +{ +public: + /** + * Get state instance. + * + * @return State instance. + */ + static ErrorState& getInstance() + { + static ErrorState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + + /** + * Set error message, which to show on the display. + * + * @param[in] msg Error message + */ + void setErrorMsg(const char* msg); + +protected: +private: + /** + * The error message string size in bytes, which + * includes the terminating character. + */ + static const size_t ERROR_MSG_SIZE = 20; + + char m_errorMsg[ERROR_MSG_SIZE]; /**< Error message, which to show. */ + + /** + * Default constructor. + */ + ErrorState() : m_errorMsg() + { + m_errorMsg[0] = '\0'; + } + + /** + * Default destructor. + */ + ~ErrorState() + { + } + + /* Not allowed. */ + ErrorState(const ErrorState& state); /**< Copy construction of an instance. */ + ErrorState& operator=(const ErrorState& state); /**< Assignment of an instance. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* ERROR_STATE_H */ +/** @} */ diff --git a/lib/APPCalib/MotorSpeedCalibrationState.cpp b/lib/APPCalib/MotorSpeedCalibrationState.cpp index e7d65626..1d80eef7 100644 --- a/lib/APPCalib/MotorSpeedCalibrationState.cpp +++ b/lib/APPCalib/MotorSpeedCalibrationState.cpp @@ -1,246 +1,246 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Motor speed calibration state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "MotorSpeedCalibrationState.h" -#include -#include -#include -#include -#include -#include -#include "ReadyState.h" -#include "ErrorState.h" -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/** - * Logging source. - */ -LOG_TAG("MSCState"); - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void MotorSpeedCalibrationState::entry() -{ - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - IDisplay& display = Board::getInstance().getDisplay(); - - display.clear(); - display.print("Run"); - display.gotoXY(0, 1); - display.print("MCAL"); - - /* Disable differential drive to avoid any bad influence. */ - diffDrive.disable(); - - /* Setup relative encoders */ - m_relEncoders.clear(); - - /* Set the max. speeds to maximum of datatype, because during calibration - * the max. speed is determined by the lowest motor speed. - */ - m_maxSpeedLeft = INT16_MAX; - m_maxSpeedRight = INT16_MAX; - - /* Wait some time, before starting the calibration drive. */ - m_phase = PHASE_1_BACK; - m_timer.start(WAIT_TIME); -} - -void MotorSpeedCalibrationState::process(StateMachine& sm) -{ - if (true == m_timer.isTimeout()) - { - /* Control motors directly and not via differential drive control, - * because the differential drive control needs first to be updated - * regarding the max. possible motor speed in [steps/s] which is - * determined by this calibration. - */ - IMotors& motors = Board::getInstance().getMotors(); - - switch (m_phase) - { - case PHASE_1_BACK: - /* Drive full back. */ - motors.setSpeeds(-motors.getMaxSpeed(), -motors.getMaxSpeed()); - - m_timer.start(CALIB_DURATION); - m_phase = PHASE_2_FORWARD; - break; - - case PHASE_2_FORWARD: - motors.setSpeeds(0, 0); - determineMaxMotorSpeed(); - - /* Drive full forward. */ - motors.setSpeeds(motors.getMaxSpeed(), motors.getMaxSpeed()); - - m_timer.restart(); - m_phase = PHASE_3_FINISHED; - break; - - case PHASE_3_FINISHED: - motors.setSpeeds(0, 0); - determineMaxMotorSpeed(); - - m_timer.stop(); - finishCalibration(sm); - break; - - default: - break; - } - } -} - -void MotorSpeedCalibrationState::exit() -{ - /* Nothing to do. */ -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -void MotorSpeedCalibrationState::determineMaxMotorSpeed() -{ - int32_t stepsLeft = 0; - int32_t stepsRight = 0; - - /* Determine max. speed backward. */ - stepsLeft = abs(m_relEncoders.getCountsLeft()); - stepsRight = abs(m_relEncoders.getCountsRight()); - - /* Convert number of steps to [steps/s] */ - stepsLeft *= 1000; - stepsLeft /= CALIB_DURATION; - stepsRight *= 1000; - stepsRight /= CALIB_DURATION; - - if (INT16_MAX >= stepsLeft) - { - /* Use lower speed to ensure that motor speed can be reached in both - * directions. - */ - if (stepsLeft < m_maxSpeedLeft) - { - m_maxSpeedLeft = static_cast(stepsLeft); - } - } - - if (INT16_MAX >= stepsRight) - { - /* Use lower speed to ensure that motor speed can be reached in both - * directions. - */ - if (stepsRight < m_maxSpeedRight) - { - m_maxSpeedRight = static_cast(stepsRight); - } - } - - /* Clear relative encoders */ - m_relEncoders.clear(); -} - -void MotorSpeedCalibrationState::finishCalibration(StateMachine& sm) -{ - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - ISettings& settings = Board::getInstance().getSettings(); - - /* Set the lower speed as max. motor speed to ensure that both motors - * can reach the same max. speed. - */ - int16_t maxSpeed = (m_maxSpeedLeft < m_maxSpeedRight) ? m_maxSpeedLeft : m_maxSpeedRight; - - /* Store calibrated max. motor speed in the settings. */ - settings.setMaxSpeed(maxSpeed); - - /* With setting the max. motor speed in [steps/s] the differential drive control - * can now be used. - */ - diffDrive.setMaxMotorSpeed(maxSpeed); - - /* Differential drive can now be used. */ - diffDrive.enable(); - - if (0 == maxSpeed) - { - ErrorState::getInstance().setErrorMsg("EMCAL 0"); - sm.setState(&ErrorState::getInstance()); - } - else - { - int32_t maxSpeed32 = - static_cast(maxSpeed) * 1000 / static_cast(RobotConstants::ENCODER_STEPS_PER_M); - - LOG_INFO_VAL("Calibrated max. speed (steps/s): ", maxSpeed); - LOG_INFO_VAL("Calibrated max. speed (mm/s): ", maxSpeed32); - - sm.setState(&ReadyState::getInstance()); - } -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Motor speed calibration state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "MotorSpeedCalibrationState.h" +#include +#include +#include +#include +#include +#include +#include "ReadyState.h" +#include "ErrorState.h" +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/** + * Logging source. + */ +LOG_TAG("MSCState"); + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void MotorSpeedCalibrationState::entry() +{ + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + IDisplay& display = Board::getInstance().getDisplay(); + + display.clear(); + display.print("Run"); + display.gotoXY(0, 1); + display.print("MCAL"); + + /* Disable differential drive to avoid any bad influence. */ + diffDrive.disable(); + + /* Setup relative encoders */ + m_relEncoders.clear(); + + /* Set the max. speeds to maximum of datatype, because during calibration + * the max. speed is determined by the lowest motor speed. + */ + m_maxSpeedLeft = INT16_MAX; + m_maxSpeedRight = INT16_MAX; + + /* Wait some time, before starting the calibration drive. */ + m_phase = PHASE_1_BACK; + m_timer.start(WAIT_TIME); +} + +void MotorSpeedCalibrationState::process(StateMachine& sm) +{ + if (true == m_timer.isTimeout()) + { + /* Control motors directly and not via differential drive control, + * because the differential drive control needs first to be updated + * regarding the max. possible motor speed in [steps/s] which is + * determined by this calibration. + */ + IMotors& motors = Board::getInstance().getMotors(); + + switch (m_phase) + { + case PHASE_1_BACK: + /* Drive full back. */ + motors.setSpeeds(-motors.getMaxSpeed(), -motors.getMaxSpeed()); + + m_timer.start(CALIB_DURATION); + m_phase = PHASE_2_FORWARD; + break; + + case PHASE_2_FORWARD: + motors.setSpeeds(0, 0); + determineMaxMotorSpeed(); + + /* Drive full forward. */ + motors.setSpeeds(motors.getMaxSpeed(), motors.getMaxSpeed()); + + m_timer.restart(); + m_phase = PHASE_3_FINISHED; + break; + + case PHASE_3_FINISHED: + motors.setSpeeds(0, 0); + determineMaxMotorSpeed(); + + m_timer.stop(); + finishCalibration(sm); + break; + + default: + break; + } + } +} + +void MotorSpeedCalibrationState::exit() +{ + /* Nothing to do. */ +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +void MotorSpeedCalibrationState::determineMaxMotorSpeed() +{ + int32_t stepsLeft = 0; + int32_t stepsRight = 0; + + /* Determine max. speed backward. */ + stepsLeft = abs(m_relEncoders.getCountsLeft()); + stepsRight = abs(m_relEncoders.getCountsRight()); + + /* Convert number of steps to [steps/s] */ + stepsLeft *= 1000; + stepsLeft /= CALIB_DURATION; + stepsRight *= 1000; + stepsRight /= CALIB_DURATION; + + if (INT16_MAX >= stepsLeft) + { + /* Use lower speed to ensure that motor speed can be reached in both + * directions. + */ + if (stepsLeft < m_maxSpeedLeft) + { + m_maxSpeedLeft = static_cast(stepsLeft); + } + } + + if (INT16_MAX >= stepsRight) + { + /* Use lower speed to ensure that motor speed can be reached in both + * directions. + */ + if (stepsRight < m_maxSpeedRight) + { + m_maxSpeedRight = static_cast(stepsRight); + } + } + + /* Clear relative encoders */ + m_relEncoders.clear(); +} + +void MotorSpeedCalibrationState::finishCalibration(StateMachine& sm) +{ + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + ISettings& settings = Board::getInstance().getSettings(); + + /* Set the lower speed as max. motor speed to ensure that both motors + * can reach the same max. speed. + */ + int16_t maxSpeed = (m_maxSpeedLeft < m_maxSpeedRight) ? m_maxSpeedLeft : m_maxSpeedRight; + + /* Store calibrated max. motor speed in the settings. */ + settings.setMaxSpeed(maxSpeed); + + /* With setting the max. motor speed in [steps/s] the differential drive control + * can now be used. + */ + diffDrive.setMaxMotorSpeed(maxSpeed); + + /* Differential drive can now be used. */ + diffDrive.enable(); + + if (0 == maxSpeed) + { + ErrorState::getInstance().setErrorMsg("EMCAL 0"); + sm.setState(&ErrorState::getInstance()); + } + else + { + int32_t maxSpeed32 = + static_cast(maxSpeed) * 1000 / static_cast(RobotConstants::ENCODER_STEPS_PER_M); + + LOG_INFO_VAL("Calibrated max. speed (steps/s): ", maxSpeed); + LOG_INFO_VAL("Calibrated max. speed (mm/s): ", maxSpeed32); + + sm.setState(&ReadyState::getInstance()); + } +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPCalib/MotorSpeedCalibrationState.h b/lib/APPCalib/MotorSpeedCalibrationState.h index 097cf12e..f4ea0bcc 100644 --- a/lib/APPCalib/MotorSpeedCalibrationState.h +++ b/lib/APPCalib/MotorSpeedCalibrationState.h @@ -1,165 +1,165 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Motor speed calibration state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef MOTOR_SPEED_CALIBRATION_STATE_H -#define MOTOR_SPEED_CALIBRATION_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The motor speed calibration state. */ -class MotorSpeedCalibrationState : public IState -{ -public: - /** - * Get state instance. - * - * @return State instance. - */ - static MotorSpeedCalibrationState& getInstance() - { - static MotorSpeedCalibrationState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - -protected: -private: - /** Calibration phases */ - enum Phase - { - PHASE_1_BACK, /**< Drive with max. speed backwards. */ - PHASE_2_FORWARD, /**< Drive with max. speed forwards. */ - PHASE_3_FINISHED /**< Calibration is finished. */ - }; - - /** - * Duration in ms about to wait, until the calibration drive starts. - */ - static const uint32_t WAIT_TIME = 1000; - - /** - * Calibration drive duration in ms. - * It means how long the robot is driven with max. speed forward/backward. - */ - static const uint32_t CALIB_DURATION = 1000; - - SimpleTimer m_timer; /**< Timer used to wait, until the calibration drive starts and for drive duration. */ - Phase m_phase; /**< Current calibration phase */ - int16_t m_maxSpeedLeft; /**< Max. determined left motor speed [steps/s]. */ - int16_t m_maxSpeedRight; /**< Max. determined right motor speed [steps/s]. */ - RelativeEncoders m_relEncoders; /**< Relative encoders left/right. */ - - /** - * Default constructor. - */ - MotorSpeedCalibrationState() : - m_timer(), - m_phase(PHASE_1_BACK), - m_maxSpeedLeft(0), - m_maxSpeedRight(0), - m_relEncoders(Board::getInstance().getEncoders()) - { - } - - /** - * Default destructor. - */ - ~MotorSpeedCalibrationState() - { - } - - /* Not allowed. */ - MotorSpeedCalibrationState(const MotorSpeedCalibrationState& state); /**< Copy construction of an instance. */ - MotorSpeedCalibrationState& operator=(const MotorSpeedCalibrationState& state); /**< Assignment of an instance. */ - - /** - * Determine the max. motor speed, considering both driving directions. - * There are two steps necessary: - * - Drive full backward and call this method to determine. - * - Drive full forward and call this method to determine. - */ - void determineMaxMotorSpeed(); - - /** - * Finish the calibration and determine next state. - * - * @param[in] sm State machine - */ - void finishCalibration(StateMachine& sm); -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* MOTOR_SPEED_CALIBRATION_STATE_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Motor speed calibration state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef MOTOR_SPEED_CALIBRATION_STATE_H +#define MOTOR_SPEED_CALIBRATION_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The motor speed calibration state. */ +class MotorSpeedCalibrationState : public IState +{ +public: + /** + * Get state instance. + * + * @return State instance. + */ + static MotorSpeedCalibrationState& getInstance() + { + static MotorSpeedCalibrationState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + +protected: +private: + /** Calibration phases */ + enum Phase + { + PHASE_1_BACK, /**< Drive with max. speed backwards. */ + PHASE_2_FORWARD, /**< Drive with max. speed forwards. */ + PHASE_3_FINISHED /**< Calibration is finished. */ + }; + + /** + * Duration in ms about to wait, until the calibration drive starts. + */ + static const uint32_t WAIT_TIME = 1000; + + /** + * Calibration drive duration in ms. + * It means how long the robot is driven with max. speed forward/backward. + */ + static const uint32_t CALIB_DURATION = 1000; + + SimpleTimer m_timer; /**< Timer used to wait, until the calibration drive starts and for drive duration. */ + Phase m_phase; /**< Current calibration phase */ + int16_t m_maxSpeedLeft; /**< Max. determined left motor speed [steps/s]. */ + int16_t m_maxSpeedRight; /**< Max. determined right motor speed [steps/s]. */ + RelativeEncoders m_relEncoders; /**< Relative encoders left/right. */ + + /** + * Default constructor. + */ + MotorSpeedCalibrationState() : + m_timer(), + m_phase(PHASE_1_BACK), + m_maxSpeedLeft(0), + m_maxSpeedRight(0), + m_relEncoders(Board::getInstance().getEncoders()) + { + } + + /** + * Default destructor. + */ + ~MotorSpeedCalibrationState() + { + } + + /* Not allowed. */ + MotorSpeedCalibrationState(const MotorSpeedCalibrationState& state); /**< Copy construction of an instance. */ + MotorSpeedCalibrationState& operator=(const MotorSpeedCalibrationState& state); /**< Assignment of an instance. */ + + /** + * Determine the max. motor speed, considering both driving directions. + * There are two steps necessary: + * - Drive full backward and call this method to determine. + * - Drive full forward and call this method to determine. + */ + void determineMaxMotorSpeed(); + + /** + * Finish the calibration and determine next state. + * + * @param[in] sm State machine + */ + void finishCalibration(StateMachine& sm); +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* MOTOR_SPEED_CALIBRATION_STATE_H */ +/** @} */ diff --git a/lib/APPCalib/ReadyState.cpp b/lib/APPCalib/ReadyState.cpp index 6a0cbe54..a5521357 100644 --- a/lib/APPCalib/ReadyState.cpp +++ b/lib/APPCalib/ReadyState.cpp @@ -1,196 +1,196 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Ready state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ReadyState.h" -#include -#include -#include -#include -#include -#include "DrivingState.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/** - * Logging source. - */ -LOG_TAG("RState"); - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void ReadyState::entry() -{ - showUserInfo(m_userInfoState); -} - -void ReadyState::process(StateMachine& sm) -{ - IBoard& board = Board::getInstance(); - IButton& buttonA = board.getButtonA(); - IButton& buttonB = board.getButtonB(); - IButton& buttonC = board.getButtonC(); - - /* Drive forward? */ - if (true == buttonA.isPressed()) - { - DrivingState::getInstance().setCmd(DrivingState::DRIVING_CMD_FORWARD); - buttonA.waitForRelease(); - m_releaseTimer.start(RELEASE_DURATION); - - LOG_INFO("Drive forward 10 cm."); - } - - /* Turn left? */ - if (true == buttonB.isPressed()) - { - DrivingState::getInstance().setCmd(DrivingState::DRIVING_CMD_TURN_LEFT); - buttonB.waitForRelease(); - m_releaseTimer.start(RELEASE_DURATION); - - LOG_INFO("Turn left 90°."); - } - - /* Turn right? */ - if (true == buttonC.isPressed()) - { - DrivingState::getInstance().setCmd(DrivingState::DRIVING_CMD_TURN_RIGHT); - buttonC.waitForRelease(); - m_releaseTimer.start(RELEASE_DURATION); - - LOG_INFO("Turn right 90°."); - } - - /* Release track after specific time. */ - if ((true == m_releaseTimer.isRunning()) && (true == m_releaseTimer.isTimeout())) - { - m_releaseTimer.stop(); - sm.setState(&DrivingState::getInstance()); - } - - /* Periodically change the user info on the display. */ - if (true == m_timer.isTimeout()) - { - int8_t next = m_userInfoState + 1; - - if (USER_INFO_COUNT <= next) - { - next = 0; - } - - showUserInfo(static_cast(next)); - m_timer.restart(); - } -} - -void ReadyState::exit() -{ - /* Next time start again from begin with the info. */ - m_userInfoState = USER_INFO_DRIVE_FORWARD; -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -void ReadyState::showUserInfo(UserInfo next) -{ - Board& board = Board::getInstance(); - IDisplay& display = board.getDisplay(); - - display.clear(); - - switch (next) - { - case USER_INFO_DRIVE_FORWARD: - display.print("A:"); - display.gotoXY(0, 1); - display.print("DRV FWD"); - break; - - case USER_INFO_TURN_LEFT: - display.print("B:"); - display.gotoXY(0, 1); - display.print("TURN L"); - break; - - case USER_INFO_TURN_RIGHT: - display.print("C:"); - display.gotoXY(0, 1); - display.print("TURN R"); - break; - - case USER_INFO_COUNT: - /* fallthrough */ - default: - display.print("?"); - next = USER_INFO_DRIVE_FORWARD; - break; - } - - m_userInfoState = next; - - m_timer.start(INFO_DURATION); -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Ready state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ReadyState.h" +#include +#include +#include +#include +#include +#include "DrivingState.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/** + * Logging source. + */ +LOG_TAG("RState"); + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void ReadyState::entry() +{ + showUserInfo(m_userInfoState); +} + +void ReadyState::process(StateMachine& sm) +{ + IBoard& board = Board::getInstance(); + IButton& buttonA = board.getButtonA(); + IButton& buttonB = board.getButtonB(); + IButton& buttonC = board.getButtonC(); + + /* Drive forward? */ + if (true == buttonA.isPressed()) + { + DrivingState::getInstance().setCmd(DrivingState::DRIVING_CMD_FORWARD); + buttonA.waitForRelease(); + m_releaseTimer.start(RELEASE_DURATION); + + LOG_INFO("Drive forward 10 cm."); + } + + /* Turn left? */ + if (true == buttonB.isPressed()) + { + DrivingState::getInstance().setCmd(DrivingState::DRIVING_CMD_TURN_LEFT); + buttonB.waitForRelease(); + m_releaseTimer.start(RELEASE_DURATION); + + LOG_INFO("Turn left 90°."); + } + + /* Turn right? */ + if (true == buttonC.isPressed()) + { + DrivingState::getInstance().setCmd(DrivingState::DRIVING_CMD_TURN_RIGHT); + buttonC.waitForRelease(); + m_releaseTimer.start(RELEASE_DURATION); + + LOG_INFO("Turn right 90°."); + } + + /* Release track after specific time. */ + if ((true == m_releaseTimer.isRunning()) && (true == m_releaseTimer.isTimeout())) + { + m_releaseTimer.stop(); + sm.setState(&DrivingState::getInstance()); + } + + /* Periodically change the user info on the display. */ + if (true == m_timer.isTimeout()) + { + int8_t next = m_userInfoState + 1; + + if (USER_INFO_COUNT <= next) + { + next = 0; + } + + showUserInfo(static_cast(next)); + m_timer.restart(); + } +} + +void ReadyState::exit() +{ + /* Next time start again from begin with the info. */ + m_userInfoState = USER_INFO_DRIVE_FORWARD; +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +void ReadyState::showUserInfo(UserInfo next) +{ + Board& board = Board::getInstance(); + IDisplay& display = board.getDisplay(); + + display.clear(); + + switch (next) + { + case USER_INFO_DRIVE_FORWARD: + display.print("A:"); + display.gotoXY(0, 1); + display.print("DRV FWD"); + break; + + case USER_INFO_TURN_LEFT: + display.print("B:"); + display.gotoXY(0, 1); + display.print("TURN L"); + break; + + case USER_INFO_TURN_RIGHT: + display.print("C:"); + display.gotoXY(0, 1); + display.print("TURN R"); + break; + + case USER_INFO_COUNT: + /* fallthrough */ + default: + display.print("?"); + next = USER_INFO_DRIVE_FORWARD; + break; + } + + m_userInfoState = next; + + m_timer.start(INFO_DURATION); +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPCalib/ReadyState.h b/lib/APPCalib/ReadyState.h index 8b451c7c..27721365 100644 --- a/lib/APPCalib/ReadyState.h +++ b/lib/APPCalib/ReadyState.h @@ -1,150 +1,150 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Ready state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef READY_STATE_H -#define READY_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The system ready state. */ -class ReadyState : public IState -{ -public: - /** - * Get state instance. - * - * @return State instance. - */ - static ReadyState& getInstance() - { - static ReadyState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - -protected: -private: - /** - * This type defines different kind of information, which will be shown - * to the user in the same order as defined. - */ - enum UserInfo - { - USER_INFO_DRIVE_FORWARD = 0, /**< Show button to use to drive forward. */ - USER_INFO_TURN_LEFT, /**< Show button to use to turn left 90°. */ - USER_INFO_TURN_RIGHT, /**< Show button to use to turn right 90°. */ - USER_INFO_COUNT /**< Number of user infos. */ - }; - - /** Release timer duration in ms. */ - static const uint32_t RELEASE_DURATION = 2000; - - /** - * Duration in ms how long a info on the display shall be shown, until - * the next info appears. - */ - static const uint32_t INFO_DURATION = 2000; - - SimpleTimer m_timer; /**< Used to show information for a certain time before changing to the next info. */ - UserInfo m_userInfoState; /**< Current user info state. */ - SimpleTimer m_releaseTimer; /**< Release timer */ - - /** - * Default constructor. - */ - ReadyState() : m_timer(), m_userInfoState(USER_INFO_DRIVE_FORWARD), m_releaseTimer() - { - } - - /** - * Default destructor. - */ - ~ReadyState() - { - } - - /* Not allowed. */ - ReadyState(const ReadyState& state); /**< Copy construction of an instance. */ - ReadyState& operator=(const ReadyState& state); /**< Assignment of an instance. */ - - /** - * Show next user info. - * - * @param[in] next Next user info which to show. - */ - void showUserInfo(UserInfo next); -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* READY_STATE_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Ready state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef READY_STATE_H +#define READY_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include + +/****************************************************************************** + * 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; + +protected: +private: + /** + * This type defines different kind of information, which will be shown + * to the user in the same order as defined. + */ + enum UserInfo + { + USER_INFO_DRIVE_FORWARD = 0, /**< Show button to use to drive forward. */ + USER_INFO_TURN_LEFT, /**< Show button to use to turn left 90°. */ + USER_INFO_TURN_RIGHT, /**< Show button to use to turn right 90°. */ + USER_INFO_COUNT /**< Number of user infos. */ + }; + + /** Release timer duration in ms. */ + static const uint32_t RELEASE_DURATION = 2000; + + /** + * Duration in ms how long a info on the display shall be shown, until + * the next info appears. + */ + static const uint32_t INFO_DURATION = 2000; + + SimpleTimer m_timer; /**< Used to show information for a certain time before changing to the next info. */ + UserInfo m_userInfoState; /**< Current user info state. */ + SimpleTimer m_releaseTimer; /**< Release timer */ + + /** + * Default constructor. + */ + ReadyState() : m_timer(), m_userInfoState(USER_INFO_DRIVE_FORWARD), m_releaseTimer() + { + } + + /** + * Default destructor. + */ + ~ReadyState() + { + } + + /* Not allowed. */ + ReadyState(const ReadyState& state); /**< Copy construction of an instance. */ + ReadyState& operator=(const ReadyState& state); /**< Assignment of an instance. */ + + /** + * Show next user info. + * + * @param[in] next Next user info which to show. + */ + void showUserInfo(UserInfo next); +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* READY_STATE_H */ +/** @} */ diff --git a/lib/APPCalib/StartupState.cpp b/lib/APPCalib/StartupState.cpp index 80020dff..a024758a 100644 --- a/lib/APPCalib/StartupState.cpp +++ b/lib/APPCalib/StartupState.cpp @@ -1,192 +1,192 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Startup state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "StartupState.h" -#include -#include -#include "MotorSpeedCalibrationState.h" -#include "ReadyState.h" -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void StartupState::entry() -{ - Board& board = Board::getInstance(); - ISettings& settings = board.getSettings(); - int16_t maxMotorSpeed = settings.getMaxSpeed(); - - /* If max. motor speed calibration value is available, the differential - * drive will be enabled. - */ - if (0 < maxMotorSpeed) - { - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - - /* With setting the max. motor speed in [steps/s] the differential drive control - * can now be used. - */ - diffDrive.setMaxMotorSpeed(maxMotorSpeed); - - /* Differential drive can now be used. */ - diffDrive.enable(); - - m_isMaxMotorSpeedCalibAvailable = true; - } - - showUserInfo(m_userInfoState); -} - -void StartupState::process(StateMachine& sm) -{ - IBoard& board = Board::getInstance(); - IButton& buttonA = board.getButtonA(); - IButton& buttonB = board.getButtonB(); - - /* Start max. motor speed calibration? */ - if (true == buttonA.isPressed()) - { - buttonA.waitForRelease(); - sm.setState(&MotorSpeedCalibrationState::getInstance()); - } - - /* If the max. motor speed calibration is done, it will be possible to - * continue immediately. - */ - if (true == m_isMaxMotorSpeedCalibAvailable) - { - /* Ready to drive? */ - if (true == buttonB.isPressed()) - { - buttonB.waitForRelease(); - sm.setState(&ReadyState::getInstance()); - } - } - - /* Periodically change the user info on the display. */ - if (true == m_timer.isTimeout()) - { - int8_t next = m_userInfoState + 1; - - if (USER_INFO_COUNT <= next) - { - next = 0; - } - - showUserInfo(static_cast(next)); - m_timer.restart(); - } -} - -void StartupState::exit() -{ - /* Next time start again from begin with the info. */ - m_userInfoState = USER_INFO_TEAM_NAME; -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -void StartupState::showUserInfo(UserInfo next) -{ - Board& board = Board::getInstance(); - IDisplay& display = board.getDisplay(); - - display.clear(); - - switch (next) - { - case USER_INFO_TEAM_NAME: - display.print(TEAM_NAME_LINE_1); - display.gotoXY(0, 1); - display.print(TEAM_NAME_LINE_2); - break; - - case USER_INFO_UI: - display.print("A: MCAL"); - - if (true == m_isMaxMotorSpeedCalibAvailable) - { - display.gotoXY(0, 1); - display.print("B: DRV"); - } - break; - - case USER_INFO_COUNT: - /* fallthrough */ - default: - display.print("?"); - next = USER_INFO_TEAM_NAME; - break; - } - - m_userInfoState = next; - - m_timer.start(INFO_DURATION); -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Startup state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "StartupState.h" +#include +#include +#include "MotorSpeedCalibrationState.h" +#include "ReadyState.h" +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void StartupState::entry() +{ + Board& board = Board::getInstance(); + ISettings& settings = board.getSettings(); + int16_t maxMotorSpeed = settings.getMaxSpeed(); + + /* If max. motor speed calibration value is available, the differential + * drive will be enabled. + */ + if (0 < maxMotorSpeed) + { + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + + /* With setting the max. motor speed in [steps/s] the differential drive control + * can now be used. + */ + diffDrive.setMaxMotorSpeed(maxMotorSpeed); + + /* Differential drive can now be used. */ + diffDrive.enable(); + + m_isMaxMotorSpeedCalibAvailable = true; + } + + showUserInfo(m_userInfoState); +} + +void StartupState::process(StateMachine& sm) +{ + IBoard& board = Board::getInstance(); + IButton& buttonA = board.getButtonA(); + IButton& buttonB = board.getButtonB(); + + /* Start max. motor speed calibration? */ + if (true == buttonA.isPressed()) + { + buttonA.waitForRelease(); + sm.setState(&MotorSpeedCalibrationState::getInstance()); + } + + /* If the max. motor speed calibration is done, it will be possible to + * continue immediately. + */ + if (true == m_isMaxMotorSpeedCalibAvailable) + { + /* Ready to drive? */ + if (true == buttonB.isPressed()) + { + buttonB.waitForRelease(); + sm.setState(&ReadyState::getInstance()); + } + } + + /* Periodically change the user info on the display. */ + if (true == m_timer.isTimeout()) + { + int8_t next = m_userInfoState + 1; + + if (USER_INFO_COUNT <= next) + { + next = 0; + } + + showUserInfo(static_cast(next)); + m_timer.restart(); + } +} + +void StartupState::exit() +{ + /* Next time start again from begin with the info. */ + m_userInfoState = USER_INFO_TEAM_NAME; +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +void StartupState::showUserInfo(UserInfo next) +{ + Board& board = Board::getInstance(); + IDisplay& display = board.getDisplay(); + + display.clear(); + + switch (next) + { + case USER_INFO_TEAM_NAME: + display.print(TEAM_NAME_LINE_1); + display.gotoXY(0, 1); + display.print(TEAM_NAME_LINE_2); + break; + + case USER_INFO_UI: + display.print("A: MCAL"); + + if (true == m_isMaxMotorSpeedCalibAvailable) + { + display.gotoXY(0, 1); + display.print("B: DRV"); + } + break; + + case USER_INFO_COUNT: + /* fallthrough */ + default: + display.print("?"); + next = USER_INFO_TEAM_NAME; + break; + } + + m_userInfoState = next; + + m_timer.start(INFO_DURATION); +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPCalib/StartupState.h b/lib/APPCalib/StartupState.h index 51c4f6a5..4be1f2d2 100644 --- a/lib/APPCalib/StartupState.h +++ b/lib/APPCalib/StartupState.h @@ -1,147 +1,147 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Startup state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef STARTUP_STATE_H -#define STARTUP_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The system startup state. */ -class StartupState : public IState -{ -public: - /** - * Get state instance. - * - * @return State instance. - */ - static StartupState& getInstance() - { - static StartupState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - -protected: -private: - /** - * This type defines different kind of information, which will be shown - * to the user in the same order as defined. - */ - enum UserInfo - { - USER_INFO_TEAM_NAME = 0, /**< Show the team name. */ - USER_INFO_UI, /**< Show the user interface. */ - USER_INFO_COUNT /**< Number of user infos. */ - }; - - /** - * Duration in ms how long a info on the display shall be shown, until - * the next info appears. - */ - static const uint32_t INFO_DURATION = 2000; - - bool m_isMaxMotorSpeedCalibAvailable; /**< Is max. motor speed calibration value available? */ - SimpleTimer m_timer; /**< Used to show information for a certain time before changing to the next info. */ - UserInfo m_userInfoState; /**< Current user info state. */ - - /** - * Default constructor. - */ - StartupState() : m_isMaxMotorSpeedCalibAvailable(false), m_timer(), m_userInfoState(USER_INFO_TEAM_NAME) - { - } - - /** - * Default destructor. - */ - ~StartupState() - { - } - - /* Not allowed. */ - StartupState(const StartupState& state); /**< Copy construction of an instance. */ - StartupState& operator=(const StartupState& state); /**< Assignment of an instance. */ - - /** - * Show next user info. - * - * @param[in] next Next user info which to show. - */ - void showUserInfo(UserInfo next); -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* STARTUP_STATE_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Startup state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef STARTUP_STATE_H +#define STARTUP_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The system startup state. */ +class StartupState : public IState +{ +public: + /** + * Get state instance. + * + * @return State instance. + */ + static StartupState& getInstance() + { + static StartupState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + +protected: +private: + /** + * This type defines different kind of information, which will be shown + * to the user in the same order as defined. + */ + enum UserInfo + { + USER_INFO_TEAM_NAME = 0, /**< Show the team name. */ + USER_INFO_UI, /**< Show the user interface. */ + USER_INFO_COUNT /**< Number of user infos. */ + }; + + /** + * Duration in ms how long a info on the display shall be shown, until + * the next info appears. + */ + static const uint32_t INFO_DURATION = 2000; + + bool m_isMaxMotorSpeedCalibAvailable; /**< Is max. motor speed calibration value available? */ + SimpleTimer m_timer; /**< Used to show information for a certain time before changing to the next info. */ + UserInfo m_userInfoState; /**< Current user info state. */ + + /** + * Default constructor. + */ + StartupState() : m_isMaxMotorSpeedCalibAvailable(false), m_timer(), m_userInfoState(USER_INFO_TEAM_NAME) + { + } + + /** + * Default destructor. + */ + ~StartupState() + { + } + + /* Not allowed. */ + StartupState(const StartupState& state); /**< Copy construction of an instance. */ + StartupState& operator=(const StartupState& state); /**< Assignment of an instance. */ + + /** + * Show next user info. + * + * @param[in] next Next user info which to show. + */ + void showUserInfo(UserInfo next); +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* STARTUP_STATE_H */ +/** @} */ diff --git a/lib/APPCalib/library.json b/lib/APPCalib/library.json index 73b259d3..7f4dffc2 100644 --- a/lib/APPCalib/library.json +++ b/lib/APPCalib/library.json @@ -1,19 +1,19 @@ -{ - "name": "APPCalib", - "version": "0.1.0", - "description": "Calibration 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": "APPCalib", + "version": "0.1.0", + "description": "Calibration 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/APPConvoyFollower/App.h b/lib/APPConvoyFollower/App.h index 70b590b2..7db5be14 100644 --- a/lib/APPConvoyFollower/App.h +++ b/lib/APPConvoyFollower/App.h @@ -1,206 +1,206 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief ConvoyFollower application - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef APP_H -#define APP_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ - -#include "SerialMuxChannels.h" -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The convoy follower application. */ -class App -{ -public: - /** - * Construct the convoy follower application. - */ - App() : - m_serialMuxProtChannelIdRemoteCtrlRsp(0U), - m_serialMuxProtChannelIdCurrentVehicleData(0U), - m_serialMuxProtChannelIdStatus(0U), - m_serialMuxProtChannelIdLineSensors(0U), - m_systemStateMachine(), - m_controlInterval(), - m_reportTimer(), - m_statusTimer(), - m_statusTimeoutTimer(), - m_sendLineSensorsDataInterval(), - m_smpServer(Serial, this), - m_movAvgProximitySensor() - { - } - - /** - * Destroy the convoy follower application. - */ - ~App() - { - } - - /** - * Setup the application. - */ - void setup(); - - /** - * Process the application periodically. - */ - void loop(); - - /** - * Handle remote command received via SerialMuxProt. - * - * @param[in] cmd Command to handle. - */ - void handleRemoteCommand(const Command& cmd); - - /** - * System Status callback. - * - * @param[in] status System status - */ - void systemStatusCallback(SMPChannelPayload::Status status); - -private: - /** Differential drive control period in ms. */ - static const uint32_t DIFFERENTIAL_DRIVE_CONTROL_PERIOD = 5U; - - /** Current data reporting period in ms. */ - static const uint32_t REPORTING_PERIOD = 50U; - - /** Baudrate for Serial Communication */ - static const uint32_t SERIAL_BAUDRATE = 115200U; - - /** Send status timer interval in ms. */ - static const uint32_t SEND_STATUS_TIMER_INTERVAL = 1000U; - - /** Status timeout timer interval in ms. */ - static const uint32_t STATUS_TIMEOUT_TIMER_INTERVAL = 2U * SEND_STATUS_TIMER_INTERVAL; - - /** Sending Data period in ms. */ - static const uint32_t SEND_LINE_SENSORS_DATA_PERIOD = 20U; - - /** - * Number of measurements for proximity sensors moving average filter. - */ - static const uint8_t MOVAVG_PROXIMITY_SENSOR_NUM_MEASUREMENTS = 3U; - - /** SerialMuxProt Channel id for sending remote control command responses. */ - uint8_t m_serialMuxProtChannelIdRemoteCtrlRsp; - - /** SerialMuxProt Channel id for sending the current vehicle data. */ - uint8_t m_serialMuxProtChannelIdCurrentVehicleData; - - /** SerialMuxProt Channel id for sending system status. */ - uint8_t m_serialMuxProtChannelIdStatus; - - /** SerialMuxProt Channel id for sending line sensors data. */ - uint8_t m_serialMuxProtChannelIdLineSensors; - - /** 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; - - /** Timer for sending system status to DCS. */ - SimpleTimer m_statusTimer; - - /** Timer for timeout of system status of DCS. */ - SimpleTimer m_statusTimeoutTimer; - - /** Timer used for sending data periodically. */ - SimpleTimer m_sendLineSensorsDataInterval; - - /** SerialMuxProt Server Instance. */ - SMPServer m_smpServer; - - /** - * Moving average filter for proximity sensors. - */ - MovAvg m_movAvgProximitySensor; - - /** - * Report the current vehicle data. - * Report the current position and heading of the robot using the Odometry data. - * Report the current motor speeds of the robot using the Speedometer data. - * Sends data through the SerialMuxProtServer. - */ - void reportVehicleData(); - - /** - * Setup the SerialMuxProt channels. - * - * @return If successful returns true, otherwise false. - */ - bool setupSerialMuxProt(); - - /** - * Send line sensors data via SerialMuxProt. - */ - void sendLineSensorsData() const; - - /* Not allowed. */ - App(const App& app); /**< Copy construction of an instance. */ - App& operator=(const App& app); /**< Assignment of an instance. */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* APP_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief ConvoyFollower application + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef APP_H +#define APP_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ + +#include "SerialMuxChannels.h" +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The convoy follower application. */ +class App +{ +public: + /** + * Construct the convoy follower application. + */ + App() : + m_serialMuxProtChannelIdRemoteCtrlRsp(0U), + m_serialMuxProtChannelIdCurrentVehicleData(0U), + m_serialMuxProtChannelIdStatus(0U), + m_serialMuxProtChannelIdLineSensors(0U), + m_systemStateMachine(), + m_controlInterval(), + m_reportTimer(), + m_statusTimer(), + m_statusTimeoutTimer(), + m_sendLineSensorsDataInterval(), + m_smpServer(Serial, this), + m_movAvgProximitySensor() + { + } + + /** + * Destroy the convoy follower application. + */ + ~App() + { + } + + /** + * Setup the application. + */ + void setup(); + + /** + * Process the application periodically. + */ + void loop(); + + /** + * Handle remote command received via SerialMuxProt. + * + * @param[in] cmd Command to handle. + */ + void handleRemoteCommand(const Command& cmd); + + /** + * System Status callback. + * + * @param[in] status System status + */ + void systemStatusCallback(SMPChannelPayload::Status status); + +private: + /** Differential drive control period in ms. */ + static const uint32_t DIFFERENTIAL_DRIVE_CONTROL_PERIOD = 5U; + + /** Current data reporting period in ms. */ + static const uint32_t REPORTING_PERIOD = 50U; + + /** Baudrate for Serial Communication */ + static const uint32_t SERIAL_BAUDRATE = 115200U; + + /** Send status timer interval in ms. */ + static const uint32_t SEND_STATUS_TIMER_INTERVAL = 1000U; + + /** Status timeout timer interval in ms. */ + static const uint32_t STATUS_TIMEOUT_TIMER_INTERVAL = 2U * SEND_STATUS_TIMER_INTERVAL; + + /** Sending Data period in ms. */ + static const uint32_t SEND_LINE_SENSORS_DATA_PERIOD = 20U; + + /** + * Number of measurements for proximity sensors moving average filter. + */ + static const uint8_t MOVAVG_PROXIMITY_SENSOR_NUM_MEASUREMENTS = 3U; + + /** SerialMuxProt Channel id for sending remote control command responses. */ + uint8_t m_serialMuxProtChannelIdRemoteCtrlRsp; + + /** SerialMuxProt Channel id for sending the current vehicle data. */ + uint8_t m_serialMuxProtChannelIdCurrentVehicleData; + + /** SerialMuxProt Channel id for sending system status. */ + uint8_t m_serialMuxProtChannelIdStatus; + + /** SerialMuxProt Channel id for sending line sensors data. */ + uint8_t m_serialMuxProtChannelIdLineSensors; + + /** 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; + + /** Timer for sending system status to DCS. */ + SimpleTimer m_statusTimer; + + /** Timer for timeout of system status of DCS. */ + SimpleTimer m_statusTimeoutTimer; + + /** Timer used for sending data periodically. */ + SimpleTimer m_sendLineSensorsDataInterval; + + /** SerialMuxProt Server Instance. */ + SMPServer m_smpServer; + + /** + * Moving average filter for proximity sensors. + */ + MovAvg m_movAvgProximitySensor; + + /** + * Report the current vehicle data. + * Report the current position and heading of the robot using the Odometry data. + * Report the current motor speeds of the robot using the Speedometer data. + * Sends data through the SerialMuxProtServer. + */ + void reportVehicleData(); + + /** + * Setup the SerialMuxProt channels. + * + * @return If successful returns true, otherwise false. + */ + bool setupSerialMuxProt(); + + /** + * Send line sensors data via SerialMuxProt. + */ + void sendLineSensorsData() const; + + /* Not allowed. */ + App(const App& app); /**< Copy construction of an instance. */ + App& operator=(const App& app); /**< Assignment of an instance. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* APP_H */ +/** @} */ diff --git a/lib/APPConvoyFollower/DrivingState.cpp b/lib/APPConvoyFollower/DrivingState.cpp index 04ce7176..f4bd07e2 100644 --- a/lib/APPConvoyFollower/DrivingState.cpp +++ b/lib/APPConvoyFollower/DrivingState.cpp @@ -1,109 +1,109 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Driving state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "DrivingState.h" -#include -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void DrivingState::entry() -{ - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - - m_isActive = true; - diffDrive.setLinearSpeed(0, 0); - diffDrive.enable(); -} - -void DrivingState::process(StateMachine& sm) -{ - /* Nothing to do. */ - (void)sm; -} - -void DrivingState::exit() -{ - m_isActive = false; - - /* Stop motors. */ - DifferentialDrive::getInstance().setLinearSpeed(0, 0); - DifferentialDrive::getInstance().disable(); -} - -void DrivingState::setTargetSpeeds(int16_t leftMotor, int16_t rightMotor) -{ - if (true == m_isActive) - { - DifferentialDrive::getInstance().setLinearSpeed(leftMotor, rightMotor); - } -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Driving state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "DrivingState.h" +#include +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void DrivingState::entry() +{ + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + + m_isActive = true; + diffDrive.setLinearSpeed(0, 0); + diffDrive.enable(); +} + +void DrivingState::process(StateMachine& sm) +{ + /* Nothing to do. */ + (void)sm; +} + +void DrivingState::exit() +{ + m_isActive = false; + + /* Stop motors. */ + DifferentialDrive::getInstance().setLinearSpeed(0, 0); + DifferentialDrive::getInstance().disable(); +} + +void DrivingState::setTargetSpeeds(int16_t leftMotor, int16_t rightMotor) +{ + if (true == m_isActive) + { + DifferentialDrive::getInstance().setLinearSpeed(leftMotor, rightMotor); + } +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPConvoyFollower/DrivingState.h b/lib/APPConvoyFollower/DrivingState.h index b336c119..92f48a94 100644 --- a/lib/APPConvoyFollower/DrivingState.h +++ b/lib/APPConvoyFollower/DrivingState.h @@ -1,130 +1,130 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Driving state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef DRIVING_STATE_H -#define DRIVING_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The system driving state. */ -class DrivingState : public IState -{ -public: - /** - * Get state instance. - * - * @return State instance. - */ - static DrivingState& getInstance() - { - static DrivingState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - - /** - * Set target motor speeds. - * - * @param[in] leftMotor Left motor speed. [steps/s] - * @param[in] rightMotor Right motor speed. [steps/s] - */ - void setTargetSpeeds(int16_t leftMotor, int16_t rightMotor); - -protected: -private: - /** Flag: State is active. */ - bool m_isActive; - - /** - * Default constructor. - */ - DrivingState() : IState(), m_isActive(false) - { - } - - /** - * Default destructor. - */ - ~DrivingState() - { - } - - /* Not allowed. */ - DrivingState(const DrivingState& state); /**< Copy construction of an instance. */ - DrivingState& operator=(const DrivingState& state); /**< Assignment of an instance. */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* DRIVING_STATE_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Driving state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef DRIVING_STATE_H +#define DRIVING_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The system driving state. */ +class DrivingState : public IState +{ +public: + /** + * Get state instance. + * + * @return State instance. + */ + static DrivingState& getInstance() + { + static DrivingState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + + /** + * Set target motor speeds. + * + * @param[in] leftMotor Left motor speed. [steps/s] + * @param[in] rightMotor Right motor speed. [steps/s] + */ + void setTargetSpeeds(int16_t leftMotor, int16_t rightMotor); + +protected: +private: + /** Flag: State is active. */ + bool m_isActive; + + /** + * Default constructor. + */ + DrivingState() : IState(), m_isActive(false) + { + } + + /** + * Default destructor. + */ + ~DrivingState() + { + } + + /* Not allowed. */ + DrivingState(const DrivingState& state); /**< Copy construction of an instance. */ + DrivingState& operator=(const DrivingState& state); /**< Assignment of an instance. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* DRIVING_STATE_H */ +/** @} */ diff --git a/lib/APPConvoyFollower/ErrorState.cpp b/lib/APPConvoyFollower/ErrorState.cpp index cefecb8d..dff63afd 100644 --- a/lib/APPConvoyFollower/ErrorState.cpp +++ b/lib/APPConvoyFollower/ErrorState.cpp @@ -1,113 +1,113 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Error state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ErrorState.h" -#include -#include -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void ErrorState::entry() -{ - IDisplay& display = Board::getInstance().getDisplay(); - DifferentialDrive::getInstance().disable(); - - display.clear(); - display.print("Error"); - display.gotoXY(0, 1); - display.print(m_errorMsg); -} - -void ErrorState::process(StateMachine& sm) -{ - /* Nothing to do. */ - (void)sm; -} - -void ErrorState::exit() -{ - /* Nothing to do. */ -} - -void ErrorState::setErrorMsg(const char* msg) -{ - if (nullptr == msg) - { - m_errorMsg[0] = '\0'; - } - else - { - strncpy(m_errorMsg, msg, ERROR_MSG_SIZE - 1); - m_errorMsg[ERROR_MSG_SIZE - 1] = '\0'; - } -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Error state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ErrorState.h" +#include +#include +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void ErrorState::entry() +{ + IDisplay& display = Board::getInstance().getDisplay(); + DifferentialDrive::getInstance().disable(); + + display.clear(); + display.print("Error"); + display.gotoXY(0, 1); + display.print(m_errorMsg); +} + +void ErrorState::process(StateMachine& sm) +{ + /* Nothing to do. */ + (void)sm; +} + +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/APPConvoyFollower/ErrorState.h b/lib/APPConvoyFollower/ErrorState.h index 74095203..0679e9a3 100644 --- a/lib/APPConvoyFollower/ErrorState.h +++ b/lib/APPConvoyFollower/ErrorState.h @@ -1,134 +1,134 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Error state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef ERROR_STATE_H -#define ERROR_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The system error state. */ -class ErrorState : public IState -{ -public: - /** - * Get state instance. - * - * @return State instance. - */ - static ErrorState& getInstance() - { - static ErrorState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - - /** - * Set error message, which to show on the display. - * - * @param[in] msg Error message - */ - void setErrorMsg(const char* msg); - -protected: -private: - /** - * The error message string size in bytes, which - * includes the terminating character. - */ - static const size_t ERROR_MSG_SIZE = 20; - - char m_errorMsg[ERROR_MSG_SIZE]; /**< Error message, which to show. */ - - /** - * Default constructor. - */ - ErrorState() : m_errorMsg() - { - m_errorMsg[0] = '\0'; - } - - /** - * Default destructor. - */ - ~ErrorState() - { - } - - /* Not allowed. */ - ErrorState(const ErrorState& state); /**< Copy construction of an instance. */ - ErrorState& operator=(const ErrorState& state); /**< Assignment of an instance. */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* ERROR_STATE_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Error state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef ERROR_STATE_H +#define ERROR_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The system error state. */ +class ErrorState : public IState +{ +public: + /** + * Get state instance. + * + * @return State instance. + */ + static ErrorState& getInstance() + { + static ErrorState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + + /** + * Set error message, which to show on the display. + * + * @param[in] msg Error message + */ + void setErrorMsg(const char* msg); + +protected: +private: + /** + * The error message string size in bytes, which + * includes the terminating character. + */ + static const size_t ERROR_MSG_SIZE = 20; + + char m_errorMsg[ERROR_MSG_SIZE]; /**< Error message, which to show. */ + + /** + * Default constructor. + */ + ErrorState() : m_errorMsg() + { + m_errorMsg[0] = '\0'; + } + + /** + * Default destructor. + */ + ~ErrorState() + { + } + + /* Not allowed. */ + ErrorState(const ErrorState& state); /**< Copy construction of an instance. */ + ErrorState& operator=(const ErrorState& state); /**< Assignment of an instance. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* ERROR_STATE_H */ +/** @} */ diff --git a/lib/APPConvoyFollower/LineSensorsCalibrationState.cpp b/lib/APPConvoyFollower/LineSensorsCalibrationState.cpp index c54cff2d..bd4c34b1 100644 --- a/lib/APPConvoyFollower/LineSensorsCalibrationState.cpp +++ b/lib/APPConvoyFollower/LineSensorsCalibrationState.cpp @@ -1,213 +1,213 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Line sensors calibration state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "LineSensorsCalibrationState.h" -#include -#include -#include -#include -#include -#include "ErrorState.h" -#include "DrivingState.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void LineSensorsCalibrationState::entry() -{ - IDisplay& display = Board::getInstance().getDisplay(); - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - Odometry& odometry = Odometry::getInstance(); - ILineSensors& lineSensors = Board::getInstance().getLineSensors(); - - display.clear(); - display.print("Calib"); - display.gotoXY(0, 1); - display.print("Lines"); - - /* Prepare calibration drive. */ - m_calibrationSpeed = diffDrive.getMaxMotorSpeed() / 3; - m_orientation = odometry.getOrientation(); - - /* Mandatory for each new calibration. */ - lineSensors.resetCalibration(); - - /* Wait some time, before starting the calibration drive. */ - m_phase = PHASE_1_WAIT; - m_timer.start(WAIT_TIME); -} - -void LineSensorsCalibrationState::process(StateMachine& sm) -{ - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - - switch (m_phase) - { - case PHASE_1_WAIT: - if (true == m_timer.isTimeout()) - { - m_phase = PHASE_2_TURN_LEFT; - diffDrive.setLinearSpeed(-m_calibrationSpeed, m_calibrationSpeed); - } - break; - - case PHASE_2_TURN_LEFT: - if (true == turnAndCalibrate(CALIB_ANGLE, true)) - { - m_phase = PHASE_3_TURN_RIGHT; - diffDrive.setLinearSpeed(m_calibrationSpeed, -m_calibrationSpeed); - } - break; - - case PHASE_3_TURN_RIGHT: - if (true == turnAndCalibrate(-CALIB_ANGLE, false)) - { - m_phase = PHASE_4_TURN_ORIG; - diffDrive.setLinearSpeed(-m_calibrationSpeed, m_calibrationSpeed); - } - break; - - case PHASE_4_TURN_ORIG: - if (true == turnAndCalibrate(0, true)) - { - m_phase = PHASE_5_FINISHED; - diffDrive.setLinearSpeed(0, 0); - finishCalibration(sm); - } - break; - - case PHASE_5_FINISHED: - /* fallthrough */ - default: - break; - } -} - -void LineSensorsCalibrationState::exit() -{ - m_timer.stop(); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -bool LineSensorsCalibrationState::turnAndCalibrate(int32_t calibAlpha, bool isGreaterEqual) -{ - ILineSensors& lineSensors = Board::getInstance().getLineSensors(); - Odometry& odometry = Odometry::getInstance(); - int32_t alpha = odometry.getOrientation() - m_orientation; /* [mrad] */ - bool isSuccesful = false; - - /* Continously calibrate the line sensors. */ - lineSensors.calibrate(); - - /* Is the goal that the current angle shall be lower or equal than the destination calibration angle? */ - if (false == isGreaterEqual) - { - /* Is alpha lower or equal than the destination calibration angle? */ - if (calibAlpha >= alpha) - { - isSuccesful = true; - } - } - else - { - /* Is alpha greater or equal than the destination calibration angle? */ - if (calibAlpha <= alpha) - { - isSuccesful = true; - } - } - - return isSuccesful; -} - -void LineSensorsCalibrationState::finishCalibration(StateMachine& sm) -{ - ILineSensors& lineSensors = Board::getInstance().getLineSensors(); - - if (false == lineSensors.isCalibrationSuccessful()) - { - char str[10]; - char valueStr[10]; - - Util::uintToStr(valueStr, sizeof(valueStr), lineSensors.getCalibErrorInfo()); - - strncpy(str, "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(&DrivingState::getInstance()); - } -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Line sensors calibration state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "LineSensorsCalibrationState.h" +#include +#include +#include +#include +#include +#include "ErrorState.h" +#include "DrivingState.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void LineSensorsCalibrationState::entry() +{ + IDisplay& display = Board::getInstance().getDisplay(); + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + Odometry& odometry = Odometry::getInstance(); + ILineSensors& lineSensors = Board::getInstance().getLineSensors(); + + display.clear(); + display.print("Calib"); + display.gotoXY(0, 1); + display.print("Lines"); + + /* Prepare calibration drive. */ + m_calibrationSpeed = diffDrive.getMaxMotorSpeed() / 3; + m_orientation = odometry.getOrientation(); + + /* Mandatory for each new calibration. */ + lineSensors.resetCalibration(); + + /* Wait some time, before starting the calibration drive. */ + m_phase = PHASE_1_WAIT; + m_timer.start(WAIT_TIME); +} + +void LineSensorsCalibrationState::process(StateMachine& sm) +{ + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + + switch (m_phase) + { + case PHASE_1_WAIT: + if (true == m_timer.isTimeout()) + { + m_phase = PHASE_2_TURN_LEFT; + diffDrive.setLinearSpeed(-m_calibrationSpeed, m_calibrationSpeed); + } + break; + + case PHASE_2_TURN_LEFT: + if (true == turnAndCalibrate(CALIB_ANGLE, true)) + { + m_phase = PHASE_3_TURN_RIGHT; + diffDrive.setLinearSpeed(m_calibrationSpeed, -m_calibrationSpeed); + } + break; + + case PHASE_3_TURN_RIGHT: + if (true == turnAndCalibrate(-CALIB_ANGLE, false)) + { + m_phase = PHASE_4_TURN_ORIG; + diffDrive.setLinearSpeed(-m_calibrationSpeed, m_calibrationSpeed); + } + break; + + case PHASE_4_TURN_ORIG: + if (true == turnAndCalibrate(0, true)) + { + m_phase = PHASE_5_FINISHED; + diffDrive.setLinearSpeed(0, 0); + finishCalibration(sm); + } + break; + + case PHASE_5_FINISHED: + /* fallthrough */ + default: + break; + } +} + +void LineSensorsCalibrationState::exit() +{ + m_timer.stop(); +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +bool LineSensorsCalibrationState::turnAndCalibrate(int32_t calibAlpha, bool isGreaterEqual) +{ + ILineSensors& lineSensors = Board::getInstance().getLineSensors(); + Odometry& odometry = Odometry::getInstance(); + int32_t alpha = odometry.getOrientation() - m_orientation; /* [mrad] */ + bool isSuccesful = false; + + /* Continously calibrate the line sensors. */ + lineSensors.calibrate(); + + /* Is the goal that the current angle shall be lower or equal than the destination calibration angle? */ + if (false == isGreaterEqual) + { + /* Is alpha lower or equal than the destination calibration angle? */ + if (calibAlpha >= alpha) + { + isSuccesful = true; + } + } + else + { + /* Is alpha greater or equal than the destination calibration angle? */ + if (calibAlpha <= alpha) + { + isSuccesful = true; + } + } + + return isSuccesful; +} + +void LineSensorsCalibrationState::finishCalibration(StateMachine& sm) +{ + ILineSensors& lineSensors = Board::getInstance().getLineSensors(); + + if (false == lineSensors.isCalibrationSuccessful()) + { + char str[10]; + char valueStr[10]; + + Util::uintToStr(valueStr, sizeof(valueStr), lineSensors.getCalibErrorInfo()); + + strncpy(str, "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(&DrivingState::getInstance()); + } +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPConvoyFollower/LineSensorsCalibrationState.h b/lib/APPConvoyFollower/LineSensorsCalibrationState.h index f59dd3c4..1ee29963 100644 --- a/lib/APPConvoyFollower/LineSensorsCalibrationState.h +++ b/lib/APPConvoyFollower/LineSensorsCalibrationState.h @@ -1,161 +1,161 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Line sensors calibration state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef LINE_SENSORS_CALIBRATION_STATE_H -#define LINE_SENSORS_CALIBRATION_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The line sensors calibration state. */ -class LineSensorsCalibrationState : public IState -{ -public: - /** - * Get state instance. - * - * @return State instance. - */ - static LineSensorsCalibrationState& getInstance() - { - static LineSensorsCalibrationState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - -protected: -private: - /** Calibration phases */ - enum Phase - { - PHASE_1_WAIT = 0, /**< Wait a short time before starting the calibration. */ - PHASE_2_TURN_LEFT, /**< Turn line sensors left over the line. */ - PHASE_3_TURN_RIGHT, /**< Turn line sensor right over the line. */ - PHASE_4_TURN_ORIG, /**< Turn back to origin. */ - PHASE_5_FINISHED /**< Calibration is finished. */ - }; - - /** - * Duration in ms about to wait, until the calibration drive starts. - */ - static const uint32_t WAIT_TIME = 1000; - - /** - * Calibration turn angle in mrad (corresponds to 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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Line sensors calibration state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef LINE_SENSORS_CALIBRATION_STATE_H +#define LINE_SENSORS_CALIBRATION_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The line sensors calibration state. */ +class LineSensorsCalibrationState : public IState +{ +public: + /** + * Get state instance. + * + * @return State instance. + */ + static LineSensorsCalibrationState& getInstance() + { + static LineSensorsCalibrationState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + +protected: +private: + /** Calibration phases */ + enum Phase + { + PHASE_1_WAIT = 0, /**< Wait a short time before starting the calibration. */ + PHASE_2_TURN_LEFT, /**< Turn line sensors left over the line. */ + PHASE_3_TURN_RIGHT, /**< Turn line sensor right over the line. */ + PHASE_4_TURN_ORIG, /**< Turn back to origin. */ + PHASE_5_FINISHED /**< Calibration is finished. */ + }; + + /** + * Duration in ms about to wait, until the calibration drive starts. + */ + static const uint32_t WAIT_TIME = 1000; + + /** + * Calibration turn angle in mrad (corresponds to 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/APPConvoyFollower/SerialMuxChannels.h b/lib/APPConvoyFollower/SerialMuxChannels.h index d8660246..c79b9754 100644 --- a/lib/APPConvoyFollower/SerialMuxChannels.h +++ b/lib/APPConvoyFollower/SerialMuxChannels.h @@ -1,212 +1,212 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Channel structure definition for the SerialMuxProt. - * @author Gabryel Reyes - */ - -#ifndef SERIAL_MUX_CHANNELS_H_ -#define SERIAL_MUX_CHANNELS_H_ - -/****************************************************************************** - * Includes - *****************************************************************************/ - -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/** Maximum number of SerialMuxProt Channels. */ -#define MAX_CHANNELS (10U) - -/** Name of Channel to send Commands to. */ -#define COMMAND_CHANNEL_NAME "CMD" - -/** DLC of Command Channel. */ -#define COMMAND_CHANNEL_DLC (sizeof(Command)) - -/** Name of Channel to receive Command Responses from. */ -#define COMMAND_RESPONSE_CHANNEL_NAME "CMD_RSP" - -/** DLC of Command Response Channel. */ -#define COMMAND_RESPONSE_CHANNEL_DLC (sizeof(CommandResponse)) - -/** Name of Channel to send Motor Speed Setpoints to. */ -#define SPEED_SETPOINT_CHANNEL_NAME "SPEED_SET" - -/** DLC of Speedometer Channel */ -#define SPEED_SETPOINT_CHANNEL_DLC (sizeof(SpeedData)) - -/** Name of Channel to send Current Vehicle Data to. */ -#define CURRENT_VEHICLE_DATA_CHANNEL_NAME "CURR_DATA" - -/** DLC of Current Vehicle Data Channel */ -#define CURRENT_VEHICLE_DATA_CHANNEL_DLC (sizeof(VehicleData)) - -/** Name of Channel to send system status to. */ -#define STATUS_CHANNEL_NAME "STATUS" - -/** DLC of Status Channel */ -#define STATUS_CHANNEL_DLC (sizeof(Status)) - -/** Name of 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 - *****************************************************************************/ - -/** SerialMuxProt Server with fixed template argument. */ -typedef SerialMuxProtServer SMPServer; - -/** Channel payload constants. */ -namespace SMPChannelPayload -{ - /** Remote control commands. */ - typedef enum : uint8_t - { - CMD_ID_IDLE = 0, /**< Nothing to do. */ - CMD_ID_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. */ - CMD_ID_GET_MAX_SPEED, /**< Get maximum speed. */ - CMD_ID_START_DRIVING, /**< Start driving. */ - CMD_ID_SET_INIT_POS /**< Set initial position. */ - - } CmdId; /**< Command ID */ - - /** Remote control command responses. */ - typedef enum : uint8_t - { - RSP_ID_OK = 0, /**< Command successful executed. */ - RSP_ID_PENDING, /**< Command is pending. */ - RSP_ID_ERROR /**< Command failed. */ - - } RspId; /**< Response ID */ - - /** Status flags. */ - typedef enum : uint8_t - { - STATUS_FLAG_OK = 0, /**< Everything is fine. */ - STATUS_FLAG_ERROR /**< Something is wrong. */ - - } Status; /**< Status flag */ - - /** - * Range in which a detected object may be. - * Equivalent to the brightness levels. - * Values estimated from user's guide. - */ - typedef enum : uint8_t - { - RANGE_NO_OBJECT = 0U, /**< No object detected */ - RANGE_25_30, /**< Object detected in range 25 to 30 cm */ - RANGE_20_25, /**< Object detected in range 20 to 25 cm */ - RANGE_15_20, /**< Object detected in range 15 to 20 cm */ - RANGE_10_15, /**< Object detected in range 10 to 15 cm */ - RANGE_5_10, /**< Object detected in range 5 to 10 cm */ - RANGE_0_5 /**< Object detected in range 0 to 5 cm */ - - } Range; /**< Proximity Sensor Ranges */ - -} /* namespace SMPChannelPayload */ - -/** Struct of the "Command" channel payload. */ -typedef struct _Command -{ - SMPChannelPayload::CmdId commandId; /**< Command ID */ - - /** Command payload. */ - union - { - /** Init data command payload. */ - struct - { - int32_t xPos; /**< X position [mm]. */ - int32_t yPos; /**< Y position [mm]. */ - int32_t orientation; /**< Orientation [mrad]. */ - }; - }; - -} __attribute__((packed)) Command; - -/** Struct of the "Command Response" channel payload. */ -typedef struct _CommandResponse -{ - SMPChannelPayload::CmdId commandId; /**< Command ID */ - SMPChannelPayload::RspId responseId; /**< Response to the command */ - - /** Response Payload. */ - union - { - int16_t maxMotorSpeed; /**< Max speed [steps/s]. */ - }; -} __attribute__((packed)) CommandResponse; - -/** Struct of the "Speed" channel payload. */ -typedef struct _SpeedData -{ - int16_t left; /**< Left motor speed [steps/s] */ - int16_t right; /**< Right motor speed [steps/s] */ - int16_t center; /**< Center motor speed [steps/s] */ -} __attribute__((packed)) SpeedData; - -/** Struct of the "Current Vehicle Data" channel payload. */ -typedef struct _VehicleData -{ - int32_t xPos; /**< X position [mm]. */ - int32_t yPos; /**< Y position [mm]. */ - int32_t orientation; /**< Orientation [mrad]. */ - int16_t left; /**< Left motor speed [steps/s]. */ - int16_t right; /**< Right motor speed [steps/s]. */ - int16_t center; /**< Center speed [steps/s]. */ - SMPChannelPayload::Range proximity; /**< Range at which object is found [range]. */ -} __attribute__((packed)) VehicleData; - -/** Struct of the "Status" channel payload. */ -typedef struct _Status -{ - SMPChannelPayload::Status status; /**< Status */ -} __attribute__((packed)) Status; - -/** Struct of the "Line Sensor" channel payload. */ -typedef struct _LineSensorData -{ - uint16_t lineSensorData[5U]; /**< Line sensor data [digits] normalized to max 1000 digits. */ -} __attribute__((packed)) LineSensorData; - -/****************************************************************************** - * Functions - *****************************************************************************/ - +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Channel structure definition for the SerialMuxProt. + * @author Gabryel Reyes + */ + +#ifndef SERIAL_MUX_CHANNELS_H_ +#define SERIAL_MUX_CHANNELS_H_ + +/****************************************************************************** + * Includes + *****************************************************************************/ + +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/** Maximum number of SerialMuxProt Channels. */ +#define MAX_CHANNELS (10U) + +/** Name of Channel to send Commands to. */ +#define COMMAND_CHANNEL_NAME "CMD" + +/** DLC of Command Channel. */ +#define COMMAND_CHANNEL_DLC (sizeof(Command)) + +/** Name of Channel to 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 Channel to send Current Vehicle Data to. */ +#define CURRENT_VEHICLE_DATA_CHANNEL_NAME "CURR_DATA" + +/** DLC of Current Vehicle Data Channel */ +#define CURRENT_VEHICLE_DATA_CHANNEL_DLC (sizeof(VehicleData)) + +/** Name of Channel to send system status to. */ +#define STATUS_CHANNEL_NAME "STATUS" + +/** DLC of Status Channel */ +#define STATUS_CHANNEL_DLC (sizeof(Status)) + +/** Name of 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 + *****************************************************************************/ + +/** SerialMuxProt Server with fixed template argument. */ +typedef SerialMuxProtServer SMPServer; + +/** Channel payload constants. */ +namespace SMPChannelPayload +{ + /** Remote control commands. */ + typedef enum : uint8_t + { + CMD_ID_IDLE = 0, /**< Nothing to do. */ + CMD_ID_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. */ + CMD_ID_GET_MAX_SPEED, /**< Get maximum speed. */ + CMD_ID_START_DRIVING, /**< Start driving. */ + CMD_ID_SET_INIT_POS /**< Set initial position. */ + + } CmdId; /**< Command ID */ + + /** Remote control command responses. */ + typedef enum : uint8_t + { + RSP_ID_OK = 0, /**< Command successful executed. */ + RSP_ID_PENDING, /**< Command is pending. */ + RSP_ID_ERROR /**< Command failed. */ + + } RspId; /**< Response ID */ + + /** Status flags. */ + typedef enum : uint8_t + { + STATUS_FLAG_OK = 0, /**< Everything is fine. */ + STATUS_FLAG_ERROR /**< Something is wrong. */ + + } Status; /**< Status flag */ + + /** + * Range in which a detected object may be. + * Equivalent to the brightness levels. + * Values estimated from user's guide. + */ + typedef enum : uint8_t + { + RANGE_NO_OBJECT = 0U, /**< No object detected */ + RANGE_25_30, /**< Object detected in range 25 to 30 cm */ + RANGE_20_25, /**< Object detected in range 20 to 25 cm */ + RANGE_15_20, /**< Object detected in range 15 to 20 cm */ + RANGE_10_15, /**< Object detected in range 10 to 15 cm */ + RANGE_5_10, /**< Object detected in range 5 to 10 cm */ + RANGE_0_5 /**< Object detected in range 0 to 5 cm */ + + } Range; /**< Proximity Sensor Ranges */ + +} /* namespace SMPChannelPayload */ + +/** Struct of the "Command" channel payload. */ +typedef struct _Command +{ + SMPChannelPayload::CmdId commandId; /**< Command ID */ + + /** Command payload. */ + union + { + /** Init data command payload. */ + struct + { + int32_t xPos; /**< X position [mm]. */ + int32_t yPos; /**< Y position [mm]. */ + int32_t orientation; /**< Orientation [mrad]. */ + }; + }; + +} __attribute__((packed)) Command; + +/** Struct of the "Command Response" channel payload. */ +typedef struct _CommandResponse +{ + SMPChannelPayload::CmdId commandId; /**< Command ID */ + SMPChannelPayload::RspId responseId; /**< Response to the command */ + + /** Response Payload. */ + union + { + int16_t maxMotorSpeed; /**< Max speed [steps/s]. */ + }; +} __attribute__((packed)) CommandResponse; + +/** Struct of the "Speed" channel payload. */ +typedef struct _SpeedData +{ + int16_t left; /**< Left motor speed [steps/s] */ + int16_t right; /**< Right motor speed [steps/s] */ + int16_t center; /**< Center motor speed [steps/s] */ +} __attribute__((packed)) SpeedData; + +/** Struct of the "Current Vehicle Data" channel payload. */ +typedef struct _VehicleData +{ + int32_t xPos; /**< X position [mm]. */ + int32_t yPos; /**< Y position [mm]. */ + int32_t orientation; /**< Orientation [mrad]. */ + int16_t left; /**< Left motor speed [steps/s]. */ + int16_t right; /**< Right motor speed [steps/s]. */ + int16_t center; /**< Center speed [steps/s]. */ + SMPChannelPayload::Range proximity; /**< Range at which object is found [range]. */ +} __attribute__((packed)) VehicleData; + +/** Struct of the "Status" channel payload. */ +typedef struct _Status +{ + SMPChannelPayload::Status status; /**< Status */ +} __attribute__((packed)) Status; + +/** Struct of the "Line Sensor" channel payload. */ +typedef struct _LineSensorData +{ + uint16_t lineSensorData[5U]; /**< Line sensor data [digits] normalized to max 1000 digits. */ +} __attribute__((packed)) LineSensorData; + +/****************************************************************************** + * Functions + *****************************************************************************/ + #endif /* SERIAL_MUX_CHANNELS_H_ */ \ No newline at end of file diff --git a/lib/APPConvoyFollower/StartupState.cpp b/lib/APPConvoyFollower/StartupState.cpp index 66ac447a..b3745f94 100644 --- a/lib/APPConvoyFollower/StartupState.cpp +++ b/lib/APPConvoyFollower/StartupState.cpp @@ -1,124 +1,124 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Startup state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "StartupState.h" -#include "ErrorState.h" -#include "DrivingState.h" -#include -#include -#include - -/****************************************************************************** - * 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("Convoy"); - display.gotoXY(0, 1); - display.print("Follower"); - delay(APP_NAME_DURATION); -} - -void StartupState::process(StateMachine& sm) -{ - IBoard& board = Board::getInstance(); - ISettings& settings = board.getSettings(); - int16_t maxSpeed = settings.getMaxSpeed(); - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - - /* Load the max. motor speed always from the settings, because - * it may happen that there was no calibration before. - */ - diffDrive.setMaxMotorSpeed(maxSpeed); - diffDrive.enable(); - - if (0 == maxSpeed) - { - /* Missing calibration data. Run AppCalib first. */ - ErrorState::getInstance().setErrorMsg("MCAL=0"); - sm.setState(&ErrorState::getInstance()); - } - else if (true == m_initialDataSet) - { - sm.setState(&DrivingState::getInstance()); - } -} - -void StartupState::exit() -{ - /* Nothing to do */ -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Startup state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "StartupState.h" +#include "ErrorState.h" +#include "DrivingState.h" +#include +#include +#include + +/****************************************************************************** + * 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("Convoy"); + display.gotoXY(0, 1); + display.print("Follower"); + delay(APP_NAME_DURATION); +} + +void StartupState::process(StateMachine& sm) +{ + IBoard& board = Board::getInstance(); + ISettings& settings = board.getSettings(); + int16_t maxSpeed = settings.getMaxSpeed(); + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + + /* Load the max. motor speed always from the settings, because + * it may happen that there was no calibration before. + */ + diffDrive.setMaxMotorSpeed(maxSpeed); + diffDrive.enable(); + + if (0 == maxSpeed) + { + /* Missing calibration data. Run AppCalib first. */ + ErrorState::getInstance().setErrorMsg("MCAL=0"); + sm.setState(&ErrorState::getInstance()); + } + else if (true == m_initialDataSet) + { + sm.setState(&DrivingState::getInstance()); + } +} + +void StartupState::exit() +{ + /* Nothing to do */ +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPConvoyFollower/StartupState.h b/lib/APPConvoyFollower/StartupState.h index 9fe4970f..b8d40db3 100644 --- a/lib/APPConvoyFollower/StartupState.h +++ b/lib/APPConvoyFollower/StartupState.h @@ -1,136 +1,136 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Startup state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef STARTUP_STATE_H -#define STARTUP_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include - -/****************************************************************************** - * 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; - - /** - * Notify the state, that the initial data set was received. - */ - void notifyInitialDataIsSet() - { - m_initialDataSet = true; - } - -protected: -private: - /** - * Duration in ms how long the application name shall be shown at startup. - */ - static const uint32_t APP_NAME_DURATION = 2000U; - - /** - * Flag to indicate, that the initial data was set. - */ - bool m_initialDataSet; - - /** - * Default constructor. - */ - StartupState() : m_initialDataSet(false) - { - } - - /** - * 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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Startup state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef STARTUP_STATE_H +#define STARTUP_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include + +/****************************************************************************** + * 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; + + /** + * Notify the state, that the initial data set was received. + */ + void notifyInitialDataIsSet() + { + m_initialDataSet = true; + } + +protected: +private: + /** + * Duration in ms how long the application name shall be shown at startup. + */ + static const uint32_t APP_NAME_DURATION = 2000U; + + /** + * Flag to indicate, that the initial data was set. + */ + bool m_initialDataSet; + + /** + * Default constructor. + */ + StartupState() : m_initialDataSet(false) + { + } + + /** + * 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/APPConvoyFollower/library.json b/lib/APPConvoyFollower/library.json index 921c6ab5..b68194dc 100644 --- a/lib/APPConvoyFollower/library.json +++ b/lib/APPConvoyFollower/library.json @@ -1,29 +1,29 @@ -{ - "name": "APPConvoyFollower", - "version": "0.1.0", - "description": "Convoy follower application", - "authors": [ - { - "name": "Gabryel Reyes", - "email": "gabryelrdiaz@gmail.com", - "url": "https://github.com/gabryelreyes", - "maintainer": true - } - ], - "license": "MIT", - "dependencies": [ - { - "name": "Service" - }, - { - "name": "HALInterfaces" - }, - { - "owner": "gabryelreyes", - "name": "SerialMuxProt", - "version": "^2.0.0" - } - ], - "frameworks": "*", - "platforms": "*" -} +{ + "name": "APPConvoyFollower", + "version": "0.1.0", + "description": "Convoy follower application", + "authors": [ + { + "name": "Gabryel Reyes", + "email": "gabryelrdiaz@gmail.com", + "url": "https://github.com/gabryelreyes", + "maintainer": true + } + ], + "license": "MIT", + "dependencies": [ + { + "name": "Service" + }, + { + "name": "HALInterfaces" + }, + { + "owner": "gabryelreyes", + "name": "SerialMuxProt", + "version": "^2.0.0" + } + ], + "frameworks": "*", + "platforms": "*" +} diff --git a/lib/APPConvoyLeader/DrivingState.cpp b/lib/APPConvoyLeader/DrivingState.cpp index 94857361..6425e643 100644 --- a/lib/APPConvoyLeader/DrivingState.cpp +++ b/lib/APPConvoyLeader/DrivingState.cpp @@ -1,377 +1,377 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Driving state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "DrivingState.h" -#include -#include -#include -#include -#include -#include "ErrorState.h" -#include "ParameterSets.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void DrivingState::entry() -{ - IDisplay& display = Board::getInstance().getDisplay(); - const ParameterSets::ParameterSet& parSet = ParameterSets::getInstance().getParameterSet(); - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - const int16_t maxSpeed = diffDrive.getMaxMotorSpeed(); /* [steps/s] */ - - diffDrive.enable(); - - 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(); - - /* Top speed is 0, and can only be set externally by DCS. */ - m_topSpeed = 0; - - /* Configure PID controller with selected parameter set. */ - 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); - - display.clear(); - display.print("DRIVE"); - display.gotoXY(0, 1); - display.print("GO"); -} - -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: - m_trackStatus = TRACK_STATUS_ON_TRACK; - break; - - default: - /* Fatal error */ - Sound::playAlarm(); - ErrorState::getInstance().setErrorMsg("DRV"); - sm.setState(&ErrorState::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; - } - 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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Driving state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "DrivingState.h" +#include +#include +#include +#include +#include +#include "ErrorState.h" +#include "ParameterSets.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void DrivingState::entry() +{ + IDisplay& display = Board::getInstance().getDisplay(); + const ParameterSets::ParameterSet& parSet = ParameterSets::getInstance().getParameterSet(); + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + const int16_t maxSpeed = diffDrive.getMaxMotorSpeed(); /* [steps/s] */ + + diffDrive.enable(); + + 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(); + + /* Top speed is 0, and can only be set externally by DCS. */ + m_topSpeed = 0; + + /* Configure PID controller with selected parameter set. */ + 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); + + display.clear(); + display.print("DRIVE"); + display.gotoXY(0, 1); + display.print("GO"); +} + +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: + m_trackStatus = TRACK_STATUS_ON_TRACK; + break; + + default: + /* Fatal error */ + Sound::playAlarm(); + ErrorState::getInstance().setErrorMsg("DRV"); + sm.setState(&ErrorState::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; + } + 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 0f8dd968..3cdcde83 100644 --- a/lib/APPConvoyLeader/DrivingState.h +++ b/lib/APPConvoyLeader/DrivingState.h @@ -1,224 +1,224 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Driving state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef DRIVING_STATE_H -#define DRIVING_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The system driving state. */ -class DrivingState : public IState -{ -public: - /** - * Get state instance. - * - * @return State instance. - */ - static DrivingState& getInstance() - { - static DrivingState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - - /** - * Set top speed. - * - * @param[in] topSpeed Top speed in [steps/s] - */ - void setTopSpeed(int16_t topSpeed) - { - m_topSpeed = topSpeed; - } - -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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Driving state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef DRIVING_STATE_H +#define DRIVING_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The system driving state. */ +class DrivingState : public IState +{ +public: + /** + * Get state instance. + * + * @return State instance. + */ + static DrivingState& getInstance() + { + static DrivingState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + + /** + * Set top speed. + * + * @param[in] topSpeed Top speed in [steps/s] + */ + void setTopSpeed(int16_t topSpeed) + { + m_topSpeed = topSpeed; + } + +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 ef09645b..4ac54876 100644 --- a/lib/APPConvoyLeader/ErrorState.cpp +++ b/lib/APPConvoyLeader/ErrorState.cpp @@ -1,120 +1,120 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Error state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ErrorState.h" -#include -#include -#include "StartupState.h" -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void ErrorState::entry() -{ - IDisplay& display = Board::getInstance().getDisplay(); - DifferentialDrive::getInstance().disable(); - - display.clear(); - display.print("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(&StartupState::getInstance()); - } -} - -void ErrorState::exit() -{ - /* Nothing to do. */ -} - -void ErrorState::setErrorMsg(const char* msg) -{ - if (nullptr == msg) - { - m_errorMsg[0] = '\0'; - } - else - { - strncpy(m_errorMsg, msg, ERROR_MSG_SIZE - 1); - m_errorMsg[ERROR_MSG_SIZE - 1] = '\0'; - } -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Error state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ErrorState.h" +#include +#include +#include "StartupState.h" +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void ErrorState::entry() +{ + IDisplay& display = Board::getInstance().getDisplay(); + DifferentialDrive::getInstance().disable(); + + display.clear(); + display.print("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(&StartupState::getInstance()); + } +} + +void ErrorState::exit() +{ + /* Nothing to do. */ +} + +void ErrorState::setErrorMsg(const char* msg) +{ + if (nullptr == msg) + { + m_errorMsg[0] = '\0'; + } + else + { + strncpy(m_errorMsg, msg, ERROR_MSG_SIZE - 1); + m_errorMsg[ERROR_MSG_SIZE - 1] = '\0'; + } +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPConvoyLeader/ErrorState.h b/lib/APPConvoyLeader/ErrorState.h index 74095203..0679e9a3 100644 --- a/lib/APPConvoyLeader/ErrorState.h +++ b/lib/APPConvoyLeader/ErrorState.h @@ -1,134 +1,134 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Error state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef ERROR_STATE_H -#define ERROR_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The system error state. */ -class ErrorState : public IState -{ -public: - /** - * Get state instance. - * - * @return State instance. - */ - static ErrorState& getInstance() - { - static ErrorState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - - /** - * Set error message, which to show on the display. - * - * @param[in] msg Error message - */ - void setErrorMsg(const char* msg); - -protected: -private: - /** - * The error message string size in bytes, which - * includes the terminating character. - */ - static const size_t ERROR_MSG_SIZE = 20; - - char m_errorMsg[ERROR_MSG_SIZE]; /**< Error message, which to show. */ - - /** - * Default constructor. - */ - ErrorState() : m_errorMsg() - { - m_errorMsg[0] = '\0'; - } - - /** - * Default destructor. - */ - ~ErrorState() - { - } - - /* Not allowed. */ - ErrorState(const ErrorState& state); /**< Copy construction of an instance. */ - ErrorState& operator=(const ErrorState& state); /**< Assignment of an instance. */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* ERROR_STATE_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Error state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef ERROR_STATE_H +#define ERROR_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The system error state. */ +class ErrorState : public IState +{ +public: + /** + * Get state instance. + * + * @return State instance. + */ + static ErrorState& getInstance() + { + static ErrorState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + + /** + * Set error message, which to show on the display. + * + * @param[in] msg Error message + */ + void setErrorMsg(const char* msg); + +protected: +private: + /** + * The error message string size in bytes, which + * includes the terminating character. + */ + static const size_t ERROR_MSG_SIZE = 20; + + char m_errorMsg[ERROR_MSG_SIZE]; /**< Error message, which to show. */ + + /** + * Default constructor. + */ + ErrorState() : m_errorMsg() + { + m_errorMsg[0] = '\0'; + } + + /** + * Default destructor. + */ + ~ErrorState() + { + } + + /* Not allowed. */ + ErrorState(const ErrorState& state); /**< Copy construction of an instance. */ + ErrorState& operator=(const ErrorState& state); /**< Assignment of an instance. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* ERROR_STATE_H */ +/** @} */ diff --git a/lib/APPConvoyLeader/LineSensorsCalibrationState.cpp b/lib/APPConvoyLeader/LineSensorsCalibrationState.cpp index f8aa38dd..108f8985 100644 --- a/lib/APPConvoyLeader/LineSensorsCalibrationState.cpp +++ b/lib/APPConvoyLeader/LineSensorsCalibrationState.cpp @@ -1,213 +1,213 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Line sensors calibration state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "LineSensorsCalibrationState.h" -#include -#include -#include -#include -#include -#include "DrivingState.h" -#include "ErrorState.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void LineSensorsCalibrationState::entry() -{ - IDisplay& display = Board::getInstance().getDisplay(); - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - Odometry& odometry = Odometry::getInstance(); - ILineSensors& lineSensors = Board::getInstance().getLineSensors(); - - display.clear(); - display.print("Calib"); - display.gotoXY(0, 1); - display.print("Lines"); - - /* Prepare calibration drive. */ - m_calibrationSpeed = diffDrive.getMaxMotorSpeed() / 3; - m_orientation = odometry.getOrientation(); - - /* Mandatory for each new calibration. */ - lineSensors.resetCalibration(); - - /* Wait some time, before starting the calibration drive. */ - m_phase = PHASE_1_WAIT; - m_timer.start(WAIT_TIME); -} - -void LineSensorsCalibrationState::process(StateMachine& sm) -{ - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - - switch (m_phase) - { - case PHASE_1_WAIT: - if (true == m_timer.isTimeout()) - { - m_phase = PHASE_2_TURN_LEFT; - diffDrive.setLinearSpeed(-m_calibrationSpeed, m_calibrationSpeed); - } - break; - - case PHASE_2_TURN_LEFT: - if (true == turnAndCalibrate(CALIB_ANGLE, true)) - { - m_phase = PHASE_3_TURN_RIGHT; - diffDrive.setLinearSpeed(m_calibrationSpeed, -m_calibrationSpeed); - } - break; - - case PHASE_3_TURN_RIGHT: - if (true == turnAndCalibrate(-CALIB_ANGLE, false)) - { - m_phase = PHASE_4_TURN_ORIG; - diffDrive.setLinearSpeed(-m_calibrationSpeed, m_calibrationSpeed); - } - break; - - case PHASE_4_TURN_ORIG: - if (true == turnAndCalibrate(0, true)) - { - m_phase = PHASE_5_FINISHED; - diffDrive.setLinearSpeed(0, 0); - finishCalibration(sm); - } - break; - - case PHASE_5_FINISHED: - /* fallthrough */ - default: - break; - } -} - -void LineSensorsCalibrationState::exit() -{ - m_timer.stop(); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -bool LineSensorsCalibrationState::turnAndCalibrate(int32_t calibAlpha, bool isGreaterEqual) -{ - ILineSensors& lineSensors = Board::getInstance().getLineSensors(); - Odometry& odometry = Odometry::getInstance(); - int32_t alpha = odometry.getOrientation() - m_orientation; /* [mrad] */ - bool isSuccesful = false; - - /* Continously calibrate the line sensors. */ - lineSensors.calibrate(); - - /* Is the goal that the current angle shall be lower or equal than the destination calibration angle? */ - if (false == isGreaterEqual) - { - /* Is alpha lower or equal than the destination calibration angle? */ - if (calibAlpha >= alpha) - { - isSuccesful = true; - } - } - else - { - /* Is alpha greater or equal than the destination calibration angle? */ - if (calibAlpha <= alpha) - { - isSuccesful = true; - } - } - - return isSuccesful; -} - -void LineSensorsCalibrationState::finishCalibration(StateMachine& sm) -{ - ILineSensors& lineSensors = Board::getInstance().getLineSensors(); - - if (false == lineSensors.isCalibrationSuccessful()) - { - char str[10]; - char valueStr[10]; - - Util::uintToStr(valueStr, sizeof(valueStr), lineSensors.getCalibErrorInfo()); - - strncpy(str, "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(&DrivingState::getInstance()); - } -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Line sensors calibration state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "LineSensorsCalibrationState.h" +#include +#include +#include +#include +#include +#include "DrivingState.h" +#include "ErrorState.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void LineSensorsCalibrationState::entry() +{ + IDisplay& display = Board::getInstance().getDisplay(); + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + Odometry& odometry = Odometry::getInstance(); + ILineSensors& lineSensors = Board::getInstance().getLineSensors(); + + display.clear(); + display.print("Calib"); + display.gotoXY(0, 1); + display.print("Lines"); + + /* Prepare calibration drive. */ + m_calibrationSpeed = diffDrive.getMaxMotorSpeed() / 3; + m_orientation = odometry.getOrientation(); + + /* Mandatory for each new calibration. */ + lineSensors.resetCalibration(); + + /* Wait some time, before starting the calibration drive. */ + m_phase = PHASE_1_WAIT; + m_timer.start(WAIT_TIME); +} + +void LineSensorsCalibrationState::process(StateMachine& sm) +{ + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + + switch (m_phase) + { + case PHASE_1_WAIT: + if (true == m_timer.isTimeout()) + { + m_phase = PHASE_2_TURN_LEFT; + diffDrive.setLinearSpeed(-m_calibrationSpeed, m_calibrationSpeed); + } + break; + + case PHASE_2_TURN_LEFT: + if (true == turnAndCalibrate(CALIB_ANGLE, true)) + { + m_phase = PHASE_3_TURN_RIGHT; + diffDrive.setLinearSpeed(m_calibrationSpeed, -m_calibrationSpeed); + } + break; + + case PHASE_3_TURN_RIGHT: + if (true == turnAndCalibrate(-CALIB_ANGLE, false)) + { + m_phase = PHASE_4_TURN_ORIG; + diffDrive.setLinearSpeed(-m_calibrationSpeed, m_calibrationSpeed); + } + break; + + case PHASE_4_TURN_ORIG: + if (true == turnAndCalibrate(0, true)) + { + m_phase = PHASE_5_FINISHED; + diffDrive.setLinearSpeed(0, 0); + finishCalibration(sm); + } + break; + + case PHASE_5_FINISHED: + /* fallthrough */ + default: + break; + } +} + +void LineSensorsCalibrationState::exit() +{ + m_timer.stop(); +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +bool LineSensorsCalibrationState::turnAndCalibrate(int32_t calibAlpha, bool isGreaterEqual) +{ + ILineSensors& lineSensors = Board::getInstance().getLineSensors(); + Odometry& odometry = Odometry::getInstance(); + int32_t alpha = odometry.getOrientation() - m_orientation; /* [mrad] */ + bool isSuccesful = false; + + /* Continously calibrate the line sensors. */ + lineSensors.calibrate(); + + /* Is the goal that the current angle shall be lower or equal than the destination calibration angle? */ + if (false == isGreaterEqual) + { + /* Is alpha lower or equal than the destination calibration angle? */ + if (calibAlpha >= alpha) + { + isSuccesful = true; + } + } + else + { + /* Is alpha greater or equal than the destination calibration angle? */ + if (calibAlpha <= alpha) + { + isSuccesful = true; + } + } + + return isSuccesful; +} + +void LineSensorsCalibrationState::finishCalibration(StateMachine& sm) +{ + ILineSensors& lineSensors = Board::getInstance().getLineSensors(); + + if (false == lineSensors.isCalibrationSuccessful()) + { + char str[10]; + char valueStr[10]; + + Util::uintToStr(valueStr, sizeof(valueStr), lineSensors.getCalibErrorInfo()); + + strncpy(str, "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(&DrivingState::getInstance()); + } +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPConvoyLeader/LineSensorsCalibrationState.h b/lib/APPConvoyLeader/LineSensorsCalibrationState.h index f59dd3c4..1ee29963 100644 --- a/lib/APPConvoyLeader/LineSensorsCalibrationState.h +++ b/lib/APPConvoyLeader/LineSensorsCalibrationState.h @@ -1,161 +1,161 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Line sensors calibration state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef LINE_SENSORS_CALIBRATION_STATE_H -#define LINE_SENSORS_CALIBRATION_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The line sensors calibration state. */ -class LineSensorsCalibrationState : public IState -{ -public: - /** - * Get state instance. - * - * @return State instance. - */ - static LineSensorsCalibrationState& getInstance() - { - static LineSensorsCalibrationState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - -protected: -private: - /** Calibration phases */ - enum Phase - { - PHASE_1_WAIT = 0, /**< Wait a short time before starting the calibration. */ - PHASE_2_TURN_LEFT, /**< Turn line sensors left over the line. */ - PHASE_3_TURN_RIGHT, /**< Turn line sensor right over the line. */ - PHASE_4_TURN_ORIG, /**< Turn back to origin. */ - PHASE_5_FINISHED /**< Calibration is finished. */ - }; - - /** - * Duration in ms about to wait, until the calibration drive starts. - */ - static const uint32_t WAIT_TIME = 1000; - - /** - * Calibration turn angle in mrad (corresponds to 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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Line sensors calibration state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef LINE_SENSORS_CALIBRATION_STATE_H +#define LINE_SENSORS_CALIBRATION_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The line sensors calibration state. */ +class LineSensorsCalibrationState : public IState +{ +public: + /** + * Get state instance. + * + * @return State instance. + */ + static LineSensorsCalibrationState& getInstance() + { + static LineSensorsCalibrationState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + +protected: +private: + /** Calibration phases */ + enum Phase + { + PHASE_1_WAIT = 0, /**< Wait a short time before starting the calibration. */ + PHASE_2_TURN_LEFT, /**< Turn line sensors left over the line. */ + PHASE_3_TURN_RIGHT, /**< Turn line sensor right over the line. */ + PHASE_4_TURN_ORIG, /**< Turn back to origin. */ + PHASE_5_FINISHED /**< Calibration is finished. */ + }; + + /** + * Duration in ms about to wait, until the calibration drive starts. + */ + static const uint32_t WAIT_TIME = 1000; + + /** + * Calibration turn angle in mrad (corresponds to 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/ParameterSets.cpp b/lib/APPConvoyLeader/ParameterSets.cpp index ee0d3282..75f971b5 100644 --- a/lib/APPConvoyLeader/ParameterSets.cpp +++ b/lib/APPConvoyLeader/ParameterSets.cpp @@ -1,150 +1,150 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Calibration state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ParameterSets.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void ParameterSets::choose(uint8_t setId) -{ - if (MAX_SETS > setId) - { - m_currentSetId = setId; - } -} - -void ParameterSets::next() -{ - ++m_currentSetId; - m_currentSetId %= MAX_SETS; -} - -uint8_t ParameterSets::getCurrentSetId() const -{ - return m_currentSetId; -} - -const ParameterSets::ParameterSet& ParameterSets::getParameterSet() const -{ - return m_parSets[m_currentSetId]; -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -ParameterSets::ParameterSets() : m_currentSetId(0), m_parSets() -{ - m_parSets[0] = { - "PD VF", /* Name - VF: very fast */ - 4200, /* Top speed in steps/s */ - 4, /* Kp Numerator */ - 1, /* Kp Denominator */ - 0, /* Ki Numerator */ - 1, /* Ki Denominator */ - 60, /* Kd Numerator */ - 1 /* Kd Denominator */ - }; - - m_parSets[1] = { - "PD F", /* Name - F: fast */ - 3000, /* Top speed in steps/s */ - 2, /* Kp Numerator */ - 1, /* Kp Denominator */ - 0, /* Ki Numerator */ - 1, /* Ki Denominator */ - 30, /* Kd Numerator */ - 1 /* Kd Denominator */ - }; - - m_parSets[2] = { - "PD S", /* Name - S: slow */ - 2000, /* Top speed in steps/s */ - 2, /* Kp Numerator */ - 1, /* Kp Denominator */ - 0, /* Ki Numerator */ - 1, /* Ki Denominator */ - 30, /* Kd Numerator */ - 1 /* Kd Denominator */ - }; - - m_parSets[3] = { - "PD VS", /* Name - VS: very slow */ - 1000, /* Top speed in steps/s */ - 3, /* Kp Numerator */ - 2, /* Kp Denominator */ - 0, /* Ki Numerator */ - 1, /* Ki Denominator */ - 10, /* Kd Numerator */ - 1 /* Kd Denominator */ - }; -} - -ParameterSets::~ParameterSets() -{ -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Calibration state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ParameterSets.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void ParameterSets::choose(uint8_t setId) +{ + if (MAX_SETS > setId) + { + m_currentSetId = setId; + } +} + +void ParameterSets::next() +{ + ++m_currentSetId; + m_currentSetId %= MAX_SETS; +} + +uint8_t ParameterSets::getCurrentSetId() const +{ + return m_currentSetId; +} + +const ParameterSets::ParameterSet& ParameterSets::getParameterSet() const +{ + return m_parSets[m_currentSetId]; +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +ParameterSets::ParameterSets() : m_currentSetId(0), m_parSets() +{ + m_parSets[0] = { + "PD VF", /* Name - VF: very fast */ + 4200, /* Top speed in steps/s */ + 4, /* Kp Numerator */ + 1, /* Kp Denominator */ + 0, /* Ki Numerator */ + 1, /* Ki Denominator */ + 60, /* Kd Numerator */ + 1 /* Kd Denominator */ + }; + + m_parSets[1] = { + "PD F", /* Name - F: fast */ + 3000, /* Top speed in steps/s */ + 2, /* Kp Numerator */ + 1, /* Kp Denominator */ + 0, /* Ki Numerator */ + 1, /* Ki Denominator */ + 30, /* Kd Numerator */ + 1 /* Kd Denominator */ + }; + + m_parSets[2] = { + "PD S", /* Name - S: slow */ + 2000, /* Top speed in steps/s */ + 2, /* Kp Numerator */ + 1, /* Kp Denominator */ + 0, /* Ki Numerator */ + 1, /* Ki Denominator */ + 30, /* Kd Numerator */ + 1 /* Kd Denominator */ + }; + + m_parSets[3] = { + "PD VS", /* Name - VS: very slow */ + 1000, /* Top speed in steps/s */ + 3, /* Kp Numerator */ + 2, /* Kp Denominator */ + 0, /* Ki Numerator */ + 1, /* Ki Denominator */ + 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 51696265..a046236a 100644 --- a/lib/APPConvoyLeader/ParameterSets.h +++ b/lib/APPConvoyLeader/ParameterSets.h @@ -1,145 +1,145 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Parameter state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef PARAMETER_SETS_H -#define PARAMETER_SETS_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** Parameter set with different driving configurations. */ -class ParameterSets -{ -public: - /** - * A single parameter set. - */ - struct ParameterSet - { - const char* name; /**< Name of the parameter set */ - int16_t topSpeed; /**< Top speed in steps/s */ - int16_t kPNumerator; /**< Kp numerator value */ - int16_t kPDenominator; /**< Kp denominator value */ - int16_t kINumerator; /**< Ki numerator value */ - int16_t kIDenominator; /**< Ki denominator value */ - int16_t kDNumerator; /**< Kd numerator value */ - int16_t kDDenominator; /**< Kd denominator value */ - }; - - /** - * Get parameter set instance. - * - * @return Parameter set instance. - */ - static ParameterSets& getInstance() - { - static ParameterSets instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * Choose a specific parameter set. - * If a invalid set id is given, nothing changes. - * - * @param[in] setId Parameter set id - */ - void choose(uint8_t setId); - - /** - * Change to next parameter set. - * After the last set, the first will be choosen. - */ - void next(); - - /** - * Get current set id. - * - * @return Parameter set id. - */ - uint8_t getCurrentSetId() const; - - /** - * Get selected parameter set. - * - * @return Parameter set - */ - const ParameterSet& getParameterSet() const; - - /** Max. number of parameter sets. */ - static const uint8_t MAX_SETS = 4U; - -protected: -private: - uint8_t m_currentSetId; /**< Set id of current selected set. */ - ParameterSet m_parSets[MAX_SETS]; /**< All parameter sets */ - - /** - * Default constructor. - */ - ParameterSets(); - - /** - * Default destructor. - */ - ~ParameterSets(); - - /* 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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Parameter state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef PARAMETER_SETS_H +#define PARAMETER_SETS_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** Parameter set with different driving configurations. */ +class ParameterSets +{ +public: + /** + * A single parameter set. + */ + struct ParameterSet + { + const char* name; /**< Name of the parameter set */ + int16_t topSpeed; /**< Top speed in steps/s */ + int16_t kPNumerator; /**< Kp numerator value */ + int16_t kPDenominator; /**< Kp denominator value */ + int16_t kINumerator; /**< Ki numerator value */ + int16_t kIDenominator; /**< Ki denominator value */ + int16_t kDNumerator; /**< Kd numerator value */ + int16_t kDDenominator; /**< Kd denominator value */ + }; + + /** + * Get parameter set instance. + * + * @return Parameter set instance. + */ + static ParameterSets& getInstance() + { + static ParameterSets instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * Choose a specific parameter set. + * If a invalid set id is given, nothing changes. + * + * @param[in] setId Parameter set id + */ + void choose(uint8_t setId); + + /** + * Change to next parameter set. + * After the last set, the first will be choosen. + */ + void next(); + + /** + * Get current set id. + * + * @return Parameter set id. + */ + uint8_t getCurrentSetId() const; + + /** + * Get selected parameter set. + * + * @return Parameter set + */ + const ParameterSet& getParameterSet() const; + + /** Max. number of parameter sets. */ + static const uint8_t MAX_SETS = 4U; + +protected: +private: + uint8_t m_currentSetId; /**< Set id of current selected set. */ + ParameterSet m_parSets[MAX_SETS]; /**< All parameter sets */ + + /** + * Default constructor. + */ + ParameterSets(); + + /** + * Default destructor. + */ + ~ParameterSets(); + + /* 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/StartupState.h b/lib/APPConvoyLeader/StartupState.h index d70b3752..99f8a1a5 100644 --- a/lib/APPConvoyLeader/StartupState.h +++ b/lib/APPConvoyLeader/StartupState.h @@ -1,131 +1,131 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief 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; - - /** - * Notify the state, that the initial data set was received. - */ - void notifyInitialDataIsSet() - { - m_initialDataSet = true; - } - -protected: -private: - /** - * Flag to indicate, that the initial data was set. - */ - bool m_initialDataSet; - - /** - * Default constructor. - */ - StartupState() : m_initialDataSet(false) - { - } - - /** - * 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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Startup state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef STARTUP_STATE_H +#define STARTUP_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include + +/****************************************************************************** + * 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; + + /** + * Notify the state, that the initial data set was received. + */ + void notifyInitialDataIsSet() + { + m_initialDataSet = true; + } + +protected: +private: + /** + * Flag to indicate, that the initial data was set. + */ + bool m_initialDataSet; + + /** + * Default constructor. + */ + StartupState() : m_initialDataSet(false) + { + } + + /** + * 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 f9e59335..7cade5d2 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.h b/lib/APPLineFollower/App.h index c70a8eca..dc5e939e 100644 --- a/lib/APPLineFollower/App.h +++ b/lib/APPLineFollower/App.h @@ -1,109 +1,109 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief LineFollower application - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef APP_H -#define APP_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The line follower application. */ -class App -{ -public: - /** - * Construct the line follower application. - */ - App() : m_systemStateMachine(), m_controlInterval() - { - } - - /** - * Destroy the line follower application. - */ - ~App() - { - } - - /** - * Setup the application. - */ - void setup(); - - /** - * Process the application periodically. - */ - void loop(); - -private: - /** Differential drive control period in ms. */ - static const uint32_t DIFFERENTIAL_DRIVE_CONTROL_PERIOD = 5U; - - /** Baudrate for Serial Communication */ - static const uint32_t SERIAL_BAUDRATE = 115200U; - - /** The system state machine. */ - StateMachine m_systemStateMachine; - - /** Timer used for differential drive control processing. */ - SimpleTimer m_controlInterval; - - /* 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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief LineFollower application + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef APP_H +#define APP_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The line follower application. */ +class App +{ +public: + /** + * Construct the line follower application. + */ + App() : m_systemStateMachine(), m_controlInterval() + { + } + + /** + * Destroy the line follower application. + */ + ~App() + { + } + + /** + * Setup the application. + */ + void setup(); + + /** + * Process the application periodically. + */ + void loop(); + +private: + /** Differential drive control period in ms. */ + static const uint32_t DIFFERENTIAL_DRIVE_CONTROL_PERIOD = 5U; + + /** Baudrate for Serial Communication */ + static const uint32_t SERIAL_BAUDRATE = 115200U; + + /** The system state machine. */ + StateMachine m_systemStateMachine; + + /** Timer used for differential drive control processing. */ + SimpleTimer m_controlInterval; + + /* 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/DrivingState.cpp b/lib/APPLineFollower/DrivingState.cpp index fb304a6d..281c8875 100644 --- a/lib/APPLineFollower/DrivingState.cpp +++ b/lib/APPLineFollower/DrivingState.cpp @@ -1,899 +1,899 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Driving state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "DrivingState.h" -#include -#include -#include -#include -#include -#include "ReadyState.h" -#include "ParameterSets.h" -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -#ifdef DEBUG_ALGORITHM -static void logCsvDataTitles(uint8_t length); -static void logCsvDataTimestamp(); -static void logCsvData(const uint16_t* lineSensorValues, uint8_t length, int16_t pos, int16_t pos3, bool isPos3Valid); -void logCsvDataTrackStatus(DrivingState::TrackStatus trackStatus); -static void logCsvDataSpeed(int16_t leftSpeed, int16_t rightSpeed); -#endif /* DEBUG_ALGORITHM */ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -#ifdef DEBUG_ALGORITHM -static int16_t gSpeedLeft = 0; -static int16_t gSpeedRight = 0; -#endif /* DEBUG_ALGORITHM */ - -const uint16_t DrivingState::SENSOR_VALUE_MAX = Board::getInstance().getLineSensors().getSensorValueMax(); - -/* Calculate the position set point to be generic. */ -const int16_t DrivingState::POSITION_SET_POINT = - (SENSOR_VALUE_MAX * (Board::getInstance().getLineSensors().getNumLineSensors() - 1)) / 2; - -/* Initialize the required sensor IDs to be generic. */ -const uint8_t DrivingState::SENSOR_ID_MOST_LEFT = 0U; -const uint8_t DrivingState::SENSOR_ID_MIDDLE = Board::getInstance().getLineSensors().getNumLineSensors() / 2U; -const uint8_t DrivingState::SENSOR_ID_MOST_RIGHT = Board::getInstance().getLineSensors().getNumLineSensors() - 1U; - -/* Initialize the position values used by the algorithmic. */ -const int16_t DrivingState::POSITION_MIN = 0; -const int16_t DrivingState::POSITION_MAX = - static_cast(Board::getInstance().getLineSensors().getNumLineSensors() - 1U) * 1000; -const int16_t DrivingState::POSITION_MIDDLE_MIN = POSITION_SET_POINT - (SENSOR_VALUE_MAX / 2); -const int16_t DrivingState::POSITION_MIDDLE_MAX = POSITION_SET_POINT + (SENSOR_VALUE_MAX / 2); - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void DrivingState::entry() -{ - IDisplay& display = Board::getInstance().getDisplay(); - const ParameterSets::ParameterSet& parSet = ParameterSets::getInstance().getParameterSet(); - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - const int16_t maxSpeed = diffDrive.getMaxMotorSpeed(); /* [steps/s] */ - - m_observationTimer.start(OBSERVATION_DURATION); - m_pidProcessTime.start(0); /* Immediate */ - m_lineStatus = LINE_STATUS_NO_START_LINE_DETECTED; - m_trackStatus = TRACK_STATUS_NORMAL; /* Assume that the robot is placed on track. */ - m_isTrackLost = false; /* Assume that the robot is placed on track. */ - m_isStartStopLineDetected = false; - - /* Configure PID controller with selected parameter set. */ - m_topSpeed = parSet.topSpeed; - m_pidCtrl.clear(); - m_pidCtrl.setPFactor(parSet.kPNumerator, parSet.kPDenominator); - m_pidCtrl.setIFactor(parSet.kINumerator, parSet.kIDenominator); - m_pidCtrl.setDFactor(parSet.kDNumerator, parSet.kDDenominator); - m_pidCtrl.setSampleTime(PID_PROCESS_PERIOD); - m_pidCtrl.setLimits(-maxSpeed, maxSpeed); - m_pidCtrl.setDerivativeOnMeasurement(false); - - display.clear(); - display.print("DRV"); - -#ifdef DEBUG_ALGORITHM - logCsvDataTitles(Board::getInstance().getLineSensors().getNumLineSensors()); -#endif /* DEBUG_ALGORITHM */ -} - -void DrivingState::process(StateMachine& sm) -{ - ILineSensors& lineSensors = Board::getInstance().getLineSensors(); - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - TrackStatus nextTrackStatus = m_trackStatus; - bool allowNegativeMotorSpeed = true; - - /* Get the position of the line and each sensor value. */ - int16_t position = lineSensors.readLine(); - const uint16_t* lineSensorValues = lineSensors.getSensorValues(); - uint8_t numLineSensors = lineSensors.getNumLineSensors(); - int16_t position3 = 0; - bool isPosition3Valid = calcPosition3(position3, lineSensorValues, numLineSensors); - bool isTrackLost = isNoLineDetected(lineSensorValues, numLineSensors); - -#ifdef DEBUG_ALGORITHM - logCsvDataTimestamp(); - logCsvData(lineSensorValues, numLineSensors, position, position3, isPosition3Valid); -#endif /* DEBUG_ALGORITHM */ - - /* If the position calculated with the inner sensors is not valid, the - * position will be taken. - */ - if (true == isPosition3Valid) - { - position3 = position; - } - - /* ======================================================================== - * Evaluate the situation based on the sensor values. - * ======================================================================== - */ - nextTrackStatus = evaluateSituation(lineSensorValues, numLineSensors, position, position3, isTrackLost); - - /* ======================================================================== - * Initiate measures depended on the situation. - * ======================================================================== - */ - processSituation(position, allowNegativeMotorSpeed, nextTrackStatus, position3); - - /* If the tracks status changes, the PID integral and derivative part - * will be reset to provide a smooth reaction. - */ - if (m_trackStatus != nextTrackStatus) - { - m_pidCtrl.clear(); - } - - /* ======================================================================== - * Handle start-/stop-line actions. - * ======================================================================== - */ - if ((TRACK_STATUS_START_STOP_LINE != m_trackStatus) && (TRACK_STATUS_START_STOP_LINE == nextTrackStatus)) - { - /* Start line detected? */ - if (LINE_STATUS_NO_START_LINE_DETECTED == m_lineStatus) - { - /* Measure the lap time and use as start point the detected start line. */ - m_lapTime.start(0); - - m_lineStatus = LINE_STATUS_START_LINE_DETECTED; - } - /* Stop line detected. */ - else - { - /* Calculate lap time and show it. */ - ReadyState::getInstance().setLapTime(m_lapTime.getCurrentDuration()); - - m_lineStatus = LINE_STATUS_STOP_LINE_DETECTED; - - /* Overwrite track status. */ - nextTrackStatus = TRACK_STATUS_FINISHED; - } - - /* Notify user about start-/stop-line detection. */ - Sound::playBeep(); - } - - /* ======================================================================== - * Handle track lost or back on track actions. - * ======================================================================== - */ - - /* Track lost just in this process cycle? */ - if ((false == m_isTrackLost) && (true == isTrackLost)) - { - /* Notify user by yellow LED. */ - Board::getInstance().getYellowLed().enable(true); - - /* Set mileage to 0, to be able to measure the max. distance, till - * the track must be found again. - */ - Odometry::getInstance().clearMileage(); - } - /* Track found again just in this process cycle? */ - else if ((true == m_isTrackLost) && (false == isTrackLost)) - { - Board::getInstance().getYellowLed().enable(false); - } - else - { - ; - } - - /* ======================================================================== - * Handle the abort conditions which will cause a alarm stop. - * ======================================================================== - */ - - /* Check whether the abort conditions are true. */ - if (true == isAbortRequired()) - { - /* Stop motors immediately. Don't move this to a later position, - * as this would extend the driven length. - */ - diffDrive.setLinearSpeed(0, 0); - - Sound::playAlarm(); /* Blocking! */ - - /* Clear lap time. */ - ReadyState::getInstance().setLapTime(0); - - /* Overwrite track status. */ - nextTrackStatus = TRACK_STATUS_FINISHED; - } - - /* ======================================================================== - * Handle driving based on position or normal stop condition. - * ======================================================================== - */ - - /* Periodically adapt driving and check the abort conditions, except - * the round is finished. - */ - if (TRACK_STATUS_FINISHED != nextTrackStatus) - { - /* Process the line follower PID controller periodically to adapt driving. */ - if (true == m_pidProcessTime.isTimeout()) - { - adaptDriving(position, allowNegativeMotorSpeed); - - m_pidProcessTime.start(PID_PROCESS_PERIOD); - } - } - /* Finished. */ - else - { - /* Stop motors immediately. Don't move this to a later position, - * as this would extend the driven length. - */ - diffDrive.setLinearSpeed(0, 0); - - /* Change to ready state. */ - sm.setState(&ReadyState::getInstance()); - } - -#ifdef DEBUG_ALGORITHM - logCsvDataTrackStatus(nextTrackStatus); - logCsvDataSpeed(gSpeedLeft, gSpeedRight); - printf("\n"); -#endif /* DEBUG_ALGORITHM */ - - /* Take over values for next cycle. */ - m_trackStatus = nextTrackStatus; - m_isTrackLost = isTrackLost; - m_lastPosition = position; -} - -void DrivingState::exit() -{ - m_observationTimer.stop(); - Board::getInstance().getYellowLed().enable(false); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -DrivingState::DrivingState() : - m_observationTimer(), - m_lapTime(), - m_pidProcessTime(), - m_pidCtrl(), - m_topSpeed(0), - m_lineStatus(LINE_STATUS_NO_START_LINE_DETECTED), - m_trackStatus(TRACK_STATUS_NORMAL), - m_isStartStopLineDetected(false), - m_lastSensorIdSawTrack(SENSOR_ID_MIDDLE), - m_lastPosition(0), - m_isTrackLost(false) -{ -} - -bool DrivingState::calcPosition3(int16_t& position, const uint16_t* lineSensorValues, uint8_t length) const -{ - const int32_t WEIGHT = SENSOR_VALUE_MAX; - bool isValid = true; - int32_t numerator = 0U; - int32_t denominator = 0U; - int32_t idxBegin = 1; - int32_t idxEnd = length - 1; - - for (int32_t idx = idxBegin; idx < idxEnd; ++idx) - { - int32_t sensorValue = static_cast(lineSensorValues[idx]); - - numerator += idx * WEIGHT * sensorValue; - denominator += sensorValue; - } - - if (0 == denominator) - { - isValid = false; - } - else - { - position = numerator / denominator; - } - - return isValid; -} - -DrivingState::TrackStatus DrivingState::evaluateSituation(const uint16_t* lineSensorValues, uint8_t length, - int16_t position, int16_t position3, bool isTrackLost) const -{ - TrackStatus nextTrackStatus = m_trackStatus; - - /* Driving over start-/stop-line? */ - if (TRACK_STATUS_START_STOP_LINE == m_trackStatus) - { - /* Left the start-/stop-line? - * If the robot is not exact on the start-/stop-line, the calculated position - * may misslead. Therefore additional the most left and right sensor values - * are evaluated too. - */ - if ((POSITION_MIDDLE_MIN <= position) && (POSITION_MIDDLE_MAX >= position) && - (LINE_SENSOR_ON_TRACK_MIN_VALUE > lineSensorValues[SENSOR_ID_MOST_LEFT]) && - (LINE_SENSOR_ON_TRACK_MIN_VALUE > lineSensorValues[SENSOR_ID_MOST_RIGHT])) - { - nextTrackStatus = TRACK_STATUS_NORMAL; - } - } - /* Is the start-/stop-line detected? */ - else if (true == isStartStopLineDetected(lineSensorValues, length)) - { - nextTrackStatus = TRACK_STATUS_START_STOP_LINE; - } - else if (TRACK_STATUS_RIGHT_ANGLE_CURVE_LEFT == m_trackStatus) - { - if ((POSITION_MIDDLE_MIN <= position) && (POSITION_MIDDLE_MAX >= position)) - { - nextTrackStatus = TRACK_STATUS_NORMAL; - } - } - else if (TRACK_STATUS_RIGHT_ANGLE_CURVE_RIGHT == m_trackStatus) - { - if ((POSITION_MIDDLE_MIN <= position) && (POSITION_MIDDLE_MAX >= position)) - { - nextTrackStatus = TRACK_STATUS_NORMAL; - } - } - /* Sharp curve to the left expected? */ - else if (TRACK_STATUS_SHARP_CURVE_LEFT == m_trackStatus) - { - /* Turn just before the robot leaves the line. */ - if (true == isTrackLost) - { - nextTrackStatus = TRACK_STATUS_SHARP_CURVE_LEFT_TURN; - } - } - /* Sharp curve to the right expected? */ - else if (TRACK_STATUS_SHARP_CURVE_RIGHT == m_trackStatus) - { - /* Turn just before the robot leaves the line. */ - if (true == isTrackLost) - { - nextTrackStatus = TRACK_STATUS_SHARP_CURVE_RIGHT_TURN; - } - } - /* Sharp curve to the left turning now! */ - else if (TRACK_STATUS_SHARP_CURVE_LEFT_TURN == m_trackStatus) - { - if ((POSITION_MIDDLE_MIN <= position3) && (POSITION_MIDDLE_MAX >= position3)) - { - nextTrackStatus = TRACK_STATUS_NORMAL; - } - } - /* Sharp curve to the right turning now! */ - else if (TRACK_STATUS_SHARP_CURVE_RIGHT_TURN == m_trackStatus) - { - if ((POSITION_MIDDLE_MIN <= position3) && (POSITION_MIDDLE_MAX >= position3)) - { - nextTrackStatus = TRACK_STATUS_NORMAL; - } - } - /* Is the track lost or just a gap in the track? */ - else if (true == isTrackLost) - { - const int16_t POS_MIN = POSITION_SET_POINT - SENSOR_VALUE_MAX; - const int16_t POS_MAX = POSITION_SET_POINT + SENSOR_VALUE_MAX; - - /* If its a gap in the track, last position will be well. */ - if ((POS_MIN <= m_lastPosition) && (POS_MAX > m_lastPosition)) - { - nextTrackStatus = TRACK_STATUS_TRACK_LOST_BY_GAP; - } - else - { - /* In any other case, route the calculated position through and - * hope. It will be the position of the most left sensor or the - * most right sensor. - */ - nextTrackStatus = TRACK_STATUS_TRACK_LOST_BY_MANOEUVRE; - } - } - /* Right angle curve to left detected? */ - else if (true == isRightAngleCurveToLeft(lineSensorValues, length)) - { - nextTrackStatus = TRACK_STATUS_RIGHT_ANGLE_CURVE_LEFT; - } - /* Right angle curve to right detected? */ - else if (true == isRightAngleCurveToRight(lineSensorValues, length)) - { - nextTrackStatus = TRACK_STATUS_RIGHT_ANGLE_CURVE_RIGHT; - } - /* Sharp curve to left detected? */ - else if (true == isSharpLeftCurveDetected(lineSensorValues, length, position3)) - { - nextTrackStatus = TRACK_STATUS_SHARP_CURVE_LEFT; - } - /* Sharp curve to right detected? */ - else if (true == isSharpRightCurveDetected(lineSensorValues, length, position3)) - { - nextTrackStatus = TRACK_STATUS_SHARP_CURVE_RIGHT; - } - /* Nothing special. */ - else - { - /* Just follow the line by calculated position. */ - nextTrackStatus = TRACK_STATUS_NORMAL; - } - - return nextTrackStatus; -} - -bool DrivingState::isStartStopLineDetected(const uint16_t* lineSensorValues, uint8_t length) const -{ - bool isDetected = false; - const uint32_t LINE_MAX_30 = (SENSOR_VALUE_MAX * 3U) / 10U; /* 30 % of max. value */ - const uint32_t LINE_MAX_70 = (SENSOR_VALUE_MAX * 7U) / 10U; /* 70 % of max. value */ - - /* - * === = === - * + + + + + - * L M R - */ - if ((LINE_MAX_30 <= lineSensorValues[SENSOR_ID_MOST_LEFT]) && - (LINE_MAX_70 > lineSensorValues[SENSOR_ID_MIDDLE - 1U]) && - (LINE_MAX_70 <= lineSensorValues[SENSOR_ID_MIDDLE]) && - (LINE_MAX_70 > lineSensorValues[SENSOR_ID_MIDDLE + 1U]) && - (LINE_MAX_30 <= lineSensorValues[SENSOR_ID_MOST_RIGHT])) - { - isDetected = true; - } - - return isDetected; -} - -bool DrivingState::isNoLineDetected(const uint16_t* lineSensorValues, uint8_t length) const -{ - bool isDetected = true; - uint8_t idx = SENSOR_ID_MOST_RIGHT; - - /* - * - * + + + + + - * L M R - */ - for (idx = SENSOR_ID_MOST_LEFT; idx <= SENSOR_ID_MOST_RIGHT; ++idx) - { - if (LINE_SENSOR_ON_TRACK_MIN_VALUE <= lineSensorValues[idx]) - { - isDetected = false; - break; - } - } - - return isDetected; -} - -bool DrivingState::isRightAngleCurveToLeft(const uint16_t* lineSensorValues, uint8_t length) const -{ - bool isDetected = true; - uint8_t idx = SENSOR_ID_MOST_LEFT; - - /* - * ========= - * + + + + + - * L M R - */ - for (idx = SENSOR_ID_MOST_LEFT; idx <= SENSOR_ID_MIDDLE; ++idx) - { - if (LINE_SENSOR_ON_TRACK_MIN_VALUE > lineSensorValues[idx]) - { - isDetected = false; - break; - } - } - - return isDetected; -} - -bool DrivingState::isRightAngleCurveToRight(const uint16_t* lineSensorValues, uint8_t length) const -{ - bool isDetected = true; - uint8_t idx = SENSOR_ID_MOST_RIGHT; - - /* - * ========= - * + + + + + - * L M R - */ - for (idx = SENSOR_ID_MIDDLE; idx <= SENSOR_ID_MOST_RIGHT; ++idx) - { - if (LINE_SENSOR_ON_TRACK_MIN_VALUE > lineSensorValues[idx]) - { - isDetected = false; - break; - } - } - - return isDetected; -} - -bool DrivingState::isSharpLeftCurveDetected(const uint16_t* lineSensorValues, uint8_t length, int16_t position3) const -{ - bool isDetected = false; - const uint32_t LINE_MAX_30 = (SENSOR_VALUE_MAX * 3U) / 10U; /* 30 % of max. value */ - - /* - * = = - * + + + + + - * L M R - */ - if ((LINE_MAX_30 <= lineSensorValues[SENSOR_ID_MOST_LEFT]) && (POSITION_MIDDLE_MIN <= position3) && - (POSITION_MIDDLE_MAX >= position3)) - { - isDetected = true; - } - - return isDetected; -} - -bool DrivingState::isSharpRightCurveDetected(const uint16_t* lineSensorValues, uint8_t length, int16_t position3) const -{ - bool isDetected = false; - const uint32_t LINE_MAX_30 = (SENSOR_VALUE_MAX * 3U) / 10U; /* 30 % of max. value */ - - /* - * = = - * + + + + + - * L M R - */ - if ((LINE_MAX_30 <= lineSensorValues[SENSOR_ID_MOST_RIGHT]) && (POSITION_MIDDLE_MIN <= position3) && - (POSITION_MIDDLE_MAX >= position3)) - { - isDetected = true; - } - - return isDetected; -} - -void DrivingState::processSituation(int16_t& position, bool& allowNegativeMotorSpeed, TrackStatus trackStatus, int16_t position3) -{ - switch (trackStatus) - { - case TRACK_STATUS_NORMAL: - /* The position is used by default to follow the line. */ - break; - - case TRACK_STATUS_START_STOP_LINE: - /* Use the inner sensors only for driving to avoid jerky movements, caused - * by the most left or right sensor. - */ - position = position3; - - /* Avoid that the robot turns in place. */ - allowNegativeMotorSpeed = false; - break; - - case TRACK_STATUS_RIGHT_ANGLE_CURVE_LEFT: - /* Enfore max. error in PID to turn sharp left at place. */ - position = POSITION_MIN; - break; - - case TRACK_STATUS_RIGHT_ANGLE_CURVE_RIGHT: - /* Enfore max. error in PID to turn sharp right at place. */ - position = POSITION_MAX; - break; - - case TRACK_STATUS_SHARP_CURVE_LEFT: - /* Stratey is to drive till the end of the curve with the inner sensors. - * Then a hard in place turn will appear, see TRACK_STATUS_SHARP_CURVE_LEFT_TURN. - */ - position = position3; - break; - - case TRACK_STATUS_SHARP_CURVE_RIGHT: - /* Stratey is to drive till the end of the curve with the inner sensors. - * Then a hard in place turn will appear, see TRACK_STATUS_SHARP_CURVE_LEFT_TURN. - */ - position = position3; - break; - - case TRACK_STATUS_SHARP_CURVE_LEFT_TURN: - /* Enfore max. error in PID to turn sharp left at place. */ - position = POSITION_MIN; - break; - - case TRACK_STATUS_SHARP_CURVE_RIGHT_TURN: - /* Enfore max. error in PID to turn sharp right at place. */ - position = POSITION_MAX; - break; - - case TRACK_STATUS_TRACK_LOST_BY_GAP: - /* Overwrite the positin to drive straight forward in hope to see the - * line again. - */ - position = POSITION_SET_POINT; - - /* Avoid that the robots turns. */ - allowNegativeMotorSpeed = false; - break; - - case TRACK_STATUS_TRACK_LOST_BY_MANOEUVRE: - /* If the track is lost by a robot manoeuvre and not becase the track - * has a gap, the last position given by most left or right sensor - * will be automatically used to find the track again. - * - * See ILineSensors::readLine() description regarding the behaviour in - * case the line is not detected anymore. - */ - break; - - case TRACK_STATUS_FINISHED: - /* Nothing to do. */ - break; - - default: - /* Should never happen. */ - break; - } -} - -void DrivingState::adaptDriving(int16_t position, bool allowNegativeMotorSpeed) -{ - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - const int16_t MAX_MOTOR_SPEED = diffDrive.getMaxMotorSpeed(); - const int16_t MIN_MOTOR_SPEED = (false == allowNegativeMotorSpeed) ? 0 : (-MAX_MOTOR_SPEED); - int16_t speedDifference = 0; /* [steps/s] */ - int16_t leftSpeed = 0; /* [steps/s] */ - int16_t rightSpeed = 0; /* [steps/s] */ - - /* Our "error" is how far we are away from the center of the - * line, which corresponds to position (max. line sensor value multiplied - * with sensor index). - * - * Get motor speed difference using PID terms. - */ - speedDifference = m_pidCtrl.calculate(POSITION_SET_POINT, position); - - /* Get individual motor speeds. The sign of speedDifference - * determines if the robot turns left or right. - */ - leftSpeed = m_topSpeed - speedDifference; - rightSpeed = m_topSpeed + speedDifference; - - /* Constrain our motor speeds to be between 0 and maxSpeed. - * One motor will always be turning at maxSpeed, and the other - * will be at maxSpeed-|speedDifference| if that is positive, - * else it will be stationary. For some applications, you - * might want to allow the motor speed to go negative so that - * it can spin in reverse. - */ - leftSpeed = constrain(leftSpeed, MIN_MOTOR_SPEED, MAX_MOTOR_SPEED); - rightSpeed = constrain(rightSpeed, MIN_MOTOR_SPEED, MAX_MOTOR_SPEED); - -#ifdef DEBUG_ALGORITHM - gSpeedLeft = leftSpeed; - gSpeedRight = rightSpeed; -#endif /* DEBUG_ALGORITHM */ - - diffDrive.setLinearSpeed(leftSpeed, rightSpeed); -} - -bool DrivingState::isAbortRequired() -{ - bool isAbort = false; - - /* If track is lost over a certain distance, abort driving. */ - if (true == m_isTrackLost) - { - /* Max. distance driven, but track still not found? */ - if (MAX_DISTANCE < Odometry::getInstance().getMileageCenter()) - { - isAbort = true; - } - } - - /* If track is not finished over a certain time, abort driving. */ - if (TRACK_STATUS_FINISHED != m_trackStatus) - { - if (true == m_observationTimer.isTimeout()) - { - isAbort = true; - } - } - - return isAbort; -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ - -#ifdef DEBUG_ALGORITHM - -/** - * Log the titles of each column as CSV data. - * - * @param[in] length Number of line sensors. - */ -static void logCsvDataTitles(uint8_t length) -{ - uint8_t idx = 0; - - printf("Timestamp"); - - while (length > idx) - { - printf(";Sensor %u", idx); - - ++idx; - } - - printf(";Position;Position3;Scenario;Speed Left; Speed Right\n"); -} - -/** - * Log the timestamp as CSV data. - */ -static void logCsvDataTimestamp() -{ - printf("%u;", millis()); -} - -/** - * Log the sensor values and the calculated positions as CSV data. - * - * @param[in] lineSensorValues The array of line sensor values. - * @param[in] length The number of line sensors. - * @param[in] pos The calculated position by all line sensors. - * @param[in] pos3 The calculated position by the inner line sensors. - * @param[in] isPos3Valid Flag to see whether the inner line sensors position is valid or not. - */ -static void logCsvData(const uint16_t* lineSensorValues, uint8_t length, int16_t pos, int16_t pos3, bool isPos3Valid) -{ - uint8_t idx = 0; - - while (length > idx) - { - if (0 < idx) - { - printf(";"); - } - - printf("%u", lineSensorValues[idx]); - - ++idx; - } - - printf(";%d;", pos); - - if (true == isPos3Valid) - { - printf("%d", pos3); - } -} - -/** - * Log the track status as CSV data. - * - * @param[in] trackStatus The track status. - */ -void logCsvDataTrackStatus(DrivingState::TrackStatus trackStatus) -{ - switch (trackStatus) - { - case DrivingState::TRACK_STATUS_NORMAL: - printf(";\"Normal\""); - break; - - case DrivingState::TRACK_STATUS_START_STOP_LINE: - printf(";\"Start-/Stop-line\""); - break; - - case DrivingState::TRACK_STATUS_RIGHT_ANGLE_CURVE_LEFT: - printf(";\"Right angle curve left\""); - break; - - case DrivingState::TRACK_STATUS_RIGHT_ANGLE_CURVE_RIGHT: - printf(";\"Right angle curve right\""); - break; - - case DrivingState::TRACK_STATUS_SHARP_CURVE_LEFT: - printf(";\"Sharp curve left\""); - break; - - case DrivingState::TRACK_STATUS_SHARP_CURVE_RIGHT: - printf(";\"Sharp curve right\""); - break; - - case DrivingState::TRACK_STATUS_SHARP_CURVE_LEFT_TURN: - printf(";\"Sharp curve left turn\""); - break; - - case DrivingState::TRACK_STATUS_SHARP_CURVE_RIGHT_TURN: - printf(";\"Sharp curve right turn\""); - break; - - case DrivingState::TRACK_STATUS_TRACK_LOST_BY_GAP: - printf(";\"Track lost by gap\""); - break; - - case DrivingState::TRACK_STATUS_TRACK_LOST_BY_MANOEUVRE: - printf(";\"Track lost by manoeuvre\""); - break; - - case DrivingState::TRACK_STATUS_FINISHED: - printf(";\"Track finished\""); - break; - - default: - printf(";\"?\""); - break; - } -} - -/** - * Log the motor speed set points as CSV data. - * - * @param[in] leftSpeed Right motor speed set point in steps/s. - * @param[in] rightSpeed Left motor speed set point in step/s. - */ -static void logCsvDataSpeed(int16_t leftSpeed, int16_t rightSpeed) -{ - printf(";%d;%d", leftSpeed, rightSpeed); -} - +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Driving state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "DrivingState.h" +#include +#include +#include +#include +#include +#include "ReadyState.h" +#include "ParameterSets.h" +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +#ifdef DEBUG_ALGORITHM +static void logCsvDataTitles(uint8_t length); +static void logCsvDataTimestamp(); +static void logCsvData(const uint16_t* lineSensorValues, uint8_t length, int16_t pos, int16_t pos3, bool isPos3Valid); +void logCsvDataTrackStatus(DrivingState::TrackStatus trackStatus); +static void logCsvDataSpeed(int16_t leftSpeed, int16_t rightSpeed); +#endif /* DEBUG_ALGORITHM */ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +#ifdef DEBUG_ALGORITHM +static int16_t gSpeedLeft = 0; +static int16_t gSpeedRight = 0; +#endif /* DEBUG_ALGORITHM */ + +const uint16_t DrivingState::SENSOR_VALUE_MAX = Board::getInstance().getLineSensors().getSensorValueMax(); + +/* Calculate the position set point to be generic. */ +const int16_t DrivingState::POSITION_SET_POINT = + (SENSOR_VALUE_MAX * (Board::getInstance().getLineSensors().getNumLineSensors() - 1)) / 2; + +/* Initialize the required sensor IDs to be generic. */ +const uint8_t DrivingState::SENSOR_ID_MOST_LEFT = 0U; +const uint8_t DrivingState::SENSOR_ID_MIDDLE = Board::getInstance().getLineSensors().getNumLineSensors() / 2U; +const uint8_t DrivingState::SENSOR_ID_MOST_RIGHT = Board::getInstance().getLineSensors().getNumLineSensors() - 1U; + +/* Initialize the position values used by the algorithmic. */ +const int16_t DrivingState::POSITION_MIN = 0; +const int16_t DrivingState::POSITION_MAX = + static_cast(Board::getInstance().getLineSensors().getNumLineSensors() - 1U) * 1000; +const int16_t DrivingState::POSITION_MIDDLE_MIN = POSITION_SET_POINT - (SENSOR_VALUE_MAX / 2); +const int16_t DrivingState::POSITION_MIDDLE_MAX = POSITION_SET_POINT + (SENSOR_VALUE_MAX / 2); + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void DrivingState::entry() +{ + IDisplay& display = Board::getInstance().getDisplay(); + const ParameterSets::ParameterSet& parSet = ParameterSets::getInstance().getParameterSet(); + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + const int16_t maxSpeed = diffDrive.getMaxMotorSpeed(); /* [steps/s] */ + + m_observationTimer.start(OBSERVATION_DURATION); + m_pidProcessTime.start(0); /* Immediate */ + m_lineStatus = LINE_STATUS_NO_START_LINE_DETECTED; + m_trackStatus = TRACK_STATUS_NORMAL; /* Assume that the robot is placed on track. */ + m_isTrackLost = false; /* Assume that the robot is placed on track. */ + m_isStartStopLineDetected = false; + + /* Configure PID controller with selected parameter set. */ + m_topSpeed = parSet.topSpeed; + m_pidCtrl.clear(); + m_pidCtrl.setPFactor(parSet.kPNumerator, parSet.kPDenominator); + m_pidCtrl.setIFactor(parSet.kINumerator, parSet.kIDenominator); + m_pidCtrl.setDFactor(parSet.kDNumerator, parSet.kDDenominator); + m_pidCtrl.setSampleTime(PID_PROCESS_PERIOD); + m_pidCtrl.setLimits(-maxSpeed, maxSpeed); + m_pidCtrl.setDerivativeOnMeasurement(false); + + display.clear(); + display.print("DRV"); + +#ifdef DEBUG_ALGORITHM + logCsvDataTitles(Board::getInstance().getLineSensors().getNumLineSensors()); +#endif /* DEBUG_ALGORITHM */ +} + +void DrivingState::process(StateMachine& sm) +{ + ILineSensors& lineSensors = Board::getInstance().getLineSensors(); + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + TrackStatus nextTrackStatus = m_trackStatus; + bool allowNegativeMotorSpeed = true; + + /* Get the position of the line and each sensor value. */ + int16_t position = lineSensors.readLine(); + const uint16_t* lineSensorValues = lineSensors.getSensorValues(); + uint8_t numLineSensors = lineSensors.getNumLineSensors(); + int16_t position3 = 0; + bool isPosition3Valid = calcPosition3(position3, lineSensorValues, numLineSensors); + bool isTrackLost = isNoLineDetected(lineSensorValues, numLineSensors); + +#ifdef DEBUG_ALGORITHM + logCsvDataTimestamp(); + logCsvData(lineSensorValues, numLineSensors, position, position3, isPosition3Valid); +#endif /* DEBUG_ALGORITHM */ + + /* If the position calculated with the inner sensors is not valid, the + * position will be taken. + */ + if (true == isPosition3Valid) + { + position3 = position; + } + + /* ======================================================================== + * Evaluate the situation based on the sensor values. + * ======================================================================== + */ + nextTrackStatus = evaluateSituation(lineSensorValues, numLineSensors, position, position3, isTrackLost); + + /* ======================================================================== + * Initiate measures depended on the situation. + * ======================================================================== + */ + processSituation(position, allowNegativeMotorSpeed, nextTrackStatus, position3); + + /* If the tracks status changes, the PID integral and derivative part + * will be reset to provide a smooth reaction. + */ + if (m_trackStatus != nextTrackStatus) + { + m_pidCtrl.clear(); + } + + /* ======================================================================== + * Handle start-/stop-line actions. + * ======================================================================== + */ + if ((TRACK_STATUS_START_STOP_LINE != m_trackStatus) && (TRACK_STATUS_START_STOP_LINE == nextTrackStatus)) + { + /* Start line detected? */ + if (LINE_STATUS_NO_START_LINE_DETECTED == m_lineStatus) + { + /* Measure the lap time and use as start point the detected start line. */ + m_lapTime.start(0); + + m_lineStatus = LINE_STATUS_START_LINE_DETECTED; + } + /* Stop line detected. */ + else + { + /* Calculate lap time and show it. */ + ReadyState::getInstance().setLapTime(m_lapTime.getCurrentDuration()); + + m_lineStatus = LINE_STATUS_STOP_LINE_DETECTED; + + /* Overwrite track status. */ + nextTrackStatus = TRACK_STATUS_FINISHED; + } + + /* Notify user about start-/stop-line detection. */ + Sound::playBeep(); + } + + /* ======================================================================== + * Handle track lost or back on track actions. + * ======================================================================== + */ + + /* Track lost just in this process cycle? */ + if ((false == m_isTrackLost) && (true == isTrackLost)) + { + /* Notify user by yellow LED. */ + Board::getInstance().getYellowLed().enable(true); + + /* Set mileage to 0, to be able to measure the max. distance, till + * the track must be found again. + */ + Odometry::getInstance().clearMileage(); + } + /* Track found again just in this process cycle? */ + else if ((true == m_isTrackLost) && (false == isTrackLost)) + { + Board::getInstance().getYellowLed().enable(false); + } + else + { + ; + } + + /* ======================================================================== + * Handle the abort conditions which will cause a alarm stop. + * ======================================================================== + */ + + /* Check whether the abort conditions are true. */ + if (true == isAbortRequired()) + { + /* Stop motors immediately. Don't move this to a later position, + * as this would extend the driven length. + */ + diffDrive.setLinearSpeed(0, 0); + + Sound::playAlarm(); /* Blocking! */ + + /* Clear lap time. */ + ReadyState::getInstance().setLapTime(0); + + /* Overwrite track status. */ + nextTrackStatus = TRACK_STATUS_FINISHED; + } + + /* ======================================================================== + * Handle driving based on position or normal stop condition. + * ======================================================================== + */ + + /* Periodically adapt driving and check the abort conditions, except + * the round is finished. + */ + if (TRACK_STATUS_FINISHED != nextTrackStatus) + { + /* Process the line follower PID controller periodically to adapt driving. */ + if (true == m_pidProcessTime.isTimeout()) + { + adaptDriving(position, allowNegativeMotorSpeed); + + m_pidProcessTime.start(PID_PROCESS_PERIOD); + } + } + /* Finished. */ + else + { + /* Stop motors immediately. Don't move this to a later position, + * as this would extend the driven length. + */ + diffDrive.setLinearSpeed(0, 0); + + /* Change to ready state. */ + sm.setState(&ReadyState::getInstance()); + } + +#ifdef DEBUG_ALGORITHM + logCsvDataTrackStatus(nextTrackStatus); + logCsvDataSpeed(gSpeedLeft, gSpeedRight); + printf("\n"); +#endif /* DEBUG_ALGORITHM */ + + /* Take over values for next cycle. */ + m_trackStatus = nextTrackStatus; + m_isTrackLost = isTrackLost; + m_lastPosition = position; +} + +void DrivingState::exit() +{ + m_observationTimer.stop(); + Board::getInstance().getYellowLed().enable(false); +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +DrivingState::DrivingState() : + m_observationTimer(), + m_lapTime(), + m_pidProcessTime(), + m_pidCtrl(), + m_topSpeed(0), + m_lineStatus(LINE_STATUS_NO_START_LINE_DETECTED), + m_trackStatus(TRACK_STATUS_NORMAL), + m_isStartStopLineDetected(false), + m_lastSensorIdSawTrack(SENSOR_ID_MIDDLE), + m_lastPosition(0), + m_isTrackLost(false) +{ +} + +bool DrivingState::calcPosition3(int16_t& position, const uint16_t* lineSensorValues, uint8_t length) const +{ + const int32_t WEIGHT = SENSOR_VALUE_MAX; + bool isValid = true; + int32_t numerator = 0U; + int32_t denominator = 0U; + int32_t idxBegin = 1; + int32_t idxEnd = length - 1; + + for (int32_t idx = idxBegin; idx < idxEnd; ++idx) + { + int32_t sensorValue = static_cast(lineSensorValues[idx]); + + numerator += idx * WEIGHT * sensorValue; + denominator += sensorValue; + } + + if (0 == denominator) + { + isValid = false; + } + else + { + position = numerator / denominator; + } + + return isValid; +} + +DrivingState::TrackStatus DrivingState::evaluateSituation(const uint16_t* lineSensorValues, uint8_t length, + int16_t position, int16_t position3, bool isTrackLost) const +{ + TrackStatus nextTrackStatus = m_trackStatus; + + /* Driving over start-/stop-line? */ + if (TRACK_STATUS_START_STOP_LINE == m_trackStatus) + { + /* Left the start-/stop-line? + * If the robot is not exact on the start-/stop-line, the calculated position + * may misslead. Therefore additional the most left and right sensor values + * are evaluated too. + */ + if ((POSITION_MIDDLE_MIN <= position) && (POSITION_MIDDLE_MAX >= position) && + (LINE_SENSOR_ON_TRACK_MIN_VALUE > lineSensorValues[SENSOR_ID_MOST_LEFT]) && + (LINE_SENSOR_ON_TRACK_MIN_VALUE > lineSensorValues[SENSOR_ID_MOST_RIGHT])) + { + nextTrackStatus = TRACK_STATUS_NORMAL; + } + } + /* Is the start-/stop-line detected? */ + else if (true == isStartStopLineDetected(lineSensorValues, length)) + { + nextTrackStatus = TRACK_STATUS_START_STOP_LINE; + } + else if (TRACK_STATUS_RIGHT_ANGLE_CURVE_LEFT == m_trackStatus) + { + if ((POSITION_MIDDLE_MIN <= position) && (POSITION_MIDDLE_MAX >= position)) + { + nextTrackStatus = TRACK_STATUS_NORMAL; + } + } + else if (TRACK_STATUS_RIGHT_ANGLE_CURVE_RIGHT == m_trackStatus) + { + if ((POSITION_MIDDLE_MIN <= position) && (POSITION_MIDDLE_MAX >= position)) + { + nextTrackStatus = TRACK_STATUS_NORMAL; + } + } + /* Sharp curve to the left expected? */ + else if (TRACK_STATUS_SHARP_CURVE_LEFT == m_trackStatus) + { + /* Turn just before the robot leaves the line. */ + if (true == isTrackLost) + { + nextTrackStatus = TRACK_STATUS_SHARP_CURVE_LEFT_TURN; + } + } + /* Sharp curve to the right expected? */ + else if (TRACK_STATUS_SHARP_CURVE_RIGHT == m_trackStatus) + { + /* Turn just before the robot leaves the line. */ + if (true == isTrackLost) + { + nextTrackStatus = TRACK_STATUS_SHARP_CURVE_RIGHT_TURN; + } + } + /* Sharp curve to the left turning now! */ + else if (TRACK_STATUS_SHARP_CURVE_LEFT_TURN == m_trackStatus) + { + if ((POSITION_MIDDLE_MIN <= position3) && (POSITION_MIDDLE_MAX >= position3)) + { + nextTrackStatus = TRACK_STATUS_NORMAL; + } + } + /* Sharp curve to the right turning now! */ + else if (TRACK_STATUS_SHARP_CURVE_RIGHT_TURN == m_trackStatus) + { + if ((POSITION_MIDDLE_MIN <= position3) && (POSITION_MIDDLE_MAX >= position3)) + { + nextTrackStatus = TRACK_STATUS_NORMAL; + } + } + /* Is the track lost or just a gap in the track? */ + else if (true == isTrackLost) + { + const int16_t POS_MIN = POSITION_SET_POINT - SENSOR_VALUE_MAX; + const int16_t POS_MAX = POSITION_SET_POINT + SENSOR_VALUE_MAX; + + /* If its a gap in the track, last position will be well. */ + if ((POS_MIN <= m_lastPosition) && (POS_MAX > m_lastPosition)) + { + nextTrackStatus = TRACK_STATUS_TRACK_LOST_BY_GAP; + } + else + { + /* In any other case, route the calculated position through and + * hope. It will be the position of the most left sensor or the + * most right sensor. + */ + nextTrackStatus = TRACK_STATUS_TRACK_LOST_BY_MANOEUVRE; + } + } + /* Right angle curve to left detected? */ + else if (true == isRightAngleCurveToLeft(lineSensorValues, length)) + { + nextTrackStatus = TRACK_STATUS_RIGHT_ANGLE_CURVE_LEFT; + } + /* Right angle curve to right detected? */ + else if (true == isRightAngleCurveToRight(lineSensorValues, length)) + { + nextTrackStatus = TRACK_STATUS_RIGHT_ANGLE_CURVE_RIGHT; + } + /* Sharp curve to left detected? */ + else if (true == isSharpLeftCurveDetected(lineSensorValues, length, position3)) + { + nextTrackStatus = TRACK_STATUS_SHARP_CURVE_LEFT; + } + /* Sharp curve to right detected? */ + else if (true == isSharpRightCurveDetected(lineSensorValues, length, position3)) + { + nextTrackStatus = TRACK_STATUS_SHARP_CURVE_RIGHT; + } + /* Nothing special. */ + else + { + /* Just follow the line by calculated position. */ + nextTrackStatus = TRACK_STATUS_NORMAL; + } + + return nextTrackStatus; +} + +bool DrivingState::isStartStopLineDetected(const uint16_t* lineSensorValues, uint8_t length) const +{ + bool isDetected = false; + const uint32_t LINE_MAX_30 = (SENSOR_VALUE_MAX * 3U) / 10U; /* 30 % of max. value */ + const uint32_t LINE_MAX_70 = (SENSOR_VALUE_MAX * 7U) / 10U; /* 70 % of max. value */ + + /* + * === = === + * + + + + + + * L M R + */ + if ((LINE_MAX_30 <= lineSensorValues[SENSOR_ID_MOST_LEFT]) && + (LINE_MAX_70 > lineSensorValues[SENSOR_ID_MIDDLE - 1U]) && + (LINE_MAX_70 <= lineSensorValues[SENSOR_ID_MIDDLE]) && + (LINE_MAX_70 > lineSensorValues[SENSOR_ID_MIDDLE + 1U]) && + (LINE_MAX_30 <= lineSensorValues[SENSOR_ID_MOST_RIGHT])) + { + isDetected = true; + } + + return isDetected; +} + +bool DrivingState::isNoLineDetected(const uint16_t* lineSensorValues, uint8_t length) const +{ + bool isDetected = true; + uint8_t idx = SENSOR_ID_MOST_RIGHT; + + /* + * + * + + + + + + * L M R + */ + for (idx = SENSOR_ID_MOST_LEFT; idx <= SENSOR_ID_MOST_RIGHT; ++idx) + { + if (LINE_SENSOR_ON_TRACK_MIN_VALUE <= lineSensorValues[idx]) + { + isDetected = false; + break; + } + } + + return isDetected; +} + +bool DrivingState::isRightAngleCurveToLeft(const uint16_t* lineSensorValues, uint8_t length) const +{ + bool isDetected = true; + uint8_t idx = SENSOR_ID_MOST_LEFT; + + /* + * ========= + * + + + + + + * L M R + */ + for (idx = SENSOR_ID_MOST_LEFT; idx <= SENSOR_ID_MIDDLE; ++idx) + { + if (LINE_SENSOR_ON_TRACK_MIN_VALUE > lineSensorValues[idx]) + { + isDetected = false; + break; + } + } + + return isDetected; +} + +bool DrivingState::isRightAngleCurveToRight(const uint16_t* lineSensorValues, uint8_t length) const +{ + bool isDetected = true; + uint8_t idx = SENSOR_ID_MOST_RIGHT; + + /* + * ========= + * + + + + + + * L M R + */ + for (idx = SENSOR_ID_MIDDLE; idx <= SENSOR_ID_MOST_RIGHT; ++idx) + { + if (LINE_SENSOR_ON_TRACK_MIN_VALUE > lineSensorValues[idx]) + { + isDetected = false; + break; + } + } + + return isDetected; +} + +bool DrivingState::isSharpLeftCurveDetected(const uint16_t* lineSensorValues, uint8_t length, int16_t position3) const +{ + bool isDetected = false; + const uint32_t LINE_MAX_30 = (SENSOR_VALUE_MAX * 3U) / 10U; /* 30 % of max. value */ + + /* + * = = + * + + + + + + * L M R + */ + if ((LINE_MAX_30 <= lineSensorValues[SENSOR_ID_MOST_LEFT]) && (POSITION_MIDDLE_MIN <= position3) && + (POSITION_MIDDLE_MAX >= position3)) + { + isDetected = true; + } + + return isDetected; +} + +bool DrivingState::isSharpRightCurveDetected(const uint16_t* lineSensorValues, uint8_t length, int16_t position3) const +{ + bool isDetected = false; + const uint32_t LINE_MAX_30 = (SENSOR_VALUE_MAX * 3U) / 10U; /* 30 % of max. value */ + + /* + * = = + * + + + + + + * L M R + */ + if ((LINE_MAX_30 <= lineSensorValues[SENSOR_ID_MOST_RIGHT]) && (POSITION_MIDDLE_MIN <= position3) && + (POSITION_MIDDLE_MAX >= position3)) + { + isDetected = true; + } + + return isDetected; +} + +void DrivingState::processSituation(int16_t& position, bool& allowNegativeMotorSpeed, TrackStatus trackStatus, int16_t position3) +{ + switch (trackStatus) + { + case TRACK_STATUS_NORMAL: + /* The position is used by default to follow the line. */ + break; + + case TRACK_STATUS_START_STOP_LINE: + /* Use the inner sensors only for driving to avoid jerky movements, caused + * by the most left or right sensor. + */ + position = position3; + + /* Avoid that the robot turns in place. */ + allowNegativeMotorSpeed = false; + break; + + case TRACK_STATUS_RIGHT_ANGLE_CURVE_LEFT: + /* Enfore max. error in PID to turn sharp left at place. */ + position = POSITION_MIN; + break; + + case TRACK_STATUS_RIGHT_ANGLE_CURVE_RIGHT: + /* Enfore max. error in PID to turn sharp right at place. */ + position = POSITION_MAX; + break; + + case TRACK_STATUS_SHARP_CURVE_LEFT: + /* Stratey is to drive till the end of the curve with the inner sensors. + * Then a hard in place turn will appear, see TRACK_STATUS_SHARP_CURVE_LEFT_TURN. + */ + position = position3; + break; + + case TRACK_STATUS_SHARP_CURVE_RIGHT: + /* Stratey is to drive till the end of the curve with the inner sensors. + * Then a hard in place turn will appear, see TRACK_STATUS_SHARP_CURVE_LEFT_TURN. + */ + position = position3; + break; + + case TRACK_STATUS_SHARP_CURVE_LEFT_TURN: + /* Enfore max. error in PID to turn sharp left at place. */ + position = POSITION_MIN; + break; + + case TRACK_STATUS_SHARP_CURVE_RIGHT_TURN: + /* Enfore max. error in PID to turn sharp right at place. */ + position = POSITION_MAX; + break; + + case TRACK_STATUS_TRACK_LOST_BY_GAP: + /* Overwrite the positin to drive straight forward in hope to see the + * line again. + */ + position = POSITION_SET_POINT; + + /* Avoid that the robots turns. */ + allowNegativeMotorSpeed = false; + break; + + case TRACK_STATUS_TRACK_LOST_BY_MANOEUVRE: + /* If the track is lost by a robot manoeuvre and not becase the track + * has a gap, the last position given by most left or right sensor + * will be automatically used to find the track again. + * + * See ILineSensors::readLine() description regarding the behaviour in + * case the line is not detected anymore. + */ + break; + + case TRACK_STATUS_FINISHED: + /* Nothing to do. */ + break; + + default: + /* Should never happen. */ + break; + } +} + +void DrivingState::adaptDriving(int16_t position, bool allowNegativeMotorSpeed) +{ + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + const int16_t MAX_MOTOR_SPEED = diffDrive.getMaxMotorSpeed(); + const int16_t MIN_MOTOR_SPEED = (false == allowNegativeMotorSpeed) ? 0 : (-MAX_MOTOR_SPEED); + int16_t speedDifference = 0; /* [steps/s] */ + int16_t leftSpeed = 0; /* [steps/s] */ + int16_t rightSpeed = 0; /* [steps/s] */ + + /* Our "error" is how far we are away from the center of the + * line, which corresponds to position (max. line sensor value multiplied + * with sensor index). + * + * Get motor speed difference using PID terms. + */ + speedDifference = m_pidCtrl.calculate(POSITION_SET_POINT, position); + + /* Get individual motor speeds. The sign of speedDifference + * determines if the robot turns left or right. + */ + leftSpeed = m_topSpeed - speedDifference; + rightSpeed = m_topSpeed + speedDifference; + + /* Constrain our motor speeds to be between 0 and maxSpeed. + * One motor will always be turning at maxSpeed, and the other + * will be at maxSpeed-|speedDifference| if that is positive, + * else it will be stationary. For some applications, you + * might want to allow the motor speed to go negative so that + * it can spin in reverse. + */ + leftSpeed = constrain(leftSpeed, MIN_MOTOR_SPEED, MAX_MOTOR_SPEED); + rightSpeed = constrain(rightSpeed, MIN_MOTOR_SPEED, MAX_MOTOR_SPEED); + +#ifdef DEBUG_ALGORITHM + gSpeedLeft = leftSpeed; + gSpeedRight = rightSpeed; +#endif /* DEBUG_ALGORITHM */ + + diffDrive.setLinearSpeed(leftSpeed, rightSpeed); +} + +bool DrivingState::isAbortRequired() +{ + bool isAbort = false; + + /* If track is lost over a certain distance, abort driving. */ + if (true == m_isTrackLost) + { + /* Max. distance driven, but track still not found? */ + if (MAX_DISTANCE < Odometry::getInstance().getMileageCenter()) + { + isAbort = true; + } + } + + /* If track is not finished over a certain time, abort driving. */ + if (TRACK_STATUS_FINISHED != m_trackStatus) + { + if (true == m_observationTimer.isTimeout()) + { + isAbort = true; + } + } + + return isAbort; +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ + +#ifdef DEBUG_ALGORITHM + +/** + * Log the titles of each column as CSV data. + * + * @param[in] length Number of line sensors. + */ +static void logCsvDataTitles(uint8_t length) +{ + uint8_t idx = 0; + + printf("Timestamp"); + + while (length > idx) + { + printf(";Sensor %u", idx); + + ++idx; + } + + printf(";Position;Position3;Scenario;Speed Left; Speed Right\n"); +} + +/** + * Log the timestamp as CSV data. + */ +static void logCsvDataTimestamp() +{ + printf("%u;", millis()); +} + +/** + * Log the sensor values and the calculated positions as CSV data. + * + * @param[in] lineSensorValues The array of line sensor values. + * @param[in] length The number of line sensors. + * @param[in] pos The calculated position by all line sensors. + * @param[in] pos3 The calculated position by the inner line sensors. + * @param[in] isPos3Valid Flag to see whether the inner line sensors position is valid or not. + */ +static void logCsvData(const uint16_t* lineSensorValues, uint8_t length, int16_t pos, int16_t pos3, bool isPos3Valid) +{ + uint8_t idx = 0; + + while (length > idx) + { + if (0 < idx) + { + printf(";"); + } + + printf("%u", lineSensorValues[idx]); + + ++idx; + } + + printf(";%d;", pos); + + if (true == isPos3Valid) + { + printf("%d", pos3); + } +} + +/** + * Log the track status as CSV data. + * + * @param[in] trackStatus The track status. + */ +void logCsvDataTrackStatus(DrivingState::TrackStatus trackStatus) +{ + switch (trackStatus) + { + case DrivingState::TRACK_STATUS_NORMAL: + printf(";\"Normal\""); + break; + + case DrivingState::TRACK_STATUS_START_STOP_LINE: + printf(";\"Start-/Stop-line\""); + break; + + case DrivingState::TRACK_STATUS_RIGHT_ANGLE_CURVE_LEFT: + printf(";\"Right angle curve left\""); + break; + + case DrivingState::TRACK_STATUS_RIGHT_ANGLE_CURVE_RIGHT: + printf(";\"Right angle curve right\""); + break; + + case DrivingState::TRACK_STATUS_SHARP_CURVE_LEFT: + printf(";\"Sharp curve left\""); + break; + + case DrivingState::TRACK_STATUS_SHARP_CURVE_RIGHT: + printf(";\"Sharp curve right\""); + break; + + case DrivingState::TRACK_STATUS_SHARP_CURVE_LEFT_TURN: + printf(";\"Sharp curve left turn\""); + break; + + case DrivingState::TRACK_STATUS_SHARP_CURVE_RIGHT_TURN: + printf(";\"Sharp curve right turn\""); + break; + + case DrivingState::TRACK_STATUS_TRACK_LOST_BY_GAP: + printf(";\"Track lost by gap\""); + break; + + case DrivingState::TRACK_STATUS_TRACK_LOST_BY_MANOEUVRE: + printf(";\"Track lost by manoeuvre\""); + break; + + case DrivingState::TRACK_STATUS_FINISHED: + printf(";\"Track finished\""); + break; + + default: + printf(";\"?\""); + break; + } +} + +/** + * Log the motor speed set points as CSV data. + * + * @param[in] leftSpeed Right motor speed set point in steps/s. + * @param[in] rightSpeed Left motor speed set point in step/s. + */ +static void logCsvDataSpeed(int16_t leftSpeed, int16_t rightSpeed) +{ + printf(";%d;%d", leftSpeed, rightSpeed); +} + #endif /* DEBUG_ALGORITHM */ \ No newline at end of file diff --git a/lib/APPLineFollower/DrivingState.h b/lib/APPLineFollower/DrivingState.h index c4666ded..cef98de6 100644 --- a/lib/APPLineFollower/DrivingState.h +++ b/lib/APPLineFollower/DrivingState.h @@ -1,346 +1,346 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Driving state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef DRIVING_STATE_H -#define DRIVING_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The system driving state. */ -class DrivingState : public IState -{ -public: - /** - * Get state instance. - * - * @return State instance. - */ - static DrivingState& getInstance() - { - static DrivingState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - -protected: -private: - /** - * The line detection status. - */ - enum LineStatus - { - LINE_STATUS_NO_START_LINE_DETECTED = 0, /**< No start line detected. */ - LINE_STATUS_START_LINE_DETECTED, /**< Start line detected. */ - LINE_STATUS_STOP_LINE_DETECTED /**< Stop line detected. */ - }; - - /** - * The track status. - */ - enum TrackStatus - { - TRACK_STATUS_NORMAL = 0, /**< Normal line conditions. */ - TRACK_STATUS_START_STOP_LINE, /**< Driving over start-/stop-line. */ - TRACK_STATUS_RIGHT_ANGLE_CURVE_LEFT, /**< Right angle curve to the left detected. */ - TRACK_STATUS_RIGHT_ANGLE_CURVE_RIGHT, /**< Right angle curve to the right detected. */ - TRACK_STATUS_SHARP_CURVE_LEFT, /**< Sharp curve to the left detected. */ - TRACK_STATUS_SHARP_CURVE_RIGHT, /**< Sharp curve to the right detected. */ - TRACK_STATUS_SHARP_CURVE_LEFT_TURN, /**< Sharp curve to the left turns now. */ - TRACK_STATUS_SHARP_CURVE_RIGHT_TURN, /**< Sharp curve to the right turns now. */ - TRACK_STATUS_TRACK_LOST_BY_GAP, /**< Track lost by gap. */ - TRACK_STATUS_TRACK_LOST_BY_MANOEUVRE, /**< Track lost by driving manoeuvre. */ - TRACK_STATUS_FINISHED /**< Robot found the end line or a error happened. */ - }; - - /** Observation duration in ms. This is the max. time within the robot must be finished its drive. */ - static const uint32_t OBSERVATION_DURATION = 3000000; - - /** Max. distance in mm after a lost track must be found again. */ - static const uint32_t MAX_DISTANCE = 200; - - /** Period in ms for PID processing. */ - static const uint32_t PID_PROCESS_PERIOD = 10; - - /** - * The line sensor threshold (normalized) used to detect the track. - * The track is detected in case the received value is greater or equal than - * the threshold. - */ - static const uint16_t LINE_SENSOR_ON_TRACK_MIN_VALUE = 200U; - - /** - * The max. normalized value of a sensor in digits. - */ - static const uint16_t SENSOR_VALUE_MAX; - - /** - * Position set point which is the perfect on track position. - * This is the goal to achieve. - */ - static const int16_t POSITION_SET_POINT; - - /** - * ID of most left sensor. - */ - static const uint8_t SENSOR_ID_MOST_LEFT; - - /** - * ID of most right sensor. - */ - static const uint8_t SENSOR_ID_MIDDLE; - - /** - * ID of middle sensor. - */ - static const uint8_t SENSOR_ID_MOST_RIGHT; - - /** - * Minimum position in digits. - */ - static const int16_t POSITION_MIN; - - /** - * Maximum position in digits. - */ - static const int16_t POSITION_MAX; - - /** - * Lower border position in digits for driving will on the line. - */ - static const int16_t POSITION_MIDDLE_MIN; - - /** - * Higher border position in digits for driving will on the line. - */ - static const int16_t POSITION_MIDDLE_MAX; - - SimpleTimer m_observationTimer; /**< Observation timer to observe the max. time per challenge. */ - SimpleTimer m_lapTime; /**< Timer used to calculate the lap time. */ - SimpleTimer m_pidProcessTime; /**< Timer used for periodically PID processing. */ - PIDController m_pidCtrl; /**< PID controller, used for driving. */ - int16_t m_topSpeed; /**< Top speed in [steps/s]. It might be lower or equal to the max. speed! */ - LineStatus m_lineStatus; /**< Status of start-/end line detection */ - TrackStatus m_trackStatus; /**< Status of track which means on track or track lost, etc. */ - bool m_isStartStopLineDetected; /**< Is the start/stop line detected? */ - uint8_t m_lastSensorIdSawTrack; /**< The sensor id of the sensor which saw the track as last. */ - int16_t m_lastPosition; /**< Last position, used to decide strategy in case of a track gap. */ - bool m_isTrackLost; /**< Is the track lost? Lost means the line sensors didn't detect it. */ - - /** - * Default constructor. - */ - DrivingState(); - - /** - * Default destructor. - */ - ~DrivingState() - { - } - - /* Not allowed. */ - DrivingState(const DrivingState& state); /**< Copy construction of an instance. */ - DrivingState& operator=(const DrivingState& state); /**< Assignment of an instance. */ - - /** - * Calculate the position with the inner 3 line sensors. - * - * @param[out] position The position result. - * @param[in] lineSensorValues Array of line sensor values. - * @param[in] length Array length. - * - * @return If successful, it will return true otherwise false. - */ - bool calcPosition3(int16_t& position, const uint16_t* lineSensorValues, uint8_t length) const; - - /** - * Evaluate the situation by line sensor values and position and determine - * the track status. The result influences the measures to keep track on - * the line. - * - * @param[in] lineSensorValues The line sensor values as array. - * @param[in] length The number of line sensor values. - * @param[in] position The position calculated with all sensors. - * @param[in] position3 The position calculated with the inner 3 sensors only. - * @param[in] isTrackLost Information whether the track is lost or not. - * - * @return The track status result. - */ - TrackStatus evaluateSituation(const uint16_t* lineSensorValues, uint8_t length, int16_t position, int16_t position3, bool isTrackLost) const; - - /** - * Is the start/stop line detected? - * - * @param[in] lineSensorValues The line sensor values as array. - * @param[in] length The number of line sensor values. - * - * @return If start/stop line detected, it will return true otherwise false. - */ - bool isStartStopLineDetected(const uint16_t* lineSensorValues, uint8_t length) const; - - /** - * Is no line detected? - * - * @param[in] lineSensorValues The line sensor values as array. - * @param[in] length The number of line sensor values. - * - * @return If no line is detected, it will return true otherwise false. - */ - bool isNoLineDetected(const uint16_t* lineSensorValues, uint8_t length) const; - - /** - * Is right angle curve to the left detected? - * - * @param[in] lineSensorValues The line sensor values as array. - * @param[in] length The number of line sensor values. - * - * @return If right angle curve to the left is detected, it will return true otherwise false. - */ - bool isRightAngleCurveToLeft(const uint16_t* lineSensorValues, uint8_t length) const; - - /** - * Is right angle curve to the right detected? - * - * @param[in] lineSensorValues The line sensor values as array. - * @param[in] length The number of line sensor values. - * - * @return If right angle curve to the right is detected, it will return true otherwise false. - */ - bool isRightAngleCurveToRight(const uint16_t* lineSensorValues, uint8_t length) const; - - /** - * Is a sharp left curve detected? - * - * @param[in] lineSensorValues The line sensor values as array. - * @param[in] length The number of line sensor values. - * @param[in] position3 The position calculated with the inner line sensors in digits. - * - * @return If sharp left curve is detected, it will return true otherwise false. - */ - bool isSharpLeftCurveDetected(const uint16_t* lineSensorValues, uint8_t length, int16_t position3) const; - - /** - * Is a sharp right curve detected? - * - * @param[in] lineSensorValues The line sensor values as array. - * @param[in] length The number of line sensor values. - * @param[in] position3 The position calculated with the inner line sensors in digits. - * - * @return If sharp right curve is detected, it will return true otherwise false. - */ - bool isSharpRightCurveDetected(const uint16_t* lineSensorValues, uint8_t length, int16_t position3) const; - - /** - * Process the situation and decide which measures to take. - * Measures will influence the position or whether its allowed to have - * a negative motor speed. - * - * @param[in, out] position The position calculated with all sensors in digits. - * @param[out] allowNegativeMotorSpeed Allow negative motor speed or not. - * @param[in] trackStatus The evaluated track status. - * @param[in] position3 The position calculated with the inner sensors in digits. - */ - void processSituation(int16_t& position, bool& allowNegativeMotorSpeed, TrackStatus trackStatus, int16_t position3); - - /** - * Adapt driving by using a PID algorithm, depended on the position - * input. - * - * @param[in] position Position in digits - * @param[in] allowNegativeMotorSpeed Allow negative motor speed. - */ - void adaptDriving(int16_t position, bool allowNegativeMotorSpeed); - - /** - * Check the abort conditions while driving the challenge. - * - * @return If abort is required, it will return true otherwise false. - */ - bool isAbortRequired(); - - -#ifdef DEBUG_ALGORITHM - /** - * Required for debugging purposes. - * - * @param[in] trackStatus The track status. - */ - friend void logCsvDataTrackStatus(TrackStatus trackStatus); -#endif /* DEBUG_ALGORITHM */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* DRIVING_STATE_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Driving state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef DRIVING_STATE_H +#define DRIVING_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The system driving state. */ +class DrivingState : public IState +{ +public: + /** + * Get state instance. + * + * @return State instance. + */ + static DrivingState& getInstance() + { + static DrivingState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + +protected: +private: + /** + * The line detection status. + */ + enum LineStatus + { + LINE_STATUS_NO_START_LINE_DETECTED = 0, /**< No start line detected. */ + LINE_STATUS_START_LINE_DETECTED, /**< Start line detected. */ + LINE_STATUS_STOP_LINE_DETECTED /**< Stop line detected. */ + }; + + /** + * The track status. + */ + enum TrackStatus + { + TRACK_STATUS_NORMAL = 0, /**< Normal line conditions. */ + TRACK_STATUS_START_STOP_LINE, /**< Driving over start-/stop-line. */ + TRACK_STATUS_RIGHT_ANGLE_CURVE_LEFT, /**< Right angle curve to the left detected. */ + TRACK_STATUS_RIGHT_ANGLE_CURVE_RIGHT, /**< Right angle curve to the right detected. */ + TRACK_STATUS_SHARP_CURVE_LEFT, /**< Sharp curve to the left detected. */ + TRACK_STATUS_SHARP_CURVE_RIGHT, /**< Sharp curve to the right detected. */ + TRACK_STATUS_SHARP_CURVE_LEFT_TURN, /**< Sharp curve to the left turns now. */ + TRACK_STATUS_SHARP_CURVE_RIGHT_TURN, /**< Sharp curve to the right turns now. */ + TRACK_STATUS_TRACK_LOST_BY_GAP, /**< Track lost by gap. */ + TRACK_STATUS_TRACK_LOST_BY_MANOEUVRE, /**< Track lost by driving manoeuvre. */ + TRACK_STATUS_FINISHED /**< Robot found the end line or a error happened. */ + }; + + /** Observation duration in ms. This is the max. time within the robot must be finished its drive. */ + static const uint32_t OBSERVATION_DURATION = 3000000; + + /** Max. distance in mm after a lost track must be found again. */ + static const uint32_t MAX_DISTANCE = 200; + + /** Period in ms for PID processing. */ + static const uint32_t PID_PROCESS_PERIOD = 10; + + /** + * The line sensor threshold (normalized) used to detect the track. + * The track is detected in case the received value is greater or equal than + * the threshold. + */ + static const uint16_t LINE_SENSOR_ON_TRACK_MIN_VALUE = 200U; + + /** + * The max. normalized value of a sensor in digits. + */ + static const uint16_t SENSOR_VALUE_MAX; + + /** + * Position set point which is the perfect on track position. + * This is the goal to achieve. + */ + static const int16_t POSITION_SET_POINT; + + /** + * ID of most left sensor. + */ + static const uint8_t SENSOR_ID_MOST_LEFT; + + /** + * ID of most right sensor. + */ + static const uint8_t SENSOR_ID_MIDDLE; + + /** + * ID of middle sensor. + */ + static const uint8_t SENSOR_ID_MOST_RIGHT; + + /** + * Minimum position in digits. + */ + static const int16_t POSITION_MIN; + + /** + * Maximum position in digits. + */ + static const int16_t POSITION_MAX; + + /** + * Lower border position in digits for driving will on the line. + */ + static const int16_t POSITION_MIDDLE_MIN; + + /** + * Higher border position in digits for driving will on the line. + */ + static const int16_t POSITION_MIDDLE_MAX; + + SimpleTimer m_observationTimer; /**< Observation timer to observe the max. time per challenge. */ + SimpleTimer m_lapTime; /**< Timer used to calculate the lap time. */ + SimpleTimer m_pidProcessTime; /**< Timer used for periodically PID processing. */ + PIDController m_pidCtrl; /**< PID controller, used for driving. */ + int16_t m_topSpeed; /**< Top speed in [steps/s]. It might be lower or equal to the max. speed! */ + LineStatus m_lineStatus; /**< Status of start-/end line detection */ + TrackStatus m_trackStatus; /**< Status of track which means on track or track lost, etc. */ + bool m_isStartStopLineDetected; /**< Is the start/stop line detected? */ + uint8_t m_lastSensorIdSawTrack; /**< The sensor id of the sensor which saw the track as last. */ + int16_t m_lastPosition; /**< Last position, used to decide strategy in case of a track gap. */ + bool m_isTrackLost; /**< Is the track lost? Lost means the line sensors didn't detect it. */ + + /** + * Default constructor. + */ + DrivingState(); + + /** + * Default destructor. + */ + ~DrivingState() + { + } + + /* Not allowed. */ + DrivingState(const DrivingState& state); /**< Copy construction of an instance. */ + DrivingState& operator=(const DrivingState& state); /**< Assignment of an instance. */ + + /** + * Calculate the position with the inner 3 line sensors. + * + * @param[out] position The position result. + * @param[in] lineSensorValues Array of line sensor values. + * @param[in] length Array length. + * + * @return If successful, it will return true otherwise false. + */ + bool calcPosition3(int16_t& position, const uint16_t* lineSensorValues, uint8_t length) const; + + /** + * Evaluate the situation by line sensor values and position and determine + * the track status. The result influences the measures to keep track on + * the line. + * + * @param[in] lineSensorValues The line sensor values as array. + * @param[in] length The number of line sensor values. + * @param[in] position The position calculated with all sensors. + * @param[in] position3 The position calculated with the inner 3 sensors only. + * @param[in] isTrackLost Information whether the track is lost or not. + * + * @return The track status result. + */ + TrackStatus evaluateSituation(const uint16_t* lineSensorValues, uint8_t length, int16_t position, int16_t position3, bool isTrackLost) const; + + /** + * Is the start/stop line detected? + * + * @param[in] lineSensorValues The line sensor values as array. + * @param[in] length The number of line sensor values. + * + * @return If start/stop line detected, it will return true otherwise false. + */ + bool isStartStopLineDetected(const uint16_t* lineSensorValues, uint8_t length) const; + + /** + * Is no line detected? + * + * @param[in] lineSensorValues The line sensor values as array. + * @param[in] length The number of line sensor values. + * + * @return If no line is detected, it will return true otherwise false. + */ + bool isNoLineDetected(const uint16_t* lineSensorValues, uint8_t length) const; + + /** + * Is right angle curve to the left detected? + * + * @param[in] lineSensorValues The line sensor values as array. + * @param[in] length The number of line sensor values. + * + * @return If right angle curve to the left is detected, it will return true otherwise false. + */ + bool isRightAngleCurveToLeft(const uint16_t* lineSensorValues, uint8_t length) const; + + /** + * Is right angle curve to the right detected? + * + * @param[in] lineSensorValues The line sensor values as array. + * @param[in] length The number of line sensor values. + * + * @return If right angle curve to the right is detected, it will return true otherwise false. + */ + bool isRightAngleCurveToRight(const uint16_t* lineSensorValues, uint8_t length) const; + + /** + * Is a sharp left curve detected? + * + * @param[in] lineSensorValues The line sensor values as array. + * @param[in] length The number of line sensor values. + * @param[in] position3 The position calculated with the inner line sensors in digits. + * + * @return If sharp left curve is detected, it will return true otherwise false. + */ + bool isSharpLeftCurveDetected(const uint16_t* lineSensorValues, uint8_t length, int16_t position3) const; + + /** + * Is a sharp right curve detected? + * + * @param[in] lineSensorValues The line sensor values as array. + * @param[in] length The number of line sensor values. + * @param[in] position3 The position calculated with the inner line sensors in digits. + * + * @return If sharp right curve is detected, it will return true otherwise false. + */ + bool isSharpRightCurveDetected(const uint16_t* lineSensorValues, uint8_t length, int16_t position3) const; + + /** + * Process the situation and decide which measures to take. + * Measures will influence the position or whether its allowed to have + * a negative motor speed. + * + * @param[in, out] position The position calculated with all sensors in digits. + * @param[out] allowNegativeMotorSpeed Allow negative motor speed or not. + * @param[in] trackStatus The evaluated track status. + * @param[in] position3 The position calculated with the inner sensors in digits. + */ + void processSituation(int16_t& position, bool& allowNegativeMotorSpeed, TrackStatus trackStatus, int16_t position3); + + /** + * Adapt driving by using a PID algorithm, depended on the position + * input. + * + * @param[in] position Position in digits + * @param[in] allowNegativeMotorSpeed Allow negative motor speed. + */ + void adaptDriving(int16_t position, bool allowNegativeMotorSpeed); + + /** + * Check the abort conditions while driving the challenge. + * + * @return If abort is required, it will return true otherwise false. + */ + bool isAbortRequired(); + + +#ifdef DEBUG_ALGORITHM + /** + * Required for debugging purposes. + * + * @param[in] trackStatus The track status. + */ + friend void logCsvDataTrackStatus(TrackStatus trackStatus); +#endif /* DEBUG_ALGORITHM */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* DRIVING_STATE_H */ +/** @} */ diff --git a/lib/APPLineFollower/ErrorState.cpp b/lib/APPLineFollower/ErrorState.cpp index b5ca6714..12bef91a 100644 --- a/lib/APPLineFollower/ErrorState.cpp +++ b/lib/APPLineFollower/ErrorState.cpp @@ -1,137 +1,137 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Error state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ErrorState.h" -#include -#include -#include "StartupState.h" -#include -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/** - * Error logging tag. - */ -LOG_TAG("EState"); - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void ErrorState::entry() -{ - IDisplay& display = Board::getInstance().getDisplay(); - - DifferentialDrive::getInstance().disable(); - - display.clear(); - display.print("A: CONT"); - display.gotoXY(0, 1); - - if ('\0' == m_errorMsg[0]) - { - display.print("ERR"); - } - else - { - display.print(m_errorMsg); - } - - LOG_ERROR_VAL("Error: ", m_errorMsg); -} - -void ErrorState::process(StateMachine& sm) -{ - IButton& buttonA = Board::getInstance().getButtonA(); - - /* Restart calibration? */ - if (true == buttonA.isPressed()) - { - buttonA.waitForRelease(); - sm.setState(&StartupState::getInstance()); - } -} - -void ErrorState::exit() -{ - /* Nothing to do. */ -} - -void ErrorState::setErrorMsg(const char* msg) -{ - if (nullptr == msg) - { - m_errorMsg[0] = '\0'; - } - else - { - strncpy(m_errorMsg, msg, ERROR_MSG_SIZE - 1); - m_errorMsg[ERROR_MSG_SIZE - 1] = '\0'; - } -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Error state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ErrorState.h" +#include +#include +#include "StartupState.h" +#include +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/** + * Error logging tag. + */ +LOG_TAG("EState"); + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void ErrorState::entry() +{ + IDisplay& display = Board::getInstance().getDisplay(); + + DifferentialDrive::getInstance().disable(); + + display.clear(); + display.print("A: CONT"); + display.gotoXY(0, 1); + + if ('\0' == m_errorMsg[0]) + { + display.print("ERR"); + } + else + { + display.print(m_errorMsg); + } + + LOG_ERROR_VAL("Error: ", m_errorMsg); +} + +void ErrorState::process(StateMachine& sm) +{ + IButton& buttonA = Board::getInstance().getButtonA(); + + /* Restart calibration? */ + if (true == buttonA.isPressed()) + { + buttonA.waitForRelease(); + sm.setState(&StartupState::getInstance()); + } +} + +void ErrorState::exit() +{ + /* Nothing to do. */ +} + +void ErrorState::setErrorMsg(const char* msg) +{ + if (nullptr == msg) + { + m_errorMsg[0] = '\0'; + } + else + { + strncpy(m_errorMsg, msg, ERROR_MSG_SIZE - 1); + m_errorMsg[ERROR_MSG_SIZE - 1] = '\0'; + } +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPLineFollower/ErrorState.h b/lib/APPLineFollower/ErrorState.h index 74095203..0679e9a3 100644 --- a/lib/APPLineFollower/ErrorState.h +++ b/lib/APPLineFollower/ErrorState.h @@ -1,134 +1,134 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Error state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef ERROR_STATE_H -#define ERROR_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The system error state. */ -class ErrorState : public IState -{ -public: - /** - * Get state instance. - * - * @return State instance. - */ - static ErrorState& getInstance() - { - static ErrorState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - - /** - * Set error message, which to show on the display. - * - * @param[in] msg Error message - */ - void setErrorMsg(const char* msg); - -protected: -private: - /** - * The error message string size in bytes, which - * includes the terminating character. - */ - static const size_t ERROR_MSG_SIZE = 20; - - char m_errorMsg[ERROR_MSG_SIZE]; /**< Error message, which to show. */ - - /** - * Default constructor. - */ - ErrorState() : m_errorMsg() - { - m_errorMsg[0] = '\0'; - } - - /** - * Default destructor. - */ - ~ErrorState() - { - } - - /* Not allowed. */ - ErrorState(const ErrorState& state); /**< Copy construction of an instance. */ - ErrorState& operator=(const ErrorState& state); /**< Assignment of an instance. */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* ERROR_STATE_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Error state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef ERROR_STATE_H +#define ERROR_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The system error state. */ +class ErrorState : public IState +{ +public: + /** + * Get state instance. + * + * @return State instance. + */ + static ErrorState& getInstance() + { + static ErrorState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + + /** + * Set error message, which to show on the display. + * + * @param[in] msg Error message + */ + void setErrorMsg(const char* msg); + +protected: +private: + /** + * The error message string size in bytes, which + * includes the terminating character. + */ + static const size_t ERROR_MSG_SIZE = 20; + + char m_errorMsg[ERROR_MSG_SIZE]; /**< Error message, which to show. */ + + /** + * Default constructor. + */ + ErrorState() : m_errorMsg() + { + m_errorMsg[0] = '\0'; + } + + /** + * Default destructor. + */ + ~ErrorState() + { + } + + /* Not allowed. */ + ErrorState(const ErrorState& state); /**< Copy construction of an instance. */ + ErrorState& operator=(const ErrorState& state); /**< Assignment of an instance. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* ERROR_STATE_H */ +/** @} */ diff --git a/lib/APPLineFollower/LineSensorsCalibrationState.cpp b/lib/APPLineFollower/LineSensorsCalibrationState.cpp index 1f4c87fb..4a498a79 100644 --- a/lib/APPLineFollower/LineSensorsCalibrationState.cpp +++ b/lib/APPLineFollower/LineSensorsCalibrationState.cpp @@ -1,213 +1,213 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Line sensors calibration state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "LineSensorsCalibrationState.h" -#include -#include -#include -#include -#include -#include "ReadyState.h" -#include "ErrorState.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void LineSensorsCalibrationState::entry() -{ - IDisplay& display = Board::getInstance().getDisplay(); - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - Odometry& odometry = Odometry::getInstance(); - ILineSensors& lineSensors = Board::getInstance().getLineSensors(); - - display.clear(); - display.print("Run"); - display.gotoXY(0, 1); - display.print("LCAL"); - - /* Prepare calibration drive. */ - m_calibrationSpeed = diffDrive.getMaxMotorSpeed() / 4; - m_orientation = odometry.getOrientation(); - - /* Mandatory for each new calibration. */ - lineSensors.resetCalibration(); - - /* Wait some time, before starting the calibration drive. */ - m_phase = PHASE_1_WAIT; - m_timer.start(WAIT_TIME); -} - -void LineSensorsCalibrationState::process(StateMachine& sm) -{ - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - - switch (m_phase) - { - case PHASE_1_WAIT: - if (true == m_timer.isTimeout()) - { - m_phase = PHASE_2_TURN_LEFT; - diffDrive.setLinearSpeed(-m_calibrationSpeed, m_calibrationSpeed); - } - break; - - case PHASE_2_TURN_LEFT: - if (true == turnAndCalibrate(CALIB_ANGLE, true)) - { - m_phase = PHASE_3_TURN_RIGHT; - diffDrive.setLinearSpeed(m_calibrationSpeed, -m_calibrationSpeed); - } - break; - - case PHASE_3_TURN_RIGHT: - if (true == turnAndCalibrate(-CALIB_ANGLE, false)) - { - m_phase = PHASE_4_TURN_ORIG; - diffDrive.setLinearSpeed(-m_calibrationSpeed, m_calibrationSpeed); - } - break; - - case PHASE_4_TURN_ORIG: - if (true == turnAndCalibrate(0, true)) - { - m_phase = PHASE_5_FINISHED; - diffDrive.setLinearSpeed(0, 0); - finishCalibration(sm); - } - break; - - case PHASE_5_FINISHED: - /* fallthrough */ - default: - break; - } -} - -void LineSensorsCalibrationState::exit() -{ - m_timer.stop(); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -bool LineSensorsCalibrationState::turnAndCalibrate(int32_t calibAlpha, bool isGreaterEqual) -{ - ILineSensors& lineSensors = Board::getInstance().getLineSensors(); - Odometry& odometry = Odometry::getInstance(); - int32_t alpha = odometry.getOrientation() - m_orientation; /* [mrad] */ - bool isSuccesful = false; - - /* Continously calibrate the line sensors. */ - lineSensors.calibrate(); - - /* Is the goal that the current angle shall be lower or equal than the destination calibration angle? */ - if (false == isGreaterEqual) - { - /* Is alpha lower or equal than the destination calibration angle? */ - if (calibAlpha >= alpha) - { - isSuccesful = true; - } - } - else - { - /* Is alpha greater or equal than the destination calibration angle? */ - if (calibAlpha <= alpha) - { - isSuccesful = true; - } - } - - return isSuccesful; -} - -void LineSensorsCalibrationState::finishCalibration(StateMachine& sm) -{ - ILineSensors& lineSensors = Board::getInstance().getLineSensors(); - - if (false == lineSensors.isCalibrationSuccessful()) - { - char str[10]; - char valueStr[10]; - - Util::uintToStr(valueStr, sizeof(valueStr), lineSensors.getCalibErrorInfo()); - - strncpy(str, "ELCAL ", sizeof(str) - 1); - str[sizeof(str) - 1] = '\0'; - - strncat(str, valueStr, sizeof(str) - strlen(valueStr) - 1); - - ErrorState::getInstance().setErrorMsg(str); - sm.setState(&ErrorState::getInstance()); - } - else - { - sm.setState(&ReadyState::getInstance()); - } -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Line sensors calibration state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "LineSensorsCalibrationState.h" +#include +#include +#include +#include +#include +#include "ReadyState.h" +#include "ErrorState.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void LineSensorsCalibrationState::entry() +{ + IDisplay& display = Board::getInstance().getDisplay(); + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + Odometry& odometry = Odometry::getInstance(); + ILineSensors& lineSensors = Board::getInstance().getLineSensors(); + + display.clear(); + display.print("Run"); + display.gotoXY(0, 1); + display.print("LCAL"); + + /* Prepare calibration drive. */ + m_calibrationSpeed = diffDrive.getMaxMotorSpeed() / 4; + m_orientation = odometry.getOrientation(); + + /* Mandatory for each new calibration. */ + lineSensors.resetCalibration(); + + /* Wait some time, before starting the calibration drive. */ + m_phase = PHASE_1_WAIT; + m_timer.start(WAIT_TIME); +} + +void LineSensorsCalibrationState::process(StateMachine& sm) +{ + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + + switch (m_phase) + { + case PHASE_1_WAIT: + if (true == m_timer.isTimeout()) + { + m_phase = PHASE_2_TURN_LEFT; + diffDrive.setLinearSpeed(-m_calibrationSpeed, m_calibrationSpeed); + } + break; + + case PHASE_2_TURN_LEFT: + if (true == turnAndCalibrate(CALIB_ANGLE, true)) + { + m_phase = PHASE_3_TURN_RIGHT; + diffDrive.setLinearSpeed(m_calibrationSpeed, -m_calibrationSpeed); + } + break; + + case PHASE_3_TURN_RIGHT: + if (true == turnAndCalibrate(-CALIB_ANGLE, false)) + { + m_phase = PHASE_4_TURN_ORIG; + diffDrive.setLinearSpeed(-m_calibrationSpeed, m_calibrationSpeed); + } + break; + + case PHASE_4_TURN_ORIG: + if (true == turnAndCalibrate(0, true)) + { + m_phase = PHASE_5_FINISHED; + diffDrive.setLinearSpeed(0, 0); + finishCalibration(sm); + } + break; + + case PHASE_5_FINISHED: + /* fallthrough */ + default: + break; + } +} + +void LineSensorsCalibrationState::exit() +{ + m_timer.stop(); +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +bool LineSensorsCalibrationState::turnAndCalibrate(int32_t calibAlpha, bool isGreaterEqual) +{ + ILineSensors& lineSensors = Board::getInstance().getLineSensors(); + Odometry& odometry = Odometry::getInstance(); + int32_t alpha = odometry.getOrientation() - m_orientation; /* [mrad] */ + bool isSuccesful = false; + + /* Continously calibrate the line sensors. */ + lineSensors.calibrate(); + + /* Is the goal that the current angle shall be lower or equal than the destination calibration angle? */ + if (false == isGreaterEqual) + { + /* Is alpha lower or equal than the destination calibration angle? */ + if (calibAlpha >= alpha) + { + isSuccesful = true; + } + } + else + { + /* Is alpha greater or equal than the destination calibration angle? */ + if (calibAlpha <= alpha) + { + isSuccesful = true; + } + } + + return isSuccesful; +} + +void LineSensorsCalibrationState::finishCalibration(StateMachine& sm) +{ + ILineSensors& lineSensors = Board::getInstance().getLineSensors(); + + if (false == lineSensors.isCalibrationSuccessful()) + { + char str[10]; + char valueStr[10]; + + Util::uintToStr(valueStr, sizeof(valueStr), lineSensors.getCalibErrorInfo()); + + strncpy(str, "ELCAL ", sizeof(str) - 1); + str[sizeof(str) - 1] = '\0'; + + strncat(str, valueStr, sizeof(str) - strlen(valueStr) - 1); + + ErrorState::getInstance().setErrorMsg(str); + sm.setState(&ErrorState::getInstance()); + } + else + { + sm.setState(&ReadyState::getInstance()); + } +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPLineFollower/LineSensorsCalibrationState.h b/lib/APPLineFollower/LineSensorsCalibrationState.h index 564e7508..e9336431 100644 --- a/lib/APPLineFollower/LineSensorsCalibrationState.h +++ b/lib/APPLineFollower/LineSensorsCalibrationState.h @@ -1,161 +1,161 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Line sensors calibration state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef LINE_SENSORS_CALIBRATION_STATE_H -#define LINE_SENSORS_CALIBRATION_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The line sensors calibration state. */ -class LineSensorsCalibrationState : public IState -{ -public: - /** - * Get state instance. - * - * @return State instance. - */ - static LineSensorsCalibrationState& getInstance() - { - static LineSensorsCalibrationState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - -protected: -private: - /** Calibration phases */ - enum Phase - { - PHASE_1_WAIT = 0, /**< Wait a short time before starting the calibration. */ - PHASE_2_TURN_LEFT, /**< Turn line sensors left over the line. */ - PHASE_3_TURN_RIGHT, /**< Turn line sensor right over the line. */ - PHASE_4_TURN_ORIG, /**< Turn back to origin. */ - PHASE_5_FINISHED /**< Calibration is finished. */ - }; - - /** - * Duration in ms about to wait, until the calibration drive starts. - */ - static const uint32_t WAIT_TIME = 1000; - - /** - * Calibration turn angle in mrad (corresponds to 72°). - */ - static const int32_t CALIB_ANGLE = (FP_2PI() / 5); - - SimpleTimer m_timer; /**< Timer used to wait, until the calibration drive starts. */ - Phase m_phase; /**< Current calibration phase */ - int16_t m_calibrationSpeed; /**< Speed in steps/s for the calibration drive. */ - int32_t m_orientation; /**< Orientation at the begin of the calibration in [mrad]. */ - - /** - * Default constructor. - */ - LineSensorsCalibrationState() : m_timer(), m_phase(PHASE_1_WAIT), m_calibrationSpeed(0), m_orientation(0) - { - } - - /** - * Default destructor. - */ - ~LineSensorsCalibrationState() - { - } - - /* Not allowed. */ - LineSensorsCalibrationState(const LineSensorsCalibrationState& state); /**< Copy construction of an instance. */ - LineSensorsCalibrationState& operator=(const LineSensorsCalibrationState& state); /**< Assignment of an instance. */ - - /** - * Turn and calibrate the line sensors. - * - * @param[in] calibAlpha Destination calibration angle in [mrad] - * @param[in] isGreaterEqual Configure true if angle shall be greater or equal than the destination. - * - * @return If destination angle reached, it will return true otherwise false. - */ - bool turnAndCalibrate(int32_t calibAlpha, bool isGreaterEqual); - - /** - * Finish the calibration and determine next state. - * - * @param[in] sm State machine - */ - void finishCalibration(StateMachine& sm); -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* LINE_SENSORS_CALIBRATION_STATE_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Line sensors calibration state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef LINE_SENSORS_CALIBRATION_STATE_H +#define LINE_SENSORS_CALIBRATION_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The line sensors calibration state. */ +class LineSensorsCalibrationState : public IState +{ +public: + /** + * Get state instance. + * + * @return State instance. + */ + static LineSensorsCalibrationState& getInstance() + { + static LineSensorsCalibrationState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + +protected: +private: + /** Calibration phases */ + enum Phase + { + PHASE_1_WAIT = 0, /**< Wait a short time before starting the calibration. */ + PHASE_2_TURN_LEFT, /**< Turn line sensors left over the line. */ + PHASE_3_TURN_RIGHT, /**< Turn line sensor right over the line. */ + PHASE_4_TURN_ORIG, /**< Turn back to origin. */ + PHASE_5_FINISHED /**< Calibration is finished. */ + }; + + /** + * Duration in ms about to wait, until the calibration drive starts. + */ + static const uint32_t WAIT_TIME = 1000; + + /** + * Calibration turn angle in mrad (corresponds to 72°). + */ + static const int32_t CALIB_ANGLE = (FP_2PI() / 5); + + SimpleTimer m_timer; /**< Timer used to wait, until the calibration drive starts. */ + Phase m_phase; /**< Current calibration phase */ + int16_t m_calibrationSpeed; /**< Speed in steps/s for the calibration drive. */ + int32_t m_orientation; /**< Orientation at the begin of the calibration in [mrad]. */ + + /** + * Default constructor. + */ + LineSensorsCalibrationState() : m_timer(), m_phase(PHASE_1_WAIT), m_calibrationSpeed(0), m_orientation(0) + { + } + + /** + * Default destructor. + */ + ~LineSensorsCalibrationState() + { + } + + /* Not allowed. */ + LineSensorsCalibrationState(const LineSensorsCalibrationState& state); /**< Copy construction of an instance. */ + LineSensorsCalibrationState& operator=(const LineSensorsCalibrationState& state); /**< Assignment of an instance. */ + + /** + * Turn and calibrate the line sensors. + * + * @param[in] calibAlpha Destination calibration angle in [mrad] + * @param[in] isGreaterEqual Configure true if angle shall be greater or equal than the destination. + * + * @return If destination angle reached, it will return true otherwise false. + */ + bool turnAndCalibrate(int32_t calibAlpha, bool isGreaterEqual); + + /** + * Finish the calibration and determine next state. + * + * @param[in] sm State machine + */ + void finishCalibration(StateMachine& sm); +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* LINE_SENSORS_CALIBRATION_STATE_H */ +/** @} */ diff --git a/lib/APPLineFollower/MotorSpeedCalibrationState.cpp b/lib/APPLineFollower/MotorSpeedCalibrationState.cpp index 5dec5290..f9547662 100644 --- a/lib/APPLineFollower/MotorSpeedCalibrationState.cpp +++ b/lib/APPLineFollower/MotorSpeedCalibrationState.cpp @@ -1,245 +1,245 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Motor speed calibration state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "MotorSpeedCalibrationState.h" -#include -#include -#include -#include -#include -#include -#include "LineSensorsCalibrationState.h" -#include "ErrorState.h" -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/** - * Logging source. - */ -LOG_TAG("MSCState"); - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void MotorSpeedCalibrationState::entry() -{ - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - IDisplay& display = Board::getInstance().getDisplay(); - - display.clear(); - display.print("Run"); - display.gotoXY(0, 1); - display.print("MCAL"); - - /* Disable differential drive to avoid any bad influence. */ - diffDrive.disable(); - - /* Setup relative encoders */ - m_relEncoders.clear(); - - /* Set the max. speeds to maximum of datatype, because during calibration - * the max. speed is determined by the lowest motor speed. - */ - m_maxSpeedLeft = INT16_MAX; - m_maxSpeedRight = INT16_MAX; - - /* Wait some time, before starting the calibration drive. */ - m_phase = PHASE_1_BACK; - m_timer.start(WAIT_TIME); -} - -void MotorSpeedCalibrationState::process(StateMachine& sm) -{ - if (true == m_timer.isTimeout()) - { - /* Control motors directly and not via differential drive control, - * because the differential drive control needs first to be updated - * regarding the max. possible motor speed in [steps/s] which is - * determined by this calibration. - */ - IMotors& motors = Board::getInstance().getMotors(); - - switch (m_phase) - { - case PHASE_1_BACK: - /* Drive full back. */ - motors.setSpeeds(-motors.getMaxSpeed(), -motors.getMaxSpeed()); - - m_timer.start(CALIB_DURATION); - m_phase = PHASE_2_FORWARD; - break; - - case PHASE_2_FORWARD: - motors.setSpeeds(0, 0); - determineMaxMotorSpeed(); - - /* Drive full forward. */ - motors.setSpeeds(motors.getMaxSpeed(), motors.getMaxSpeed()); - - m_timer.restart(); - m_phase = PHASE_3_FINISHED; - break; - - case PHASE_3_FINISHED: - motors.setSpeeds(0, 0); - determineMaxMotorSpeed(); - - m_timer.stop(); - finishCalibration(sm); - break; - - default: - break; - } - } -} - -void MotorSpeedCalibrationState::exit() -{ - /* Nothing to do. */ -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -void MotorSpeedCalibrationState::determineMaxMotorSpeed() -{ - int32_t stepsLeft = 0; - int32_t stepsRight = 0; - - /* Determine max. speed backward. */ - stepsLeft = abs(m_relEncoders.getCountsLeft()); - stepsRight = abs(m_relEncoders.getCountsRight()); - - /* Convert number of steps to [steps/s] */ - stepsLeft *= 1000; - stepsLeft /= CALIB_DURATION; - stepsRight *= 1000; - stepsRight /= CALIB_DURATION; - - if (INT16_MAX >= stepsLeft) - { - /* Use lower speed to ensure that motor speed can be reached in both - * directions. - */ - if (stepsLeft < m_maxSpeedLeft) - { - m_maxSpeedLeft = static_cast(stepsLeft); - } - } - - if (INT16_MAX >= stepsRight) - { - /* Use lower speed to ensure that motor speed can be reached in both - * directions. - */ - if (stepsRight < m_maxSpeedRight) - { - m_maxSpeedRight = static_cast(stepsRight); - } - } - - /* Clear relative encoders */ - m_relEncoders.clear(); -} - -void MotorSpeedCalibrationState::finishCalibration(StateMachine& sm) -{ - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - ISettings& settings = Board::getInstance().getSettings(); - - /* Set the lower speed as max. motor speed to ensure that both motors - * can reach the same max. speed. - */ - int16_t maxSpeed = (m_maxSpeedLeft < m_maxSpeedRight) ? m_maxSpeedLeft : m_maxSpeedRight; - - /* Store calibrated max. motor speed in the settings. */ - settings.setMaxSpeed(maxSpeed); - - /* With setting the max. motor speed in [steps/s] the differential drive control - * can now be used. - */ - diffDrive.setMaxMotorSpeed(maxSpeed); - - /* Differential drive can now be used. */ - diffDrive.enable(); - - if (0 == maxSpeed) - { - ErrorState::getInstance().setErrorMsg("EMCAL 0"); - sm.setState(&ErrorState::getInstance()); - } - else - { - int32_t maxSpeed32 = static_cast(maxSpeed) * 1000 / static_cast(RobotConstants::ENCODER_STEPS_PER_M); - - LOG_INFO_VAL("Calibrated max. speed (steps/s): ", maxSpeed); - LOG_INFO_VAL("Calibrated max. speed (mm/s): ", maxSpeed32); - - sm.setState(&LineSensorsCalibrationState::getInstance()); - } -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Motor speed calibration state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "MotorSpeedCalibrationState.h" +#include +#include +#include +#include +#include +#include +#include "LineSensorsCalibrationState.h" +#include "ErrorState.h" +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/** + * Logging source. + */ +LOG_TAG("MSCState"); + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void MotorSpeedCalibrationState::entry() +{ + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + IDisplay& display = Board::getInstance().getDisplay(); + + display.clear(); + display.print("Run"); + display.gotoXY(0, 1); + display.print("MCAL"); + + /* Disable differential drive to avoid any bad influence. */ + diffDrive.disable(); + + /* Setup relative encoders */ + m_relEncoders.clear(); + + /* Set the max. speeds to maximum of datatype, because during calibration + * the max. speed is determined by the lowest motor speed. + */ + m_maxSpeedLeft = INT16_MAX; + m_maxSpeedRight = INT16_MAX; + + /* Wait some time, before starting the calibration drive. */ + m_phase = PHASE_1_BACK; + m_timer.start(WAIT_TIME); +} + +void MotorSpeedCalibrationState::process(StateMachine& sm) +{ + if (true == m_timer.isTimeout()) + { + /* Control motors directly and not via differential drive control, + * because the differential drive control needs first to be updated + * regarding the max. possible motor speed in [steps/s] which is + * determined by this calibration. + */ + IMotors& motors = Board::getInstance().getMotors(); + + switch (m_phase) + { + case PHASE_1_BACK: + /* Drive full back. */ + motors.setSpeeds(-motors.getMaxSpeed(), -motors.getMaxSpeed()); + + m_timer.start(CALIB_DURATION); + m_phase = PHASE_2_FORWARD; + break; + + case PHASE_2_FORWARD: + motors.setSpeeds(0, 0); + determineMaxMotorSpeed(); + + /* Drive full forward. */ + motors.setSpeeds(motors.getMaxSpeed(), motors.getMaxSpeed()); + + m_timer.restart(); + m_phase = PHASE_3_FINISHED; + break; + + case PHASE_3_FINISHED: + motors.setSpeeds(0, 0); + determineMaxMotorSpeed(); + + m_timer.stop(); + finishCalibration(sm); + break; + + default: + break; + } + } +} + +void MotorSpeedCalibrationState::exit() +{ + /* Nothing to do. */ +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +void MotorSpeedCalibrationState::determineMaxMotorSpeed() +{ + int32_t stepsLeft = 0; + int32_t stepsRight = 0; + + /* Determine max. speed backward. */ + stepsLeft = abs(m_relEncoders.getCountsLeft()); + stepsRight = abs(m_relEncoders.getCountsRight()); + + /* Convert number of steps to [steps/s] */ + stepsLeft *= 1000; + stepsLeft /= CALIB_DURATION; + stepsRight *= 1000; + stepsRight /= CALIB_DURATION; + + if (INT16_MAX >= stepsLeft) + { + /* Use lower speed to ensure that motor speed can be reached in both + * directions. + */ + if (stepsLeft < m_maxSpeedLeft) + { + m_maxSpeedLeft = static_cast(stepsLeft); + } + } + + if (INT16_MAX >= stepsRight) + { + /* Use lower speed to ensure that motor speed can be reached in both + * directions. + */ + if (stepsRight < m_maxSpeedRight) + { + m_maxSpeedRight = static_cast(stepsRight); + } + } + + /* Clear relative encoders */ + m_relEncoders.clear(); +} + +void MotorSpeedCalibrationState::finishCalibration(StateMachine& sm) +{ + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + ISettings& settings = Board::getInstance().getSettings(); + + /* Set the lower speed as max. motor speed to ensure that both motors + * can reach the same max. speed. + */ + int16_t maxSpeed = (m_maxSpeedLeft < m_maxSpeedRight) ? m_maxSpeedLeft : m_maxSpeedRight; + + /* Store calibrated max. motor speed in the settings. */ + settings.setMaxSpeed(maxSpeed); + + /* With setting the max. motor speed in [steps/s] the differential drive control + * can now be used. + */ + diffDrive.setMaxMotorSpeed(maxSpeed); + + /* Differential drive can now be used. */ + diffDrive.enable(); + + if (0 == maxSpeed) + { + ErrorState::getInstance().setErrorMsg("EMCAL 0"); + sm.setState(&ErrorState::getInstance()); + } + else + { + int32_t maxSpeed32 = static_cast(maxSpeed) * 1000 / static_cast(RobotConstants::ENCODER_STEPS_PER_M); + + LOG_INFO_VAL("Calibrated max. speed (steps/s): ", maxSpeed); + LOG_INFO_VAL("Calibrated max. speed (mm/s): ", maxSpeed32); + + sm.setState(&LineSensorsCalibrationState::getInstance()); + } +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPLineFollower/MotorSpeedCalibrationState.h b/lib/APPLineFollower/MotorSpeedCalibrationState.h index 097cf12e..f4ea0bcc 100644 --- a/lib/APPLineFollower/MotorSpeedCalibrationState.h +++ b/lib/APPLineFollower/MotorSpeedCalibrationState.h @@ -1,165 +1,165 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Motor speed calibration state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef MOTOR_SPEED_CALIBRATION_STATE_H -#define MOTOR_SPEED_CALIBRATION_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The motor speed calibration state. */ -class MotorSpeedCalibrationState : public IState -{ -public: - /** - * Get state instance. - * - * @return State instance. - */ - static MotorSpeedCalibrationState& getInstance() - { - static MotorSpeedCalibrationState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - -protected: -private: - /** Calibration phases */ - enum Phase - { - PHASE_1_BACK, /**< Drive with max. speed backwards. */ - PHASE_2_FORWARD, /**< Drive with max. speed forwards. */ - PHASE_3_FINISHED /**< Calibration is finished. */ - }; - - /** - * Duration in ms about to wait, until the calibration drive starts. - */ - static const uint32_t WAIT_TIME = 1000; - - /** - * Calibration drive duration in ms. - * It means how long the robot is driven with max. speed forward/backward. - */ - static const uint32_t CALIB_DURATION = 1000; - - SimpleTimer m_timer; /**< Timer used to wait, until the calibration drive starts and for drive duration. */ - Phase m_phase; /**< Current calibration phase */ - int16_t m_maxSpeedLeft; /**< Max. determined left motor speed [steps/s]. */ - int16_t m_maxSpeedRight; /**< Max. determined right motor speed [steps/s]. */ - RelativeEncoders m_relEncoders; /**< Relative encoders left/right. */ - - /** - * Default constructor. - */ - MotorSpeedCalibrationState() : - m_timer(), - m_phase(PHASE_1_BACK), - m_maxSpeedLeft(0), - m_maxSpeedRight(0), - m_relEncoders(Board::getInstance().getEncoders()) - { - } - - /** - * Default destructor. - */ - ~MotorSpeedCalibrationState() - { - } - - /* Not allowed. */ - MotorSpeedCalibrationState(const MotorSpeedCalibrationState& state); /**< Copy construction of an instance. */ - MotorSpeedCalibrationState& operator=(const MotorSpeedCalibrationState& state); /**< Assignment of an instance. */ - - /** - * Determine the max. motor speed, considering both driving directions. - * There are two steps necessary: - * - Drive full backward and call this method to determine. - * - Drive full forward and call this method to determine. - */ - void determineMaxMotorSpeed(); - - /** - * Finish the calibration and determine next state. - * - * @param[in] sm State machine - */ - void finishCalibration(StateMachine& sm); -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* MOTOR_SPEED_CALIBRATION_STATE_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Motor speed calibration state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef MOTOR_SPEED_CALIBRATION_STATE_H +#define MOTOR_SPEED_CALIBRATION_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The motor speed calibration state. */ +class MotorSpeedCalibrationState : public IState +{ +public: + /** + * Get state instance. + * + * @return State instance. + */ + static MotorSpeedCalibrationState& getInstance() + { + static MotorSpeedCalibrationState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + +protected: +private: + /** Calibration phases */ + enum Phase + { + PHASE_1_BACK, /**< Drive with max. speed backwards. */ + PHASE_2_FORWARD, /**< Drive with max. speed forwards. */ + PHASE_3_FINISHED /**< Calibration is finished. */ + }; + + /** + * Duration in ms about to wait, until the calibration drive starts. + */ + static const uint32_t WAIT_TIME = 1000; + + /** + * Calibration drive duration in ms. + * It means how long the robot is driven with max. speed forward/backward. + */ + static const uint32_t CALIB_DURATION = 1000; + + SimpleTimer m_timer; /**< Timer used to wait, until the calibration drive starts and for drive duration. */ + Phase m_phase; /**< Current calibration phase */ + int16_t m_maxSpeedLeft; /**< Max. determined left motor speed [steps/s]. */ + int16_t m_maxSpeedRight; /**< Max. determined right motor speed [steps/s]. */ + RelativeEncoders m_relEncoders; /**< Relative encoders left/right. */ + + /** + * Default constructor. + */ + MotorSpeedCalibrationState() : + m_timer(), + m_phase(PHASE_1_BACK), + m_maxSpeedLeft(0), + m_maxSpeedRight(0), + m_relEncoders(Board::getInstance().getEncoders()) + { + } + + /** + * Default destructor. + */ + ~MotorSpeedCalibrationState() + { + } + + /* Not allowed. */ + MotorSpeedCalibrationState(const MotorSpeedCalibrationState& state); /**< Copy construction of an instance. */ + MotorSpeedCalibrationState& operator=(const MotorSpeedCalibrationState& state); /**< Assignment of an instance. */ + + /** + * Determine the max. motor speed, considering both driving directions. + * There are two steps necessary: + * - Drive full backward and call this method to determine. + * - Drive full forward and call this method to determine. + */ + void determineMaxMotorSpeed(); + + /** + * Finish the calibration and determine next state. + * + * @param[in] sm State machine + */ + void finishCalibration(StateMachine& sm); +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* MOTOR_SPEED_CALIBRATION_STATE_H */ +/** @} */ diff --git a/lib/APPLineFollower/ParameterSets.cpp b/lib/APPLineFollower/ParameterSets.cpp index 007811f5..050d8501 100644 --- a/lib/APPLineFollower/ParameterSets.cpp +++ b/lib/APPLineFollower/ParameterSets.cpp @@ -1,150 +1,150 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Calibration state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ParameterSets.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void ParameterSets::choose(uint8_t setId) -{ - if (MAX_SETS > setId) - { - m_currentSetId = setId; - } -} - -void ParameterSets::next() -{ - ++m_currentSetId; - m_currentSetId %= MAX_SETS; -} - -uint8_t ParameterSets::getCurrentSetId() const -{ - return m_currentSetId; -} - -const ParameterSets::ParameterSet& ParameterSets::getParameterSet() const -{ - return m_parSets[m_currentSetId]; -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -ParameterSets::ParameterSets() : m_currentSetId(0), m_parSets() -{ - m_parSets[0] = { - "PD VF", /* Name - VF: very fast */ - 4000, /* Top speed in steps/s */ - 4, /* Kp Numerator */ - 1, /* Kp Denominator */ - 0, /* Ki Numerator */ - 1, /* Ki Denominator */ - 60, /* Kd Numerator */ - 1 /* Kd Denominator */ - }; - - m_parSets[1] = { - "PD F", /* Name - F: fast */ - 3000, /* Top speed in steps/s */ - 2, /* Kp Numerator */ - 1, /* Kp Denominator */ - 0, /* Ki Numerator */ - 1, /* Ki Denominator */ - 50, /* Kd Numerator */ - 1 /* Kd Denominator */ - }; - - m_parSets[2] = { - "PD S", /* Name - S: slow */ - 2000, /* Top speed in steps/s */ - 2, /* Kp Numerator */ - 1, /* Kp Denominator */ - 0, /* Ki Numerator */ - 1, /* Ki Denominator */ - 40, /* Kd Numerator */ - 1 /* Kd Denominator */ - }; - - m_parSets[3] = { - "PD VS", /* Name - VS: very slow */ - 1000, /* Top speed in steps/s */ - 3, /* Kp Numerator */ - 2, /* Kp Denominator */ - 0, /* Ki Numerator */ - 1, /* Ki Denominator */ - 30, /* Kd Numerator */ - 1 /* Kd Denominator */ - }; -} - -ParameterSets::~ParameterSets() -{ -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Calibration state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ParameterSets.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void ParameterSets::choose(uint8_t setId) +{ + if (MAX_SETS > setId) + { + m_currentSetId = setId; + } +} + +void ParameterSets::next() +{ + ++m_currentSetId; + m_currentSetId %= MAX_SETS; +} + +uint8_t ParameterSets::getCurrentSetId() const +{ + return m_currentSetId; +} + +const ParameterSets::ParameterSet& ParameterSets::getParameterSet() const +{ + return m_parSets[m_currentSetId]; +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +ParameterSets::ParameterSets() : m_currentSetId(0), m_parSets() +{ + m_parSets[0] = { + "PD VF", /* Name - VF: very fast */ + 4000, /* Top speed in steps/s */ + 4, /* Kp Numerator */ + 1, /* Kp Denominator */ + 0, /* Ki Numerator */ + 1, /* Ki Denominator */ + 60, /* Kd Numerator */ + 1 /* Kd Denominator */ + }; + + m_parSets[1] = { + "PD F", /* Name - F: fast */ + 3000, /* Top speed in steps/s */ + 2, /* Kp Numerator */ + 1, /* Kp Denominator */ + 0, /* Ki Numerator */ + 1, /* Ki Denominator */ + 50, /* Kd Numerator */ + 1 /* Kd Denominator */ + }; + + m_parSets[2] = { + "PD S", /* Name - S: slow */ + 2000, /* Top speed in steps/s */ + 2, /* Kp Numerator */ + 1, /* Kp Denominator */ + 0, /* Ki Numerator */ + 1, /* Ki Denominator */ + 40, /* Kd Numerator */ + 1 /* Kd Denominator */ + }; + + m_parSets[3] = { + "PD VS", /* Name - VS: very slow */ + 1000, /* Top speed in steps/s */ + 3, /* Kp Numerator */ + 2, /* Kp Denominator */ + 0, /* Ki Numerator */ + 1, /* Ki Denominator */ + 30, /* Kd Numerator */ + 1 /* Kd Denominator */ + }; +} + +ParameterSets::~ParameterSets() +{ +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPLineFollower/ParameterSets.h b/lib/APPLineFollower/ParameterSets.h index 51696265..a046236a 100644 --- a/lib/APPLineFollower/ParameterSets.h +++ b/lib/APPLineFollower/ParameterSets.h @@ -1,145 +1,145 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Parameter state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef PARAMETER_SETS_H -#define PARAMETER_SETS_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** Parameter set with different driving configurations. */ -class ParameterSets -{ -public: - /** - * A single parameter set. - */ - struct ParameterSet - { - const char* name; /**< Name of the parameter set */ - int16_t topSpeed; /**< Top speed in steps/s */ - int16_t kPNumerator; /**< Kp numerator value */ - int16_t kPDenominator; /**< Kp denominator value */ - int16_t kINumerator; /**< Ki numerator value */ - int16_t kIDenominator; /**< Ki denominator value */ - int16_t kDNumerator; /**< Kd numerator value */ - int16_t kDDenominator; /**< Kd denominator value */ - }; - - /** - * Get parameter set instance. - * - * @return Parameter set instance. - */ - static ParameterSets& getInstance() - { - static ParameterSets instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * Choose a specific parameter set. - * If a invalid set id is given, nothing changes. - * - * @param[in] setId Parameter set id - */ - void choose(uint8_t setId); - - /** - * Change to next parameter set. - * After the last set, the first will be choosen. - */ - void next(); - - /** - * Get current set id. - * - * @return Parameter set id. - */ - uint8_t getCurrentSetId() const; - - /** - * Get selected parameter set. - * - * @return Parameter set - */ - const ParameterSet& getParameterSet() const; - - /** Max. number of parameter sets. */ - static const uint8_t MAX_SETS = 4U; - -protected: -private: - uint8_t m_currentSetId; /**< Set id of current selected set. */ - ParameterSet m_parSets[MAX_SETS]; /**< All parameter sets */ - - /** - * Default constructor. - */ - ParameterSets(); - - /** - * Default destructor. - */ - ~ParameterSets(); - - /* 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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Parameter state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef PARAMETER_SETS_H +#define PARAMETER_SETS_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** Parameter set with different driving configurations. */ +class ParameterSets +{ +public: + /** + * A single parameter set. + */ + struct ParameterSet + { + const char* name; /**< Name of the parameter set */ + int16_t topSpeed; /**< Top speed in steps/s */ + int16_t kPNumerator; /**< Kp numerator value */ + int16_t kPDenominator; /**< Kp denominator value */ + int16_t kINumerator; /**< Ki numerator value */ + int16_t kIDenominator; /**< Ki denominator value */ + int16_t kDNumerator; /**< Kd numerator value */ + int16_t kDDenominator; /**< Kd denominator value */ + }; + + /** + * Get parameter set instance. + * + * @return Parameter set instance. + */ + static ParameterSets& getInstance() + { + static ParameterSets instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * Choose a specific parameter set. + * If a invalid set id is given, nothing changes. + * + * @param[in] setId Parameter set id + */ + void choose(uint8_t setId); + + /** + * Change to next parameter set. + * After the last set, the first will be choosen. + */ + void next(); + + /** + * Get current set id. + * + * @return Parameter set id. + */ + uint8_t getCurrentSetId() const; + + /** + * Get selected parameter set. + * + * @return Parameter set + */ + const ParameterSet& getParameterSet() const; + + /** Max. number of parameter sets. */ + static const uint8_t MAX_SETS = 4U; + +protected: +private: + uint8_t m_currentSetId; /**< Set id of current selected set. */ + ParameterSet m_parSets[MAX_SETS]; /**< All parameter sets */ + + /** + * Default constructor. + */ + ParameterSets(); + + /** + * Default destructor. + */ + ~ParameterSets(); + + /* 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 ea09de51..ba111dad 100644 --- a/lib/APPLineFollower/ReadyState.cpp +++ b/lib/APPLineFollower/ReadyState.cpp @@ -1,168 +1,168 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Ready state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ReadyState.h" -#include -#include -#include "ReleaseTrackState.h" -#include -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/** - * Logging source. - */ -LOG_TAG("RState"); - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void ReadyState::entry() -{ - IDisplay& display = Board::getInstance().getDisplay(); - const int32_t SENSOR_VALUE_OUT_PERIOD = 1000; /* ms */ - - display.clear(); - display.print("A: Go"); - - if (true == m_isLapTimeAvailable) - { - display.gotoXY(0, 1); - display.print(m_lapTime); - display.print("ms"); - - LOG_INFO_VAL("Lap time: ", m_lapTime); - } - - /* The line sensor value shall be output on console cyclic. */ - m_timer.start(SENSOR_VALUE_OUT_PERIOD); -} - -void ReadyState::process(StateMachine& sm) -{ - IButton& buttonA = Board::getInstance().getButtonA(); - - /* Shall track be released? */ - if (true == 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(); - } - else - { - /* Nothing to do. */ - ; - } -} - -void ReadyState::exit() -{ - m_timer.stop(); - m_isLapTimeAvailable = false; -} - -void ReadyState::setLapTime(uint32_t lapTime) -{ - m_isLapTimeAvailable = true; - m_lapTime = lapTime; -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Ready state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ReadyState.h" +#include +#include +#include "ReleaseTrackState.h" +#include +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/** + * Logging source. + */ +LOG_TAG("RState"); + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void ReadyState::entry() +{ + IDisplay& display = Board::getInstance().getDisplay(); + const int32_t SENSOR_VALUE_OUT_PERIOD = 1000; /* ms */ + + display.clear(); + display.print("A: Go"); + + if (true == m_isLapTimeAvailable) + { + display.gotoXY(0, 1); + display.print(m_lapTime); + display.print("ms"); + + LOG_INFO_VAL("Lap time: ", m_lapTime); + } + + /* The line sensor value shall be output on console cyclic. */ + m_timer.start(SENSOR_VALUE_OUT_PERIOD); +} + +void ReadyState::process(StateMachine& sm) +{ + IButton& buttonA = Board::getInstance().getButtonA(); + + /* Shall track be released? */ + if (true == 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(); + } + else + { + /* Nothing to do. */ + ; + } +} + +void ReadyState::exit() +{ + m_timer.stop(); + m_isLapTimeAvailable = false; +} + +void ReadyState::setLapTime(uint32_t lapTime) +{ + m_isLapTimeAvailable = true; + m_lapTime = lapTime; +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPLineFollower/ReadyState.h b/lib/APPLineFollower/ReadyState.h index 9e01f1c5..f14b6e64 100644 --- a/lib/APPLineFollower/ReadyState.h +++ b/lib/APPLineFollower/ReadyState.h @@ -1,129 +1,129 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Ready state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef READY_STATE_H -#define READY_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The system ready state. */ -class ReadyState : public IState -{ -public: - /** - * Get state instance. - * - * @return State instance. - */ - static ReadyState& getInstance() - { - static ReadyState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - - /** - * Set lap time, which to show on the display. - * - * @param[in] lapTime Lap time in ms - */ - void setLapTime(uint32_t lapTime); - -protected: -private: - SimpleTimer m_timer; /**< Timer used for cyclic debug output. */ - bool m_isLapTimeAvailable; /**< Is set (true), if a lap time is available. */ - uint32_t m_lapTime; /**< Lap time in ms of the last successful driven round. */ - - /** - * 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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Ready state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef READY_STATE_H +#define READY_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The system ready state. */ +class ReadyState : public IState +{ +public: + /** + * Get state instance. + * + * @return State instance. + */ + static ReadyState& getInstance() + { + static ReadyState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + + /** + * Set lap time, which to show on the display. + * + * @param[in] lapTime Lap time in ms + */ + void setLapTime(uint32_t lapTime); + +protected: +private: + SimpleTimer m_timer; /**< Timer used for cyclic debug output. */ + bool m_isLapTimeAvailable; /**< Is set (true), if a lap time is available. */ + uint32_t m_lapTime; /**< Lap time in ms of the last successful driven round. */ + + /** + * 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 cc26b620..10af6e79 100644 --- a/lib/APPLineFollower/ReleaseTrackState.cpp +++ b/lib/APPLineFollower/ReleaseTrackState.cpp @@ -1,129 +1,129 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Release track state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ReleaseTrackState.h" -#include -#include -#include "DrivingState.h" -#include "ParameterSets.h" - -/****************************************************************************** - * 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("A: CHG SET"); - display.gotoXY(0, 1); - display.print(parSetName); - display.print(parSetId); -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief 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("A: CHG SET"); + display.gotoXY(0, 1); + display.print(parSetName); + display.print(parSetId); +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPLineFollower/ReleaseTrackState.h b/lib/APPLineFollower/ReleaseTrackState.h index 0e0ca144..1a92031b 100644 --- a/lib/APPLineFollower/ReleaseTrackState.h +++ b/lib/APPLineFollower/ReleaseTrackState.h @@ -1,129 +1,129 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Release track state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef RELEASE_TRACK_STATE_H -#define RELEASE_TRACK_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The system release track state. */ -class ReleaseTrackState : public IState -{ -public: - /** - * Get state instance. - * - * @return State instance. - */ - static ReleaseTrackState& getInstance() - { - static ReleaseTrackState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - -protected: -private: - /** Track release timer duration in ms. */ - static const uint32_t TRACK_RELEASE_DURATION = 5000; - - SimpleTimer m_releaseTimer; /**< Track release timer */ - - /** - * 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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Release track state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef RELEASE_TRACK_STATE_H +#define RELEASE_TRACK_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The system release track state. */ +class ReleaseTrackState : public IState +{ +public: + /** + * Get state instance. + * + * @return State instance. + */ + static ReleaseTrackState& getInstance() + { + static ReleaseTrackState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + +protected: +private: + /** Track release timer duration in ms. */ + static const uint32_t TRACK_RELEASE_DURATION = 5000; + + SimpleTimer m_releaseTimer; /**< Track release timer */ + + /** + * 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.h b/lib/APPLineFollower/StartupState.h index f718292a..e50cc304 100644 --- a/lib/APPLineFollower/StartupState.h +++ b/lib/APPLineFollower/StartupState.h @@ -1,148 +1,148 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Startup state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef STARTUP_STATE_H -#define STARTUP_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The system startup state. */ -class StartupState : public IState -{ -public: - /** - * Get state instance. - * - * @return State instance. - */ - static StartupState& getInstance() - { - static StartupState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - -protected: -private: - /** - * This type defines different kind of information, which will be shown - * to the user in the same order as defined. - */ - enum UserInfo - { - USER_INFO_TEAM_NAME = 0, /**< Show the team name. */ - USER_INFO_MAX_MOTOR_SPEED, /**< Show the max. motor speed. */ - USER_INFO_UI, /**< Show the user interface. */ - USER_INFO_COUNT /**< Number of user infos. */ - }; - - /** - * Duration in ms how long a info on the display shall be shown, until - * the next info appears. - */ - static const uint32_t INFO_DURATION = 2000; - - bool m_isMaxMotorSpeedCalibAvailable; /**< Is max. motor speed calibration value available? */ - SimpleTimer m_timer; /**< Used to show information for a certain time before changing to the next info. */ - UserInfo m_userInfoState; /**< Current user info state. */ - - /** - * Default constructor. - */ - StartupState() : m_isMaxMotorSpeedCalibAvailable(false), m_timer(), m_userInfoState(USER_INFO_TEAM_NAME) - { - } - - /** - * Default destructor. - */ - ~StartupState() - { - } - - /* Not allowed. */ - StartupState(const StartupState& state); /**< Copy construction of an instance. */ - StartupState& operator=(const StartupState& state); /**< Assignment of an instance. */ - - /** - * Show next user info. - * - * @param[in] next Next user info which to show. - */ - void showUserInfo(UserInfo next); -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* STARTUP_STATE_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Startup state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef STARTUP_STATE_H +#define STARTUP_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The system startup state. */ +class StartupState : public IState +{ +public: + /** + * Get state instance. + * + * @return State instance. + */ + static StartupState& getInstance() + { + static StartupState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + +protected: +private: + /** + * This type defines different kind of information, which will be shown + * to the user in the same order as defined. + */ + enum UserInfo + { + USER_INFO_TEAM_NAME = 0, /**< Show the team name. */ + USER_INFO_MAX_MOTOR_SPEED, /**< Show the max. motor speed. */ + USER_INFO_UI, /**< Show the user interface. */ + USER_INFO_COUNT /**< Number of user infos. */ + }; + + /** + * Duration in ms how long a info on the display shall be shown, until + * the next info appears. + */ + static const uint32_t INFO_DURATION = 2000; + + bool m_isMaxMotorSpeedCalibAvailable; /**< Is max. motor speed calibration value available? */ + SimpleTimer m_timer; /**< Used to show information for a certain time before changing to the next info. */ + UserInfo m_userInfoState; /**< Current user info state. */ + + /** + * Default constructor. + */ + StartupState() : m_isMaxMotorSpeedCalibAvailable(false), m_timer(), m_userInfoState(USER_INFO_TEAM_NAME) + { + } + + /** + * Default destructor. + */ + ~StartupState() + { + } + + /* Not allowed. */ + StartupState(const StartupState& state); /**< Copy construction of an instance. */ + StartupState& operator=(const StartupState& state); /**< Assignment of an instance. */ + + /** + * Show next user info. + * + * @param[in] next Next user info which to show. + */ + void showUserInfo(UserInfo next); +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* STARTUP_STATE_H */ +/** @} */ diff --git a/lib/APPLineFollower/library.json b/lib/APPLineFollower/library.json index e23ddbda..e52e90e1 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.h b/lib/APPRemoteControl/App.h index ba77d486..c9405b0a 100644 --- a/lib/APPRemoteControl/App.h +++ b/lib/APPRemoteControl/App.h @@ -1,206 +1,206 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief RemoteControl application - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef APP_H -#define APP_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ - -#include "SerialMuxChannels.h" -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The remote control application. */ -class App -{ -public: - /** - * Construct the remote control application. - */ - App() : - m_serialMuxProtChannelIdRemoteCtrlRsp(0U), - m_serialMuxProtChannelIdCurrentVehicleData(0U), - m_serialMuxProtChannelIdStatus(0U), - m_serialMuxProtChannelIdLineSensors(0U), - m_systemStateMachine(), - m_controlInterval(), - m_reportTimer(), - m_statusTimer(), - m_statusTimeoutTimer(), - m_sendLineSensorsDataInterval(), - m_smpServer(Serial, this), - m_movAvgProximitySensor() - { - } - - /** - * Destroy the remote control application. - */ - ~App() - { - } - - /** - * Setup the application. - */ - void setup(); - - /** - * Process the application periodically. - */ - void loop(); - - /** - * Handle remote command received via SerialMuxProt. - * - * @param[in] cmd Command to handle. - */ - void handleRemoteCommand(const Command& cmd); - - /** - * System Status callback. - * - * @param[in] status System status - */ - void systemStatusCallback(SMPChannelPayload::Status status); - -private: - /** Differential drive control period in ms. */ - static const uint32_t DIFFERENTIAL_DRIVE_CONTROL_PERIOD = 5U; - - /** Current data reporting period in ms. */ - static const uint32_t REPORTING_PERIOD = 50U; - - /** Baudrate for Serial Communication */ - static const uint32_t SERIAL_BAUDRATE = 115200U; - - /** Send status timer interval in ms. */ - static const uint32_t SEND_STATUS_TIMER_INTERVAL = 1000U; - - /** Status timeout timer interval in ms. */ - static const uint32_t STATUS_TIMEOUT_TIMER_INTERVAL = 2U * SEND_STATUS_TIMER_INTERVAL; - - /** Sending Data period in ms. */ - static const uint32_t SEND_LINE_SENSORS_DATA_PERIOD = 20U; - - /** - * Number of measurements for proximity sensors moving average filter. - */ - static const uint8_t MOVAVG_PROXIMITY_SENSOR_NUM_MEASUREMENTS = 3U; - - /** SerialMuxProt Channel id for sending remote control command responses. */ - uint8_t m_serialMuxProtChannelIdRemoteCtrlRsp; - - /** SerialMuxProt Channel id for sending the current vehicle data. */ - uint8_t m_serialMuxProtChannelIdCurrentVehicleData; - - /** SerialMuxProt Channel id for sending system status. */ - uint8_t m_serialMuxProtChannelIdStatus; - - /** SerialMuxProt Channel id for sending line sensors data. */ - uint8_t m_serialMuxProtChannelIdLineSensors; - - /** 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; - - /** Timer for sending system status to DCS. */ - SimpleTimer m_statusTimer; - - /** Timer for timeout of system status of DCS. */ - SimpleTimer m_statusTimeoutTimer; - - /** Timer used for sending data periodically. */ - SimpleTimer m_sendLineSensorsDataInterval; - - /** SerialMuxProt Server Instance. */ - SMPServer m_smpServer; - - /** - * Moving average filter for proximity sensors. - */ - MovAvg m_movAvgProximitySensor; - - /** - * Report the current vehicle data. - * Report the current position and heading of the robot using the Odometry data. - * Report the current motor speeds of the robot using the Speedometer data. - * Sends data through the SerialMuxProtServer. - */ - void reportVehicleData(); - - /** - * Setup the SerialMuxProt channels. - * - * @return If successful returns true, otherwise false. - */ - bool setupSerialMuxProt(); - - /** - * Send line sensors data via SerialMuxProt. - */ - void sendLineSensorsData() const; - - /* Not allowed. */ - App(const App& app); /**< Copy construction of an instance. */ - App& operator=(const App& app); /**< Assignment of an instance. */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* APP_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief RemoteControl application + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef APP_H +#define APP_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ + +#include "SerialMuxChannels.h" +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The remote control application. */ +class App +{ +public: + /** + * Construct the remote control application. + */ + App() : + m_serialMuxProtChannelIdRemoteCtrlRsp(0U), + m_serialMuxProtChannelIdCurrentVehicleData(0U), + m_serialMuxProtChannelIdStatus(0U), + m_serialMuxProtChannelIdLineSensors(0U), + m_systemStateMachine(), + m_controlInterval(), + m_reportTimer(), + m_statusTimer(), + m_statusTimeoutTimer(), + m_sendLineSensorsDataInterval(), + m_smpServer(Serial, this), + m_movAvgProximitySensor() + { + } + + /** + * Destroy the remote control application. + */ + ~App() + { + } + + /** + * Setup the application. + */ + void setup(); + + /** + * Process the application periodically. + */ + void loop(); + + /** + * Handle remote command received via SerialMuxProt. + * + * @param[in] cmd Command to handle. + */ + void handleRemoteCommand(const Command& cmd); + + /** + * System Status callback. + * + * @param[in] status System status + */ + void systemStatusCallback(SMPChannelPayload::Status status); + +private: + /** Differential drive control period in ms. */ + static const uint32_t DIFFERENTIAL_DRIVE_CONTROL_PERIOD = 5U; + + /** Current data reporting period in ms. */ + static const uint32_t REPORTING_PERIOD = 50U; + + /** Baudrate for Serial Communication */ + static const uint32_t SERIAL_BAUDRATE = 115200U; + + /** Send status timer interval in ms. */ + static const uint32_t SEND_STATUS_TIMER_INTERVAL = 1000U; + + /** Status timeout timer interval in ms. */ + static const uint32_t STATUS_TIMEOUT_TIMER_INTERVAL = 2U * SEND_STATUS_TIMER_INTERVAL; + + /** Sending Data period in ms. */ + static const uint32_t SEND_LINE_SENSORS_DATA_PERIOD = 20U; + + /** + * Number of measurements for proximity sensors moving average filter. + */ + static const uint8_t MOVAVG_PROXIMITY_SENSOR_NUM_MEASUREMENTS = 3U; + + /** SerialMuxProt Channel id for sending remote control command responses. */ + uint8_t m_serialMuxProtChannelIdRemoteCtrlRsp; + + /** SerialMuxProt Channel id for sending the current vehicle data. */ + uint8_t m_serialMuxProtChannelIdCurrentVehicleData; + + /** SerialMuxProt Channel id for sending system status. */ + uint8_t m_serialMuxProtChannelIdStatus; + + /** SerialMuxProt Channel id for sending line sensors data. */ + uint8_t m_serialMuxProtChannelIdLineSensors; + + /** 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; + + /** Timer for sending system status to DCS. */ + SimpleTimer m_statusTimer; + + /** Timer for timeout of system status of DCS. */ + SimpleTimer m_statusTimeoutTimer; + + /** Timer used for sending data periodically. */ + SimpleTimer m_sendLineSensorsDataInterval; + + /** SerialMuxProt Server Instance. */ + SMPServer m_smpServer; + + /** + * Moving average filter for proximity sensors. + */ + MovAvg m_movAvgProximitySensor; + + /** + * Report the current vehicle data. + * Report the current position and heading of the robot using the Odometry data. + * Report the current motor speeds of the robot using the Speedometer data. + * Sends data through the SerialMuxProtServer. + */ + void reportVehicleData(); + + /** + * Setup the SerialMuxProt channels. + * + * @return If successful returns true, otherwise false. + */ + bool setupSerialMuxProt(); + + /** + * Send line sensors data via SerialMuxProt. + */ + void sendLineSensorsData() const; + + /* Not allowed. */ + App(const App& app); /**< Copy construction of an instance. */ + App& operator=(const App& app); /**< Assignment of an instance. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* APP_H */ +/** @} */ diff --git a/lib/APPRemoteControl/DrivingState.cpp b/lib/APPRemoteControl/DrivingState.cpp index 04ce7176..f4bd07e2 100644 --- a/lib/APPRemoteControl/DrivingState.cpp +++ b/lib/APPRemoteControl/DrivingState.cpp @@ -1,109 +1,109 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Driving state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "DrivingState.h" -#include -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void DrivingState::entry() -{ - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - - m_isActive = true; - diffDrive.setLinearSpeed(0, 0); - diffDrive.enable(); -} - -void DrivingState::process(StateMachine& sm) -{ - /* Nothing to do. */ - (void)sm; -} - -void DrivingState::exit() -{ - m_isActive = false; - - /* Stop motors. */ - DifferentialDrive::getInstance().setLinearSpeed(0, 0); - DifferentialDrive::getInstance().disable(); -} - -void DrivingState::setTargetSpeeds(int16_t leftMotor, int16_t rightMotor) -{ - if (true == m_isActive) - { - DifferentialDrive::getInstance().setLinearSpeed(leftMotor, rightMotor); - } -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Driving state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "DrivingState.h" +#include +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void DrivingState::entry() +{ + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + + m_isActive = true; + diffDrive.setLinearSpeed(0, 0); + diffDrive.enable(); +} + +void DrivingState::process(StateMachine& sm) +{ + /* Nothing to do. */ + (void)sm; +} + +void DrivingState::exit() +{ + m_isActive = false; + + /* Stop motors. */ + DifferentialDrive::getInstance().setLinearSpeed(0, 0); + DifferentialDrive::getInstance().disable(); +} + +void DrivingState::setTargetSpeeds(int16_t leftMotor, int16_t rightMotor) +{ + if (true == m_isActive) + { + DifferentialDrive::getInstance().setLinearSpeed(leftMotor, rightMotor); + } +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPRemoteControl/DrivingState.h b/lib/APPRemoteControl/DrivingState.h index b336c119..92f48a94 100644 --- a/lib/APPRemoteControl/DrivingState.h +++ b/lib/APPRemoteControl/DrivingState.h @@ -1,130 +1,130 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Driving state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef DRIVING_STATE_H -#define DRIVING_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The system driving state. */ -class DrivingState : public IState -{ -public: - /** - * Get state instance. - * - * @return State instance. - */ - static DrivingState& getInstance() - { - static DrivingState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - - /** - * Set target motor speeds. - * - * @param[in] leftMotor Left motor speed. [steps/s] - * @param[in] rightMotor Right motor speed. [steps/s] - */ - void setTargetSpeeds(int16_t leftMotor, int16_t rightMotor); - -protected: -private: - /** Flag: State is active. */ - bool m_isActive; - - /** - * Default constructor. - */ - DrivingState() : IState(), m_isActive(false) - { - } - - /** - * Default destructor. - */ - ~DrivingState() - { - } - - /* Not allowed. */ - DrivingState(const DrivingState& state); /**< Copy construction of an instance. */ - DrivingState& operator=(const DrivingState& state); /**< Assignment of an instance. */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* DRIVING_STATE_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Driving state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef DRIVING_STATE_H +#define DRIVING_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The system driving state. */ +class DrivingState : public IState +{ +public: + /** + * Get state instance. + * + * @return State instance. + */ + static DrivingState& getInstance() + { + static DrivingState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + + /** + * Set target motor speeds. + * + * @param[in] leftMotor Left motor speed. [steps/s] + * @param[in] rightMotor Right motor speed. [steps/s] + */ + void setTargetSpeeds(int16_t leftMotor, int16_t rightMotor); + +protected: +private: + /** Flag: State is active. */ + bool m_isActive; + + /** + * Default constructor. + */ + DrivingState() : IState(), m_isActive(false) + { + } + + /** + * Default destructor. + */ + ~DrivingState() + { + } + + /* Not allowed. */ + DrivingState(const DrivingState& state); /**< Copy construction of an instance. */ + DrivingState& operator=(const DrivingState& state); /**< Assignment of an instance. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* DRIVING_STATE_H */ +/** @} */ diff --git a/lib/APPRemoteControl/ErrorState.cpp b/lib/APPRemoteControl/ErrorState.cpp index cefecb8d..dff63afd 100644 --- a/lib/APPRemoteControl/ErrorState.cpp +++ b/lib/APPRemoteControl/ErrorState.cpp @@ -1,113 +1,113 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Error state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ErrorState.h" -#include -#include -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void ErrorState::entry() -{ - IDisplay& display = Board::getInstance().getDisplay(); - DifferentialDrive::getInstance().disable(); - - display.clear(); - display.print("Error"); - display.gotoXY(0, 1); - display.print(m_errorMsg); -} - -void ErrorState::process(StateMachine& sm) -{ - /* Nothing to do. */ - (void)sm; -} - -void ErrorState::exit() -{ - /* Nothing to do. */ -} - -void ErrorState::setErrorMsg(const char* msg) -{ - if (nullptr == msg) - { - m_errorMsg[0] = '\0'; - } - else - { - strncpy(m_errorMsg, msg, ERROR_MSG_SIZE - 1); - m_errorMsg[ERROR_MSG_SIZE - 1] = '\0'; - } -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Error state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ErrorState.h" +#include +#include +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void ErrorState::entry() +{ + IDisplay& display = Board::getInstance().getDisplay(); + DifferentialDrive::getInstance().disable(); + + display.clear(); + display.print("Error"); + display.gotoXY(0, 1); + display.print(m_errorMsg); +} + +void ErrorState::process(StateMachine& sm) +{ + /* Nothing to do. */ + (void)sm; +} + +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 74095203..0679e9a3 100644 --- a/lib/APPRemoteControl/ErrorState.h +++ b/lib/APPRemoteControl/ErrorState.h @@ -1,134 +1,134 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Error state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef ERROR_STATE_H -#define ERROR_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The system error state. */ -class ErrorState : public IState -{ -public: - /** - * Get state instance. - * - * @return State instance. - */ - static ErrorState& getInstance() - { - static ErrorState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - - /** - * Set error message, which to show on the display. - * - * @param[in] msg Error message - */ - void setErrorMsg(const char* msg); - -protected: -private: - /** - * The error message string size in bytes, which - * includes the terminating character. - */ - static const size_t ERROR_MSG_SIZE = 20; - - char m_errorMsg[ERROR_MSG_SIZE]; /**< Error message, which to show. */ - - /** - * Default constructor. - */ - ErrorState() : m_errorMsg() - { - m_errorMsg[0] = '\0'; - } - - /** - * Default destructor. - */ - ~ErrorState() - { - } - - /* Not allowed. */ - ErrorState(const ErrorState& state); /**< Copy construction of an instance. */ - ErrorState& operator=(const ErrorState& state); /**< Assignment of an instance. */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* ERROR_STATE_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Error state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef ERROR_STATE_H +#define ERROR_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The system error state. */ +class ErrorState : public IState +{ +public: + /** + * Get state instance. + * + * @return State instance. + */ + static ErrorState& getInstance() + { + static ErrorState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + + /** + * Set error message, which to show on the display. + * + * @param[in] msg Error message + */ + void setErrorMsg(const char* msg); + +protected: +private: + /** + * The error message string size in bytes, which + * includes the terminating character. + */ + static const size_t ERROR_MSG_SIZE = 20; + + char m_errorMsg[ERROR_MSG_SIZE]; /**< Error message, which to show. */ + + /** + * Default constructor. + */ + ErrorState() : m_errorMsg() + { + m_errorMsg[0] = '\0'; + } + + /** + * Default destructor. + */ + ~ErrorState() + { + } + + /* Not allowed. */ + ErrorState(const ErrorState& state); /**< Copy construction of an instance. */ + ErrorState& operator=(const ErrorState& state); /**< Assignment of an instance. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* ERROR_STATE_H */ +/** @} */ diff --git a/lib/APPRemoteControl/LineSensorsCalibrationState.cpp b/lib/APPRemoteControl/LineSensorsCalibrationState.cpp index c54cff2d..bd4c34b1 100644 --- a/lib/APPRemoteControl/LineSensorsCalibrationState.cpp +++ b/lib/APPRemoteControl/LineSensorsCalibrationState.cpp @@ -1,213 +1,213 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Line sensors calibration state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "LineSensorsCalibrationState.h" -#include -#include -#include -#include -#include -#include "ErrorState.h" -#include "DrivingState.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void LineSensorsCalibrationState::entry() -{ - IDisplay& display = Board::getInstance().getDisplay(); - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - Odometry& odometry = Odometry::getInstance(); - ILineSensors& lineSensors = Board::getInstance().getLineSensors(); - - display.clear(); - display.print("Calib"); - display.gotoXY(0, 1); - display.print("Lines"); - - /* Prepare calibration drive. */ - m_calibrationSpeed = diffDrive.getMaxMotorSpeed() / 3; - m_orientation = odometry.getOrientation(); - - /* Mandatory for each new calibration. */ - lineSensors.resetCalibration(); - - /* Wait some time, before starting the calibration drive. */ - m_phase = PHASE_1_WAIT; - m_timer.start(WAIT_TIME); -} - -void LineSensorsCalibrationState::process(StateMachine& sm) -{ - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - - switch (m_phase) - { - case PHASE_1_WAIT: - if (true == m_timer.isTimeout()) - { - m_phase = PHASE_2_TURN_LEFT; - diffDrive.setLinearSpeed(-m_calibrationSpeed, m_calibrationSpeed); - } - break; - - case PHASE_2_TURN_LEFT: - if (true == turnAndCalibrate(CALIB_ANGLE, true)) - { - m_phase = PHASE_3_TURN_RIGHT; - diffDrive.setLinearSpeed(m_calibrationSpeed, -m_calibrationSpeed); - } - break; - - case PHASE_3_TURN_RIGHT: - if (true == turnAndCalibrate(-CALIB_ANGLE, false)) - { - m_phase = PHASE_4_TURN_ORIG; - diffDrive.setLinearSpeed(-m_calibrationSpeed, m_calibrationSpeed); - } - break; - - case PHASE_4_TURN_ORIG: - if (true == turnAndCalibrate(0, true)) - { - m_phase = PHASE_5_FINISHED; - diffDrive.setLinearSpeed(0, 0); - finishCalibration(sm); - } - break; - - case PHASE_5_FINISHED: - /* fallthrough */ - default: - break; - } -} - -void LineSensorsCalibrationState::exit() -{ - m_timer.stop(); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -bool LineSensorsCalibrationState::turnAndCalibrate(int32_t calibAlpha, bool isGreaterEqual) -{ - ILineSensors& lineSensors = Board::getInstance().getLineSensors(); - Odometry& odometry = Odometry::getInstance(); - int32_t alpha = odometry.getOrientation() - m_orientation; /* [mrad] */ - bool isSuccesful = false; - - /* Continously calibrate the line sensors. */ - lineSensors.calibrate(); - - /* Is the goal that the current angle shall be lower or equal than the destination calibration angle? */ - if (false == isGreaterEqual) - { - /* Is alpha lower or equal than the destination calibration angle? */ - if (calibAlpha >= alpha) - { - isSuccesful = true; - } - } - else - { - /* Is alpha greater or equal than the destination calibration angle? */ - if (calibAlpha <= alpha) - { - isSuccesful = true; - } - } - - return isSuccesful; -} - -void LineSensorsCalibrationState::finishCalibration(StateMachine& sm) -{ - ILineSensors& lineSensors = Board::getInstance().getLineSensors(); - - if (false == lineSensors.isCalibrationSuccessful()) - { - char str[10]; - char valueStr[10]; - - Util::uintToStr(valueStr, sizeof(valueStr), lineSensors.getCalibErrorInfo()); - - strncpy(str, "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(&DrivingState::getInstance()); - } -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Line sensors calibration state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "LineSensorsCalibrationState.h" +#include +#include +#include +#include +#include +#include "ErrorState.h" +#include "DrivingState.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void LineSensorsCalibrationState::entry() +{ + IDisplay& display = Board::getInstance().getDisplay(); + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + Odometry& odometry = Odometry::getInstance(); + ILineSensors& lineSensors = Board::getInstance().getLineSensors(); + + display.clear(); + display.print("Calib"); + display.gotoXY(0, 1); + display.print("Lines"); + + /* Prepare calibration drive. */ + m_calibrationSpeed = diffDrive.getMaxMotorSpeed() / 3; + m_orientation = odometry.getOrientation(); + + /* Mandatory for each new calibration. */ + lineSensors.resetCalibration(); + + /* Wait some time, before starting the calibration drive. */ + m_phase = PHASE_1_WAIT; + m_timer.start(WAIT_TIME); +} + +void LineSensorsCalibrationState::process(StateMachine& sm) +{ + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + + switch (m_phase) + { + case PHASE_1_WAIT: + if (true == m_timer.isTimeout()) + { + m_phase = PHASE_2_TURN_LEFT; + diffDrive.setLinearSpeed(-m_calibrationSpeed, m_calibrationSpeed); + } + break; + + case PHASE_2_TURN_LEFT: + if (true == turnAndCalibrate(CALIB_ANGLE, true)) + { + m_phase = PHASE_3_TURN_RIGHT; + diffDrive.setLinearSpeed(m_calibrationSpeed, -m_calibrationSpeed); + } + break; + + case PHASE_3_TURN_RIGHT: + if (true == turnAndCalibrate(-CALIB_ANGLE, false)) + { + m_phase = PHASE_4_TURN_ORIG; + diffDrive.setLinearSpeed(-m_calibrationSpeed, m_calibrationSpeed); + } + break; + + case PHASE_4_TURN_ORIG: + if (true == turnAndCalibrate(0, true)) + { + m_phase = PHASE_5_FINISHED; + diffDrive.setLinearSpeed(0, 0); + finishCalibration(sm); + } + break; + + case PHASE_5_FINISHED: + /* fallthrough */ + default: + break; + } +} + +void LineSensorsCalibrationState::exit() +{ + m_timer.stop(); +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +bool LineSensorsCalibrationState::turnAndCalibrate(int32_t calibAlpha, bool isGreaterEqual) +{ + ILineSensors& lineSensors = Board::getInstance().getLineSensors(); + Odometry& odometry = Odometry::getInstance(); + int32_t alpha = odometry.getOrientation() - m_orientation; /* [mrad] */ + bool isSuccesful = false; + + /* Continously calibrate the line sensors. */ + lineSensors.calibrate(); + + /* Is the goal that the current angle shall be lower or equal than the destination calibration angle? */ + if (false == isGreaterEqual) + { + /* Is alpha lower or equal than the destination calibration angle? */ + if (calibAlpha >= alpha) + { + isSuccesful = true; + } + } + else + { + /* Is alpha greater or equal than the destination calibration angle? */ + if (calibAlpha <= alpha) + { + isSuccesful = true; + } + } + + return isSuccesful; +} + +void LineSensorsCalibrationState::finishCalibration(StateMachine& sm) +{ + ILineSensors& lineSensors = Board::getInstance().getLineSensors(); + + if (false == lineSensors.isCalibrationSuccessful()) + { + char str[10]; + char valueStr[10]; + + Util::uintToStr(valueStr, sizeof(valueStr), lineSensors.getCalibErrorInfo()); + + strncpy(str, "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(&DrivingState::getInstance()); + } +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPRemoteControl/LineSensorsCalibrationState.h b/lib/APPRemoteControl/LineSensorsCalibrationState.h index f59dd3c4..1ee29963 100644 --- a/lib/APPRemoteControl/LineSensorsCalibrationState.h +++ b/lib/APPRemoteControl/LineSensorsCalibrationState.h @@ -1,161 +1,161 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Line sensors calibration state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef LINE_SENSORS_CALIBRATION_STATE_H -#define LINE_SENSORS_CALIBRATION_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The line sensors calibration state. */ -class LineSensorsCalibrationState : public IState -{ -public: - /** - * Get state instance. - * - * @return State instance. - */ - static LineSensorsCalibrationState& getInstance() - { - static LineSensorsCalibrationState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - -protected: -private: - /** Calibration phases */ - enum Phase - { - PHASE_1_WAIT = 0, /**< Wait a short time before starting the calibration. */ - PHASE_2_TURN_LEFT, /**< Turn line sensors left over the line. */ - PHASE_3_TURN_RIGHT, /**< Turn line sensor right over the line. */ - PHASE_4_TURN_ORIG, /**< Turn back to origin. */ - PHASE_5_FINISHED /**< Calibration is finished. */ - }; - - /** - * Duration in ms about to wait, until the calibration drive starts. - */ - static const uint32_t WAIT_TIME = 1000; - - /** - * Calibration turn angle in mrad (corresponds to 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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Line sensors calibration state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef LINE_SENSORS_CALIBRATION_STATE_H +#define LINE_SENSORS_CALIBRATION_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The line sensors calibration state. */ +class LineSensorsCalibrationState : public IState +{ +public: + /** + * Get state instance. + * + * @return State instance. + */ + static LineSensorsCalibrationState& getInstance() + { + static LineSensorsCalibrationState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + +protected: +private: + /** Calibration phases */ + enum Phase + { + PHASE_1_WAIT = 0, /**< Wait a short time before starting the calibration. */ + PHASE_2_TURN_LEFT, /**< Turn line sensors left over the line. */ + PHASE_3_TURN_RIGHT, /**< Turn line sensor right over the line. */ + PHASE_4_TURN_ORIG, /**< Turn back to origin. */ + PHASE_5_FINISHED /**< Calibration is finished. */ + }; + + /** + * Duration in ms about to wait, until the calibration drive starts. + */ + static const uint32_t WAIT_TIME = 1000; + + /** + * Calibration turn angle in mrad (corresponds to 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/SerialMuxChannels.h b/lib/APPRemoteControl/SerialMuxChannels.h index d8660246..c79b9754 100644 --- a/lib/APPRemoteControl/SerialMuxChannels.h +++ b/lib/APPRemoteControl/SerialMuxChannels.h @@ -1,212 +1,212 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Channel structure definition for the SerialMuxProt. - * @author Gabryel Reyes - */ - -#ifndef SERIAL_MUX_CHANNELS_H_ -#define SERIAL_MUX_CHANNELS_H_ - -/****************************************************************************** - * Includes - *****************************************************************************/ - -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/** Maximum number of SerialMuxProt Channels. */ -#define MAX_CHANNELS (10U) - -/** Name of Channel to send Commands to. */ -#define COMMAND_CHANNEL_NAME "CMD" - -/** DLC of Command Channel. */ -#define COMMAND_CHANNEL_DLC (sizeof(Command)) - -/** Name of Channel to receive Command Responses from. */ -#define COMMAND_RESPONSE_CHANNEL_NAME "CMD_RSP" - -/** DLC of Command Response Channel. */ -#define COMMAND_RESPONSE_CHANNEL_DLC (sizeof(CommandResponse)) - -/** Name of Channel to send Motor Speed Setpoints to. */ -#define SPEED_SETPOINT_CHANNEL_NAME "SPEED_SET" - -/** DLC of Speedometer Channel */ -#define SPEED_SETPOINT_CHANNEL_DLC (sizeof(SpeedData)) - -/** Name of Channel to send Current Vehicle Data to. */ -#define CURRENT_VEHICLE_DATA_CHANNEL_NAME "CURR_DATA" - -/** DLC of Current Vehicle Data Channel */ -#define CURRENT_VEHICLE_DATA_CHANNEL_DLC (sizeof(VehicleData)) - -/** Name of Channel to send system status to. */ -#define STATUS_CHANNEL_NAME "STATUS" - -/** DLC of Status Channel */ -#define STATUS_CHANNEL_DLC (sizeof(Status)) - -/** Name of 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 - *****************************************************************************/ - -/** SerialMuxProt Server with fixed template argument. */ -typedef SerialMuxProtServer SMPServer; - -/** Channel payload constants. */ -namespace SMPChannelPayload -{ - /** Remote control commands. */ - typedef enum : uint8_t - { - CMD_ID_IDLE = 0, /**< Nothing to do. */ - CMD_ID_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. */ - CMD_ID_GET_MAX_SPEED, /**< Get maximum speed. */ - CMD_ID_START_DRIVING, /**< Start driving. */ - CMD_ID_SET_INIT_POS /**< Set initial position. */ - - } CmdId; /**< Command ID */ - - /** Remote control command responses. */ - typedef enum : uint8_t - { - RSP_ID_OK = 0, /**< Command successful executed. */ - RSP_ID_PENDING, /**< Command is pending. */ - RSP_ID_ERROR /**< Command failed. */ - - } RspId; /**< Response ID */ - - /** Status flags. */ - typedef enum : uint8_t - { - STATUS_FLAG_OK = 0, /**< Everything is fine. */ - STATUS_FLAG_ERROR /**< Something is wrong. */ - - } Status; /**< Status flag */ - - /** - * Range in which a detected object may be. - * Equivalent to the brightness levels. - * Values estimated from user's guide. - */ - typedef enum : uint8_t - { - RANGE_NO_OBJECT = 0U, /**< No object detected */ - RANGE_25_30, /**< Object detected in range 25 to 30 cm */ - RANGE_20_25, /**< Object detected in range 20 to 25 cm */ - RANGE_15_20, /**< Object detected in range 15 to 20 cm */ - RANGE_10_15, /**< Object detected in range 10 to 15 cm */ - RANGE_5_10, /**< Object detected in range 5 to 10 cm */ - RANGE_0_5 /**< Object detected in range 0 to 5 cm */ - - } Range; /**< Proximity Sensor Ranges */ - -} /* namespace SMPChannelPayload */ - -/** Struct of the "Command" channel payload. */ -typedef struct _Command -{ - SMPChannelPayload::CmdId commandId; /**< Command ID */ - - /** Command payload. */ - union - { - /** Init data command payload. */ - struct - { - int32_t xPos; /**< X position [mm]. */ - int32_t yPos; /**< Y position [mm]. */ - int32_t orientation; /**< Orientation [mrad]. */ - }; - }; - -} __attribute__((packed)) Command; - -/** Struct of the "Command Response" channel payload. */ -typedef struct _CommandResponse -{ - SMPChannelPayload::CmdId commandId; /**< Command ID */ - SMPChannelPayload::RspId responseId; /**< Response to the command */ - - /** Response Payload. */ - union - { - int16_t maxMotorSpeed; /**< Max speed [steps/s]. */ - }; -} __attribute__((packed)) CommandResponse; - -/** Struct of the "Speed" channel payload. */ -typedef struct _SpeedData -{ - int16_t left; /**< Left motor speed [steps/s] */ - int16_t right; /**< Right motor speed [steps/s] */ - int16_t center; /**< Center motor speed [steps/s] */ -} __attribute__((packed)) SpeedData; - -/** Struct of the "Current Vehicle Data" channel payload. */ -typedef struct _VehicleData -{ - int32_t xPos; /**< X position [mm]. */ - int32_t yPos; /**< Y position [mm]. */ - int32_t orientation; /**< Orientation [mrad]. */ - int16_t left; /**< Left motor speed [steps/s]. */ - int16_t right; /**< Right motor speed [steps/s]. */ - int16_t center; /**< Center speed [steps/s]. */ - SMPChannelPayload::Range proximity; /**< Range at which object is found [range]. */ -} __attribute__((packed)) VehicleData; - -/** Struct of the "Status" channel payload. */ -typedef struct _Status -{ - SMPChannelPayload::Status status; /**< Status */ -} __attribute__((packed)) Status; - -/** Struct of the "Line Sensor" channel payload. */ -typedef struct _LineSensorData -{ - uint16_t lineSensorData[5U]; /**< Line sensor data [digits] normalized to max 1000 digits. */ -} __attribute__((packed)) LineSensorData; - -/****************************************************************************** - * Functions - *****************************************************************************/ - +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Channel structure definition for the SerialMuxProt. + * @author Gabryel Reyes + */ + +#ifndef SERIAL_MUX_CHANNELS_H_ +#define SERIAL_MUX_CHANNELS_H_ + +/****************************************************************************** + * Includes + *****************************************************************************/ + +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/** Maximum number of SerialMuxProt Channels. */ +#define MAX_CHANNELS (10U) + +/** Name of Channel to send Commands to. */ +#define COMMAND_CHANNEL_NAME "CMD" + +/** DLC of Command Channel. */ +#define COMMAND_CHANNEL_DLC (sizeof(Command)) + +/** Name of Channel to 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 Channel to send Current Vehicle Data to. */ +#define CURRENT_VEHICLE_DATA_CHANNEL_NAME "CURR_DATA" + +/** DLC of Current Vehicle Data Channel */ +#define CURRENT_VEHICLE_DATA_CHANNEL_DLC (sizeof(VehicleData)) + +/** Name of Channel to send system status to. */ +#define STATUS_CHANNEL_NAME "STATUS" + +/** DLC of Status Channel */ +#define STATUS_CHANNEL_DLC (sizeof(Status)) + +/** Name of 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 + *****************************************************************************/ + +/** SerialMuxProt Server with fixed template argument. */ +typedef SerialMuxProtServer SMPServer; + +/** Channel payload constants. */ +namespace SMPChannelPayload +{ + /** Remote control commands. */ + typedef enum : uint8_t + { + CMD_ID_IDLE = 0, /**< Nothing to do. */ + CMD_ID_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. */ + CMD_ID_GET_MAX_SPEED, /**< Get maximum speed. */ + CMD_ID_START_DRIVING, /**< Start driving. */ + CMD_ID_SET_INIT_POS /**< Set initial position. */ + + } CmdId; /**< Command ID */ + + /** Remote control command responses. */ + typedef enum : uint8_t + { + RSP_ID_OK = 0, /**< Command successful executed. */ + RSP_ID_PENDING, /**< Command is pending. */ + RSP_ID_ERROR /**< Command failed. */ + + } RspId; /**< Response ID */ + + /** Status flags. */ + typedef enum : uint8_t + { + STATUS_FLAG_OK = 0, /**< Everything is fine. */ + STATUS_FLAG_ERROR /**< Something is wrong. */ + + } Status; /**< Status flag */ + + /** + * Range in which a detected object may be. + * Equivalent to the brightness levels. + * Values estimated from user's guide. + */ + typedef enum : uint8_t + { + RANGE_NO_OBJECT = 0U, /**< No object detected */ + RANGE_25_30, /**< Object detected in range 25 to 30 cm */ + RANGE_20_25, /**< Object detected in range 20 to 25 cm */ + RANGE_15_20, /**< Object detected in range 15 to 20 cm */ + RANGE_10_15, /**< Object detected in range 10 to 15 cm */ + RANGE_5_10, /**< Object detected in range 5 to 10 cm */ + RANGE_0_5 /**< Object detected in range 0 to 5 cm */ + + } Range; /**< Proximity Sensor Ranges */ + +} /* namespace SMPChannelPayload */ + +/** Struct of the "Command" channel payload. */ +typedef struct _Command +{ + SMPChannelPayload::CmdId commandId; /**< Command ID */ + + /** Command payload. */ + union + { + /** Init data command payload. */ + struct + { + int32_t xPos; /**< X position [mm]. */ + int32_t yPos; /**< Y position [mm]. */ + int32_t orientation; /**< Orientation [mrad]. */ + }; + }; + +} __attribute__((packed)) Command; + +/** Struct of the "Command Response" channel payload. */ +typedef struct _CommandResponse +{ + SMPChannelPayload::CmdId commandId; /**< Command ID */ + SMPChannelPayload::RspId responseId; /**< Response to the command */ + + /** Response Payload. */ + union + { + int16_t maxMotorSpeed; /**< Max speed [steps/s]. */ + }; +} __attribute__((packed)) CommandResponse; + +/** Struct of the "Speed" channel payload. */ +typedef struct _SpeedData +{ + int16_t left; /**< Left motor speed [steps/s] */ + int16_t right; /**< Right motor speed [steps/s] */ + int16_t center; /**< Center motor speed [steps/s] */ +} __attribute__((packed)) SpeedData; + +/** Struct of the "Current Vehicle Data" channel payload. */ +typedef struct _VehicleData +{ + int32_t xPos; /**< X position [mm]. */ + int32_t yPos; /**< Y position [mm]. */ + int32_t orientation; /**< Orientation [mrad]. */ + int16_t left; /**< Left motor speed [steps/s]. */ + int16_t right; /**< Right motor speed [steps/s]. */ + int16_t center; /**< Center speed [steps/s]. */ + SMPChannelPayload::Range proximity; /**< Range at which object is found [range]. */ +} __attribute__((packed)) VehicleData; + +/** Struct of the "Status" channel payload. */ +typedef struct _Status +{ + SMPChannelPayload::Status status; /**< Status */ +} __attribute__((packed)) Status; + +/** Struct of the "Line Sensor" channel payload. */ +typedef struct _LineSensorData +{ + uint16_t lineSensorData[5U]; /**< Line sensor data [digits] normalized to max 1000 digits. */ +} __attribute__((packed)) LineSensorData; + +/****************************************************************************** + * Functions + *****************************************************************************/ + #endif /* SERIAL_MUX_CHANNELS_H_ */ \ No newline at end of file diff --git a/lib/APPRemoteControl/StartupState.cpp b/lib/APPRemoteControl/StartupState.cpp index ebf2c07f..dc1ad8c0 100644 --- a/lib/APPRemoteControl/StartupState.cpp +++ b/lib/APPRemoteControl/StartupState.cpp @@ -1,124 +1,124 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Startup state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "StartupState.h" -#include "ErrorState.h" -#include "DrivingState.h" -#include -#include -#include - -/****************************************************************************** - * 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) -{ - IBoard& board = Board::getInstance(); - ISettings& settings = board.getSettings(); - int16_t maxSpeed = settings.getMaxSpeed(); - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - - /* Load the max. motor speed always from the settings, because - * it may happen that there was no calibration before. - */ - diffDrive.setMaxMotorSpeed(maxSpeed); - diffDrive.enable(); - - if (0 == maxSpeed) - { - /* Missing calibration data. Run AppCalib first. */ - ErrorState::getInstance().setErrorMsg("MCAL=0"); - sm.setState(&ErrorState::getInstance()); - } - else if (true == m_initialDataSet) - { - sm.setState(&DrivingState::getInstance()); - } -} - -void StartupState::exit() -{ - /* Nothing to do */ -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Startup state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "StartupState.h" +#include "ErrorState.h" +#include "DrivingState.h" +#include +#include +#include + +/****************************************************************************** + * 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) +{ + IBoard& board = Board::getInstance(); + ISettings& settings = board.getSettings(); + int16_t maxSpeed = settings.getMaxSpeed(); + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + + /* Load the max. motor speed always from the settings, because + * it may happen that there was no calibration before. + */ + diffDrive.setMaxMotorSpeed(maxSpeed); + diffDrive.enable(); + + if (0 == maxSpeed) + { + /* Missing calibration data. Run AppCalib first. */ + ErrorState::getInstance().setErrorMsg("MCAL=0"); + sm.setState(&ErrorState::getInstance()); + } + else if (true == m_initialDataSet) + { + sm.setState(&DrivingState::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 9fe4970f..b8d40db3 100644 --- a/lib/APPRemoteControl/StartupState.h +++ b/lib/APPRemoteControl/StartupState.h @@ -1,136 +1,136 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Startup state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef STARTUP_STATE_H -#define STARTUP_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include - -/****************************************************************************** - * 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; - - /** - * Notify the state, that the initial data set was received. - */ - void notifyInitialDataIsSet() - { - m_initialDataSet = true; - } - -protected: -private: - /** - * Duration in ms how long the application name shall be shown at startup. - */ - static const uint32_t APP_NAME_DURATION = 2000U; - - /** - * Flag to indicate, that the initial data was set. - */ - bool m_initialDataSet; - - /** - * Default constructor. - */ - StartupState() : m_initialDataSet(false) - { - } - - /** - * 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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Startup state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef STARTUP_STATE_H +#define STARTUP_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include + +/****************************************************************************** + * 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; + + /** + * Notify the state, that the initial data set was received. + */ + void notifyInitialDataIsSet() + { + m_initialDataSet = true; + } + +protected: +private: + /** + * Duration in ms how long the application name shall be shown at startup. + */ + static const uint32_t APP_NAME_DURATION = 2000U; + + /** + * Flag to indicate, that the initial data was set. + */ + bool m_initialDataSet; + + /** + * Default constructor. + */ + StartupState() : m_initialDataSet(false) + { + } + + /** + * 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 efcce554..490e3052 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 9f39468d..254d17e5 100644 --- a/lib/APPSensorFusion/App.cpp +++ b/lib/APPSensorFusion/App.cpp @@ -1,178 +1,178 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief SensorFusion application - * @author Juliane Kerpe - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "App.h" -#include "StartupState.h" -#include -#include -#include -#include -#include -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void App::setup() -{ - Serial.begin(SERIAL_BAUDRATE); - Logging::disable(); - - /* Initialize HAL */ - Board::getInstance().init(); - - /* Setup the state machine with the first state. */ - m_systemStateMachine.setState(&StartupState::getInstance()); - - /* Setup the periodically processing of robot control. */ - m_controlInterval.start(DIFFERENTIAL_DRIVE_CONTROL_PERIOD); - - /* Periodically send Send Data via SerialMuxProt. */ - m_sendSensorDataInterval.start(SEND_SENSOR_DATA_PERIOD); - - /* Providing Sensor data */ - m_smpChannelIdSensorData = m_smpServer.createChannel(SENSORDATA_CHANNEL_NAME, SENSORDATA_CHANNEL_DLC); -} - -void App::loop() -{ - Board::getInstance().process(); - 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(); - - /* Read the IMU so when the Measurement Timer runs out the Sensor Data can be accessed directly without having - * to wait for the reading. */ - IIMU& imu = Board::getInstance().getIMU(); - imu.readGyro(); - imu.readAccelerometer(); - - m_controlInterval.restart(); - } - - /* Send sensor data periodically if new data is available. */ - if (true == m_sendSensorDataInterval.isTimeout()) - { - m_sendSensorDataInterval.restart(); - - /* Send Sensor Data if the application is currently in the Driving state. */ - if (&DrivingState::getInstance() == m_systemStateMachine.getState()) - { - sendSensorData(); - } - } - - m_systemStateMachine.process(); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -void App::sendSensorData() -{ - 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); - - /* Access the Accelerometer Data (the Accelerometer is read out during the Control Interval Timeout). */ - IMUData accelerationValues; - imu.getAccelerationValues(&accelerationValues); - - /* Access the Gyro Data (the Gyro is read out during the Control Interval Timeout). */ - IMUData turnRates; - 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.turnRate = turnRates.valueZ; - payload.timePeriod = static_cast(SEND_SENSOR_DATA_PERIOD); - - /* Send the sensor data via the SerialMuxProt. */ - (void)m_smpServer.sendData(m_smpChannelIdSensorData, reinterpret_cast(&payload), sizeof(payload)); -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief SensorFusion application + * @author Juliane Kerpe + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "App.h" +#include "StartupState.h" +#include +#include +#include +#include +#include +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void App::setup() +{ + Serial.begin(SERIAL_BAUDRATE); + Logging::disable(); + + /* Initialize HAL */ + Board::getInstance().init(); + + /* Setup the state machine with the first state. */ + m_systemStateMachine.setState(&StartupState::getInstance()); + + /* Setup the periodically processing of robot control. */ + m_controlInterval.start(DIFFERENTIAL_DRIVE_CONTROL_PERIOD); + + /* Periodically send Send Data via SerialMuxProt. */ + m_sendSensorDataInterval.start(SEND_SENSOR_DATA_PERIOD); + + /* Providing Sensor data */ + m_smpChannelIdSensorData = m_smpServer.createChannel(SENSORDATA_CHANNEL_NAME, SENSORDATA_CHANNEL_DLC); +} + +void App::loop() +{ + Board::getInstance().process(); + 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(); + + /* Read the IMU so when the Measurement Timer runs out the Sensor Data can be accessed directly without having + * to wait for the reading. */ + IIMU& imu = Board::getInstance().getIMU(); + imu.readGyro(); + imu.readAccelerometer(); + + m_controlInterval.restart(); + } + + /* Send sensor data periodically if new data is available. */ + if (true == m_sendSensorDataInterval.isTimeout()) + { + m_sendSensorDataInterval.restart(); + + /* Send Sensor Data if the application is currently in the Driving state. */ + if (&DrivingState::getInstance() == m_systemStateMachine.getState()) + { + sendSensorData(); + } + } + + m_systemStateMachine.process(); +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +void App::sendSensorData() +{ + 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); + + /* Access the Accelerometer Data (the Accelerometer is read out during the Control Interval Timeout). */ + IMUData accelerationValues; + imu.getAccelerationValues(&accelerationValues); + + /* Access the Gyro Data (the Gyro is read out during the Control Interval Timeout). */ + IMUData turnRates; + 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.turnRate = turnRates.valueZ; + payload.timePeriod = static_cast(SEND_SENSOR_DATA_PERIOD); + + /* Send the sensor data via the SerialMuxProt. */ + (void)m_smpServer.sendData(m_smpChannelIdSensorData, reinterpret_cast(&payload), sizeof(payload)); +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions *****************************************************************************/ \ No newline at end of file diff --git a/lib/APPSensorFusion/App.h b/lib/APPSensorFusion/App.h index b23c8959..24bb8f95 100644 --- a/lib/APPSensorFusion/App.h +++ b/lib/APPSensorFusion/App.h @@ -1,138 +1,138 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief 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_systemStateMachine(), - m_controlInterval(), - m_sendSensorDataInterval(), - 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_SENSOR_DATA_PERIOD = 25U; - - /** 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; - - /** 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_sendSensorDataInterval; - - /** - * 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(); - - /* 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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief 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_systemStateMachine(), + m_controlInterval(), + m_sendSensorDataInterval(), + 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_SENSOR_DATA_PERIOD = 25U; + + /** 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; + + /** 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_sendSensorDataInterval; + + /** + * 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(); + + /* 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 a876b3aa..15c91fe6 100644 --- a/lib/APPSensorFusion/DrivingState.cpp +++ b/lib/APPSensorFusion/DrivingState.cpp @@ -1,376 +1,376 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Driving state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "DrivingState.h" -#include -#include -#include -#include -#include -#include "ReadyState.h" -#include "ParameterSets.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void DrivingState::entry() -{ - /* Clear the Odometry Position in favor of reproducibility. */ - Odometry::getInstance().clearPosition(); - Odometry::getInstance().setOrientation(FP_PI() / 2); - - 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) - { - 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; - - } - 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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Driving state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "DrivingState.h" +#include +#include +#include +#include +#include +#include "ReadyState.h" +#include "ParameterSets.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void DrivingState::entry() +{ + /* Clear the Odometry Position in favor of reproducibility. */ + Odometry::getInstance().clearPosition(); + Odometry::getInstance().setOrientation(FP_PI() / 2); + + 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) + { + 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; + + } + 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 8b249d3e..d8b64973 100644 --- a/lib/APPSensorFusion/DrivingState.h +++ b/lib/APPSensorFusion/DrivingState.h @@ -1,220 +1,220 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Driving state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef DRIVING_STATE_H -#define DRIVING_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The system driving state. */ -class DrivingState : public IState -{ -public: - /** - * Get state instance. - * - * @return State instance. - */ - static DrivingState& getInstance() - { - static DrivingState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - - /** - * 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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Driving state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef DRIVING_STATE_H +#define DRIVING_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The system driving state. */ +class DrivingState : public IState +{ +public: + /** + * Get state instance. + * + * @return State instance. + */ + static DrivingState& getInstance() + { + static DrivingState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + + /** + * 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 0a24e496..9fdec00a 100644 --- a/lib/APPSensorFusion/ErrorState.cpp +++ b/lib/APPSensorFusion/ErrorState.cpp @@ -1,113 +1,113 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Error state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ErrorState.h" -#include -#include -#include "ReadyState.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(&ReadyState::getInstance()); - } -} - -void ErrorState::exit() -{ - /* Nothing to do. */ -} - -void ErrorState::setErrorMsg(const char* msg) -{ - if (nullptr == msg) - { - m_errorMsg[0] = '\0'; - } - else - { - strncpy(m_errorMsg, msg, ERROR_MSG_SIZE - 1); - m_errorMsg[ERROR_MSG_SIZE - 1] = '\0'; - } -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Error state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ErrorState.h" +#include +#include +#include "ReadyState.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(&ReadyState::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 74095203..0679e9a3 100644 --- a/lib/APPSensorFusion/ErrorState.h +++ b/lib/APPSensorFusion/ErrorState.h @@ -1,134 +1,134 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Error state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef ERROR_STATE_H -#define ERROR_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The system error state. */ -class ErrorState : public IState -{ -public: - /** - * Get state instance. - * - * @return State instance. - */ - static ErrorState& getInstance() - { - static ErrorState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - - /** - * Set error message, which to show on the display. - * - * @param[in] msg Error message - */ - void setErrorMsg(const char* msg); - -protected: -private: - /** - * The error message string size in bytes, which - * includes the terminating character. - */ - static const size_t ERROR_MSG_SIZE = 20; - - char m_errorMsg[ERROR_MSG_SIZE]; /**< Error message, which to show. */ - - /** - * Default constructor. - */ - ErrorState() : m_errorMsg() - { - m_errorMsg[0] = '\0'; - } - - /** - * Default destructor. - */ - ~ErrorState() - { - } - - /* Not allowed. */ - ErrorState(const ErrorState& state); /**< Copy construction of an instance. */ - ErrorState& operator=(const ErrorState& state); /**< Assignment of an instance. */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* ERROR_STATE_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Error state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef ERROR_STATE_H +#define ERROR_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The system error state. */ +class ErrorState : public IState +{ +public: + /** + * Get state instance. + * + * @return State instance. + */ + static ErrorState& getInstance() + { + static ErrorState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + + /** + * Set error message, which to show on the display. + * + * @param[in] msg Error message + */ + void setErrorMsg(const char* msg); + +protected: +private: + /** + * The error message string size in bytes, which + * includes the terminating character. + */ + static const size_t ERROR_MSG_SIZE = 20; + + char m_errorMsg[ERROR_MSG_SIZE]; /**< Error message, which to show. */ + + /** + * Default constructor. + */ + ErrorState() : m_errorMsg() + { + m_errorMsg[0] = '\0'; + } + + /** + * Default destructor. + */ + ~ErrorState() + { + } + + /* Not allowed. */ + ErrorState(const ErrorState& state); /**< Copy construction of an instance. */ + ErrorState& operator=(const ErrorState& state); /**< Assignment of an instance. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* ERROR_STATE_H */ +/** @} */ diff --git a/lib/APPSensorFusion/IBoard.h b/lib/APPSensorFusion/IBoard.h index e9672413..98f52795 100644 --- a/lib/APPSensorFusion/IBoard.h +++ b/lib/APPSensorFusion/IBoard.h @@ -1,159 +1,159 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Board interface, which abstracts the physical board - * @author Andreas Merkle - * - * @addtogroup HALInterfaces - * - * @{ - */ - -#ifndef IBOARD_H -#define IBOARD_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** - * Abstracts the physical board interface. - */ -class IBoard -{ -public: - /** - * Destroys the board interface. - */ - virtual ~IBoard() - { - } - - /** - * Initialize the hardware. - */ - virtual void init() = 0; - - /** - * Get button A driver. - * - * @return Button A driver. - */ - virtual IButton& getButtonA() = 0; - - /** - * Get buzzer driver. - * - * @return Buzzer driver. - */ - virtual IBuzzer& getBuzzer() = 0; - - /** - * Get encoders driver. - * - * @return Encoders driver. - */ - virtual IEncoders& getEncoders() = 0; - - /** - * Get line sensors driver. - * - * @return Line sensor driver. - */ - virtual ILineSensors& getLineSensors() = 0; - - /** - * Get motor driver. - * - * @return Motor driver. - */ - virtual IMotors& getMotors() = 0; - - /** - * Get yellow LED driver. - * - * @return Yellow LED driver. - */ - virtual ILed& getYellowLed() = 0; - - /** - * Get IMU (Inertial Measurement Unit) driver. - * - * @return IMU driver - */ - virtual IIMU& getIMU() = 0; - - /** - * Get settings instance. - * - * @return Settings - */ - virtual ISettings& getSettings() = 0; - - /** - * Process actuators and sensors. - */ - virtual void process() = 0; - -protected: - /** - * Constructs the board interface. - */ - IBoard() - { - } - -private: -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* IBOARD_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Board interface, which abstracts the physical board + * @author Andreas Merkle + * + * @addtogroup HALInterfaces + * + * @{ + */ + +#ifndef IBOARD_H +#define IBOARD_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** + * Abstracts the physical board interface. + */ +class IBoard +{ +public: + /** + * Destroys the board interface. + */ + virtual ~IBoard() + { + } + + /** + * Initialize the hardware. + */ + virtual void init() = 0; + + /** + * Get button A driver. + * + * @return Button A driver. + */ + virtual IButton& getButtonA() = 0; + + /** + * Get buzzer driver. + * + * @return Buzzer driver. + */ + virtual IBuzzer& getBuzzer() = 0; + + /** + * Get encoders driver. + * + * @return Encoders driver. + */ + virtual IEncoders& getEncoders() = 0; + + /** + * Get line sensors driver. + * + * @return Line sensor driver. + */ + virtual ILineSensors& getLineSensors() = 0; + + /** + * Get motor driver. + * + * @return Motor driver. + */ + virtual IMotors& getMotors() = 0; + + /** + * Get yellow LED driver. + * + * @return Yellow LED driver. + */ + virtual ILed& getYellowLed() = 0; + + /** + * Get IMU (Inertial Measurement Unit) driver. + * + * @return IMU driver + */ + virtual IIMU& getIMU() = 0; + + /** + * Get settings instance. + * + * @return Settings + */ + virtual ISettings& getSettings() = 0; + + /** + * Process actuators and sensors. + */ + virtual void process() = 0; + +protected: + /** + * Constructs the board interface. + */ + IBoard() + { + } + +private: +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* IBOARD_H */ +/** @} */ diff --git a/lib/APPSensorFusion/LineSensorsCalibrationState.cpp b/lib/APPSensorFusion/LineSensorsCalibrationState.cpp index d3241506..d5e436eb 100644 --- a/lib/APPSensorFusion/LineSensorsCalibrationState.cpp +++ b/lib/APPSensorFusion/LineSensorsCalibrationState.cpp @@ -1,207 +1,207 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Line sensors calibration state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "LineSensorsCalibrationState.h" -#include -#include -#include -#include -#include -#include "ReadyState.h" -#include "ErrorState.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void LineSensorsCalibrationState::entry() -{ - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - Odometry& odometry = Odometry::getInstance(); - ILineSensors& lineSensors = Board::getInstance().getLineSensors(); - - /* Prepare calibration drive. */ - m_calibrationSpeed = diffDrive.getMaxMotorSpeed() / 3; - m_orientation = odometry.getOrientation(); - - /* Mandatory for each new calibration. */ - lineSensors.resetCalibration(); - - /* Wait some time, before starting the calibration drive. */ - m_phase = PHASE_1_WAIT; - m_timer.start(WAIT_TIME); -} - -void LineSensorsCalibrationState::process(StateMachine& sm) -{ - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - - switch (m_phase) - { - case PHASE_1_WAIT: - if (true == m_timer.isTimeout()) - { - m_phase = PHASE_2_TURN_LEFT; - diffDrive.setLinearSpeed(-m_calibrationSpeed, m_calibrationSpeed); - } - break; - - case PHASE_2_TURN_LEFT: - if (true == turnAndCalibrate(CALIB_ANGLE, true)) - { - m_phase = PHASE_3_TURN_RIGHT; - diffDrive.setLinearSpeed(m_calibrationSpeed, -m_calibrationSpeed); - } - break; - - case PHASE_3_TURN_RIGHT: - if (true == turnAndCalibrate(-CALIB_ANGLE, false)) - { - m_phase = PHASE_4_TURN_ORIG; - diffDrive.setLinearSpeed(-m_calibrationSpeed, m_calibrationSpeed); - } - break; - - case PHASE_4_TURN_ORIG: - if (true == turnAndCalibrate(0, true)) - { - m_phase = PHASE_5_FINISHED; - diffDrive.setLinearSpeed(0, 0); - finishCalibration(sm); - } - break; - - case PHASE_5_FINISHED: - /* fallthrough */ - default: - break; - } -} - -void LineSensorsCalibrationState::exit() -{ - m_timer.stop(); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -bool LineSensorsCalibrationState::turnAndCalibrate(int32_t calibAlpha, bool isGreaterEqual) -{ - ILineSensors& lineSensors = Board::getInstance().getLineSensors(); - Odometry& odometry = Odometry::getInstance(); - int32_t alpha = odometry.getOrientation() - m_orientation; /* [mrad] */ - bool isSuccesful = false; - - /* Continously calibrate the line sensors. */ - lineSensors.calibrate(); - - /* Is the goal that the current angle shall be lower or equal than the destination calibration angle? */ - if (false == isGreaterEqual) - { - /* Is alpha lower or equal than the destination calibration angle? */ - if (calibAlpha >= alpha) - { - isSuccesful = true; - } - } - else - { - /* Is alpha greater or equal than the destination calibration angle? */ - if (calibAlpha <= alpha) - { - isSuccesful = true; - } - } - - return isSuccesful; -} - -void LineSensorsCalibrationState::finishCalibration(StateMachine& sm) -{ - ILineSensors& lineSensors = Board::getInstance().getLineSensors(); - - if (false == lineSensors.isCalibrationSuccessful()) - { - char str[10]; - char valueStr[10]; - - Util::uintToStr(valueStr, sizeof(valueStr), lineSensors.getCalibErrorInfo()); - - strncpy(str, "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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Line sensors calibration state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "LineSensorsCalibrationState.h" +#include +#include +#include +#include +#include +#include "ReadyState.h" +#include "ErrorState.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void LineSensorsCalibrationState::entry() +{ + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + Odometry& odometry = Odometry::getInstance(); + ILineSensors& lineSensors = Board::getInstance().getLineSensors(); + + /* Prepare calibration drive. */ + m_calibrationSpeed = diffDrive.getMaxMotorSpeed() / 3; + m_orientation = odometry.getOrientation(); + + /* Mandatory for each new calibration. */ + lineSensors.resetCalibration(); + + /* Wait some time, before starting the calibration drive. */ + m_phase = PHASE_1_WAIT; + m_timer.start(WAIT_TIME); +} + +void LineSensorsCalibrationState::process(StateMachine& sm) +{ + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + + switch (m_phase) + { + case PHASE_1_WAIT: + if (true == m_timer.isTimeout()) + { + m_phase = PHASE_2_TURN_LEFT; + diffDrive.setLinearSpeed(-m_calibrationSpeed, m_calibrationSpeed); + } + break; + + case PHASE_2_TURN_LEFT: + if (true == turnAndCalibrate(CALIB_ANGLE, true)) + { + m_phase = PHASE_3_TURN_RIGHT; + diffDrive.setLinearSpeed(m_calibrationSpeed, -m_calibrationSpeed); + } + break; + + case PHASE_3_TURN_RIGHT: + if (true == turnAndCalibrate(-CALIB_ANGLE, false)) + { + m_phase = PHASE_4_TURN_ORIG; + diffDrive.setLinearSpeed(-m_calibrationSpeed, m_calibrationSpeed); + } + break; + + case PHASE_4_TURN_ORIG: + if (true == turnAndCalibrate(0, true)) + { + m_phase = PHASE_5_FINISHED; + diffDrive.setLinearSpeed(0, 0); + finishCalibration(sm); + } + break; + + case PHASE_5_FINISHED: + /* fallthrough */ + default: + break; + } +} + +void LineSensorsCalibrationState::exit() +{ + m_timer.stop(); +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +bool LineSensorsCalibrationState::turnAndCalibrate(int32_t calibAlpha, bool isGreaterEqual) +{ + ILineSensors& lineSensors = Board::getInstance().getLineSensors(); + Odometry& odometry = Odometry::getInstance(); + int32_t alpha = odometry.getOrientation() - m_orientation; /* [mrad] */ + bool isSuccesful = false; + + /* Continously calibrate the line sensors. */ + lineSensors.calibrate(); + + /* Is the goal that the current angle shall be lower or equal than the destination calibration angle? */ + if (false == isGreaterEqual) + { + /* Is alpha lower or equal than the destination calibration angle? */ + if (calibAlpha >= alpha) + { + isSuccesful = true; + } + } + else + { + /* Is alpha greater or equal than the destination calibration angle? */ + if (calibAlpha <= alpha) + { + isSuccesful = true; + } + } + + return isSuccesful; +} + +void LineSensorsCalibrationState::finishCalibration(StateMachine& sm) +{ + ILineSensors& lineSensors = Board::getInstance().getLineSensors(); + + if (false == lineSensors.isCalibrationSuccessful()) + { + char str[10]; + char valueStr[10]; + + Util::uintToStr(valueStr, sizeof(valueStr), lineSensors.getCalibErrorInfo()); + + strncpy(str, "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 f59dd3c4..1ee29963 100644 --- a/lib/APPSensorFusion/LineSensorsCalibrationState.h +++ b/lib/APPSensorFusion/LineSensorsCalibrationState.h @@ -1,161 +1,161 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Line sensors calibration state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef LINE_SENSORS_CALIBRATION_STATE_H -#define LINE_SENSORS_CALIBRATION_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The line sensors calibration state. */ -class LineSensorsCalibrationState : public IState -{ -public: - /** - * Get state instance. - * - * @return State instance. - */ - static LineSensorsCalibrationState& getInstance() - { - static LineSensorsCalibrationState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - -protected: -private: - /** Calibration phases */ - enum Phase - { - PHASE_1_WAIT = 0, /**< Wait a short time before starting the calibration. */ - PHASE_2_TURN_LEFT, /**< Turn line sensors left over the line. */ - PHASE_3_TURN_RIGHT, /**< Turn line sensor right over the line. */ - PHASE_4_TURN_ORIG, /**< Turn back to origin. */ - PHASE_5_FINISHED /**< Calibration is finished. */ - }; - - /** - * Duration in ms about to wait, until the calibration drive starts. - */ - static const uint32_t WAIT_TIME = 1000; - - /** - * Calibration turn angle in mrad (corresponds to 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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Line sensors calibration state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef LINE_SENSORS_CALIBRATION_STATE_H +#define LINE_SENSORS_CALIBRATION_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The line sensors calibration state. */ +class LineSensorsCalibrationState : public IState +{ +public: + /** + * Get state instance. + * + * @return State instance. + */ + static LineSensorsCalibrationState& getInstance() + { + static LineSensorsCalibrationState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + +protected: +private: + /** Calibration phases */ + enum Phase + { + PHASE_1_WAIT = 0, /**< Wait a short time before starting the calibration. */ + PHASE_2_TURN_LEFT, /**< Turn line sensors left over the line. */ + PHASE_3_TURN_RIGHT, /**< Turn line sensor right over the line. */ + PHASE_4_TURN_ORIG, /**< Turn back to origin. */ + PHASE_5_FINISHED /**< Calibration is finished. */ + }; + + /** + * Duration in ms about to wait, until the calibration drive starts. + */ + static const uint32_t WAIT_TIME = 1000; + + /** + * Calibration turn angle in mrad (corresponds to 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/ParameterSets.cpp b/lib/APPSensorFusion/ParameterSets.cpp index 84767c76..252b3b3c 100644 --- a/lib/APPSensorFusion/ParameterSets.cpp +++ b/lib/APPSensorFusion/ParameterSets.cpp @@ -1,139 +1,139 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Calibration state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ParameterSets.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void ParameterSets::choose(uint8_t setId) -{ - if (MAX_SETS > setId) - { - m_currentSetId = setId; - } -} - -void ParameterSets::next() -{ - ++m_currentSetId; - m_currentSetId %= MAX_SETS; -} - -uint8_t ParameterSets::getCurrentSetId() const -{ - return m_currentSetId; -} - -const ParameterSets::ParameterSet& ParameterSets::getParameterSet() const -{ - return m_parSets[m_currentSetId]; -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -ParameterSets::ParameterSets() : m_currentSetId(0), m_parSets() -{ - m_parSets[0] = { - "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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Calibration state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ParameterSets.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void ParameterSets::choose(uint8_t setId) +{ + if (MAX_SETS > setId) + { + m_currentSetId = setId; + } +} + +void ParameterSets::next() +{ + ++m_currentSetId; + m_currentSetId %= MAX_SETS; +} + +uint8_t ParameterSets::getCurrentSetId() const +{ + return m_currentSetId; +} + +const ParameterSets::ParameterSet& ParameterSets::getParameterSet() const +{ + return m_parSets[m_currentSetId]; +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +ParameterSets::ParameterSets() : m_currentSetId(0), m_parSets() +{ + m_parSets[0] = { + "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 b8bb6ab8..3053db32 100644 --- a/lib/APPSensorFusion/ParameterSets.h +++ b/lib/APPSensorFusion/ParameterSets.h @@ -1,145 +1,145 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Parameter state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef PARAMETER_SETS_H -#define PARAMETER_SETS_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** Parameter set with different driving configurations. */ -class ParameterSets -{ -public: - /** - * A single parameter set. - */ - struct ParameterSet - { - const char* name; /**< Name of the parameter set */ - int16_t topSpeed; /**< Top speed in steps/s */ - int16_t kPNumerator; /**< Kp numerator value */ - int16_t kPDenominator; /**< Kp denominator value */ - int16_t kINumerator; /**< Ki numerator value */ - int16_t kIDenominator; /**< Ki denominator value */ - int16_t kDNumerator; /**< Kd numerator value */ - int16_t kDDenominator; /**< Kd denominator value */ - }; - - /** - * Get parameter set instance. - * - * @return Parameter set instance. - */ - static ParameterSets& getInstance() - { - static ParameterSets instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * Choose a specific parameter set. - * If a invalid set id is given, nothing changes. - * - * @param[in] setId Parameter set id - */ - void choose(uint8_t setId); - - /** - * Change to next parameter set. - * After the last set, the first will be choosen. - */ - void next(); - - /** - * Get current set id. - * - * @return Parameter set id. - */ - uint8_t getCurrentSetId() const; - - /** - * Get selected parameter set. - * - * @return Parameter set - */ - const ParameterSet& getParameterSet() const; - - /** Max. number of parameter sets. */ - static const uint8_t MAX_SETS = 3U; - -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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Parameter state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef PARAMETER_SETS_H +#define PARAMETER_SETS_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** Parameter set with different driving configurations. */ +class ParameterSets +{ +public: + /** + * A single parameter set. + */ + struct ParameterSet + { + const char* name; /**< Name of the parameter set */ + int16_t topSpeed; /**< Top speed in steps/s */ + int16_t kPNumerator; /**< Kp numerator value */ + int16_t kPDenominator; /**< Kp denominator value */ + int16_t kINumerator; /**< Ki numerator value */ + int16_t kIDenominator; /**< Ki denominator value */ + int16_t kDNumerator; /**< Kd numerator value */ + int16_t kDDenominator; /**< Kd denominator value */ + }; + + /** + * Get parameter set instance. + * + * @return Parameter set instance. + */ + static ParameterSets& getInstance() + { + static ParameterSets instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * Choose a specific parameter set. + * If a invalid set id is given, nothing changes. + * + * @param[in] setId Parameter set id + */ + void choose(uint8_t setId); + + /** + * Change to next parameter set. + * After the last set, the first will be choosen. + */ + void next(); + + /** + * Get current set id. + * + * @return Parameter set id. + */ + uint8_t getCurrentSetId() const; + + /** + * Get selected parameter set. + * + * @return Parameter set + */ + const ParameterSet& getParameterSet() const; + + /** Max. number of parameter sets. */ + static const uint8_t MAX_SETS = 3U; + +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 af211a4a..a0abe913 100644 --- a/lib/APPSensorFusion/ReadyState.cpp +++ b/lib/APPSensorFusion/ReadyState.cpp @@ -1,155 +1,155 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Ready state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ReadyState.h" -#include "Odometry.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() -{ - - /* Clear the Odometry Position in favor of reproducibility. */ - Odometry::getInstance().clearPosition(); - Odometry::getInstance().setOrientation(1570); - 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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Ready state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ReadyState.h" +#include "Odometry.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() +{ + + /* Clear the Odometry Position in favor of reproducibility. */ + Odometry::getInstance().clearPosition(); + Odometry::getInstance().setOrientation(1570); + 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 9e01f1c5..f14b6e64 100644 --- a/lib/APPSensorFusion/ReadyState.h +++ b/lib/APPSensorFusion/ReadyState.h @@ -1,129 +1,129 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Ready state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef READY_STATE_H -#define READY_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The system ready state. */ -class ReadyState : public IState -{ -public: - /** - * Get state instance. - * - * @return State instance. - */ - static ReadyState& getInstance() - { - static ReadyState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - - /** - * Set lap time, which to show on the display. - * - * @param[in] lapTime Lap time in ms - */ - void setLapTime(uint32_t lapTime); - -protected: -private: - SimpleTimer m_timer; /**< Timer used for cyclic debug output. */ - bool m_isLapTimeAvailable; /**< Is set (true), if a lap time is available. */ - uint32_t m_lapTime; /**< Lap time in ms of the last successful driven round. */ - - /** - * 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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Ready state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef READY_STATE_H +#define READY_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The system ready state. */ +class ReadyState : public IState +{ +public: + /** + * Get state instance. + * + * @return State instance. + */ + static ReadyState& getInstance() + { + static ReadyState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + + /** + * Set lap time, which to show on the display. + * + * @param[in] lapTime Lap time in ms + */ + void setLapTime(uint32_t lapTime); + +protected: +private: + SimpleTimer m_timer; /**< Timer used for cyclic debug output. */ + bool m_isLapTimeAvailable; /**< Is set (true), if a lap time is available. */ + uint32_t m_lapTime; /**< Lap time in ms of the last successful driven round. */ + + /** + * 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 56bfb5af..5ff41f5b 100644 --- a/lib/APPSensorFusion/ReleaseTrackState.cpp +++ b/lib/APPSensorFusion/ReleaseTrackState.cpp @@ -1,121 +1,121 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Release track state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ReleaseTrackState.h" -#include -#include -#include "DrivingState.h" -#include "ParameterSets.h" - -/****************************************************************************** - * 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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Release track state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ReleaseTrackState.h" +#include +#include +#include "DrivingState.h" +#include "ParameterSets.h" + +/****************************************************************************** + * 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 0e0ca144..1a92031b 100644 --- a/lib/APPSensorFusion/ReleaseTrackState.h +++ b/lib/APPSensorFusion/ReleaseTrackState.h @@ -1,129 +1,129 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Release track state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef RELEASE_TRACK_STATE_H -#define RELEASE_TRACK_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The system release track state. */ -class ReleaseTrackState : public IState -{ -public: - /** - * Get state instance. - * - * @return State instance. - */ - static ReleaseTrackState& getInstance() - { - static ReleaseTrackState instance; - - /* Singleton idiom to force initialization during first usage. */ - - return instance; - } - - /** - * If the state is entered, this method will called once. - */ - void entry() final; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - void process(StateMachine& sm) final; - - /** - * If the state is left, this method will be called once. - */ - void exit() final; - -protected: -private: - /** Track release timer duration in ms. */ - static const uint32_t TRACK_RELEASE_DURATION = 5000; - - SimpleTimer m_releaseTimer; /**< Track release timer */ - - /** - * 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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Release track state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef RELEASE_TRACK_STATE_H +#define RELEASE_TRACK_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The system release track state. */ +class ReleaseTrackState : public IState +{ +public: + /** + * Get state instance. + * + * @return State instance. + */ + static ReleaseTrackState& getInstance() + { + static ReleaseTrackState instance; + + /* Singleton idiom to force initialization during first usage. */ + + return instance; + } + + /** + * If the state is entered, this method will called once. + */ + void entry() final; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + void process(StateMachine& sm) final; + + /** + * If the state is left, this method will be called once. + */ + void exit() final; + +protected: +private: + /** Track release timer duration in ms. */ + static const uint32_t TRACK_RELEASE_DURATION = 5000; + + SimpleTimer m_releaseTimer; /**< Track release timer */ + + /** + * 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 461355c6..09dba844 100644 --- a/lib/APPSensorFusion/SerialMuxChannels.h +++ b/lib/APPSensorFusion/SerialMuxChannels.h @@ -1,92 +1,92 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - * - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Channel structure definition for the SerialMuxProt. - * @author Gabryel Reyes - */ - -#ifndef SERIAL_MUX_CHANNELS_H_ -#define SERIAL_MUX_CHANNELS_H_ - -/****************************************************************************** - * Includes - *****************************************************************************/ - -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/** Maximum number of SerialMuxProt Channels. */ -#define MAX_CHANNELS (10U) - -/** Name of Channel to send Sensor Data to. */ -#define SENSORDATA_CHANNEL_NAME "SENSOR_DATA" - -/** DLC of Sensordata Channel */ -#define SENSORDATA_CHANNEL_DLC (sizeof(SensorData)) - -/****************************************************************************** - * 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; - - /** 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; - - /** Time passed since the last sensor value in milliseconds. */ - uint16_t timePeriod; -} __attribute__((packed)) SensorData; - - -/****************************************************************************** - * Functions - *****************************************************************************/ - +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Channel structure definition for the SerialMuxProt. + * @author Gabryel Reyes + */ + +#ifndef SERIAL_MUX_CHANNELS_H_ +#define SERIAL_MUX_CHANNELS_H_ + +/****************************************************************************** + * Includes + *****************************************************************************/ + +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/** Maximum number of SerialMuxProt Channels. */ +#define MAX_CHANNELS (10U) + +/** Name of Channel to send Sensor Data to. */ +#define SENSORDATA_CHANNEL_NAME "SENSOR_DATA" + +/** DLC of Sensordata Channel */ +#define SENSORDATA_CHANNEL_DLC (sizeof(SensorData)) + +/****************************************************************************** + * 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; + + /** 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; + + /** Time passed since the last sensor value in milliseconds. */ + uint16_t timePeriod; +} __attribute__((packed)) SensorData; + + +/****************************************************************************** + * 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 57fc1d96..cafe019f 100644 --- a/lib/APPSensorFusion/StartupState.cpp +++ b/lib/APPSensorFusion/StartupState.cpp @@ -1,117 +1,117 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Calibration state - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "StartupState.h" -#include -#include -#include "ErrorState.h" -#include -#include "Sound.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void StartupState::entry() -{ - /* Nothing to do. */ -} - -void StartupState::process(StateMachine& sm) -{ - IBoard& board = Board::getInstance(); - ISettings& settings = board.getSettings(); - int16_t maxMotorSpeed = settings.getMaxSpeed(); - - if (0 == maxMotorSpeed) - { - /* If the max. Speed equals 0, the Motor Speed Calibration is missing. */ - ErrorState::getInstance().setErrorMsg("MCAL=0"); - sm.setState(&ErrorState::getInstance()); - } - else - { - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - - /* With setting the max. motor speed in [steps/s] the differential drive control - * can now be used. - */ - diffDrive.setMaxMotorSpeed(maxMotorSpeed); - - /* Differential drive can now be used. */ - diffDrive.enable(); - sm.setState(&LineSensorsCalibrationState::getInstance()); - } -} - -void StartupState::exit() -{ - /* Nothing to do. */ -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Calibration state + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "StartupState.h" +#include +#include +#include "ErrorState.h" +#include +#include "Sound.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void StartupState::entry() +{ + /* Nothing to do. */ +} + +void StartupState::process(StateMachine& sm) +{ + IBoard& board = Board::getInstance(); + ISettings& settings = board.getSettings(); + int16_t maxMotorSpeed = settings.getMaxSpeed(); + + if (0 == maxMotorSpeed) + { + /* If the max. Speed equals 0, the Motor Speed Calibration is missing. */ + ErrorState::getInstance().setErrorMsg("MCAL=0"); + sm.setState(&ErrorState::getInstance()); + } + else + { + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + + /* With setting the max. motor speed in [steps/s] the differential drive control + * can now be used. + */ + diffDrive.setMaxMotorSpeed(maxMotorSpeed); + + /* Differential drive can now be used. */ + diffDrive.enable(); + sm.setState(&LineSensorsCalibrationState::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 4b8023b0..c09f981e 100644 --- a/lib/APPSensorFusion/StartupState.h +++ b/lib/APPSensorFusion/StartupState.h @@ -1,118 +1,118 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Startup state - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef STARTUP_STATE_H -#define STARTUP_STATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include "LineSensorsCalibrationState.h" -/****************************************************************************** - * 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: - /** - * 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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Startup state + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef STARTUP_STATE_H +#define STARTUP_STATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include "LineSensorsCalibrationState.h" +/****************************************************************************** + * 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: + /** + * 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 be98d623..41865f20 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/APPTest/library.json b/lib/APPTest/library.json index c56661cc..34506001 100644 --- a/lib/APPTest/library.json +++ b/lib/APPTest/library.json @@ -1,24 +1,24 @@ -{ - "name": "APPTest", - "version": "0.1.0", - "description": "Application part used for testing.", - "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": "APPTest", + "version": "0.1.0", + "description": "Application part used for testing.", + "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": "*" } \ No newline at end of file diff --git a/lib/ArduinoNative/Arduino.cpp b/lib/ArduinoNative/Arduino.cpp index 8872f917..78bb9245 100644 --- a/lib/ArduinoNative/Arduino.cpp +++ b/lib/ArduinoNative/Arduino.cpp @@ -1,415 +1,415 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Arduino native - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include "Terminal.h" - -#ifdef UNIT_TEST - -#include - -#else /* UNIT_TEST */ - -#include -#include -#include -#include -#include - -#endif /* UNIT_TEST */ - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -#ifndef UNIT_TEST - -/** This type defines the possible program arguments. */ -typedef struct -{ - const char* robotName; /**< Robot name */ - bool isZumoComSystemEnabled; /**< Is the ZumoComSystem enabled? */ - const char* serialRxChannel; /**< Serial Rx channel */ - const char* serialTxChannel; /**< Serial Tx channel */ - bool verbose; /**< Show verbose information */ - -} PrgArguments; - -#endif /* UNIT_TEST */ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -extern void setup(); -extern void loop(); - -#ifndef UNIT_TEST - -static int handleCommandLineArguments(PrgArguments& prgArguments, int argc, char** argv); -static void showPrgArguments(const PrgArguments& prgArgs); - -#endif /* UNIT_TEST */ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/** Terminal/Console stream. */ -static Terminal gTerminalStream; - -/** Serial driver, used by Arduino applications. */ -Serial_ Serial(gTerminalStream); - -#ifndef UNIT_TEST - -/** - * The maximum duration a simulated time step can have. - * Everything above would cause missbehaviour in the application. - */ -static const int MAX_TIME_STEP = 10; - -/** - * Simulation time handler, used by Arduino functions. - */ -static SimTime* gSimTime = nullptr; - -/** Supported long program arguments. */ -static const struct option LONG_OPTIONS[] = {{"help", no_argument, nullptr, 0}, - {"serialRxCh", required_argument, nullptr, 0}, - {"serialTxCh", required_argument, nullptr, 0}, - {nullptr, no_argument, nullptr, 0}}; /* Marks the end. */ - -/** Program argument default value of the robot name. */ -static const char* PRG_ARG_ROBOT_NAME_DEFAULT = ""; - -/** Program argument default value of the flag whether the ZumoComSystem is enabled or not. */ -static bool PRG_ARG_IS_ZUMO_COM_SYSTEM_ENABLED_DEFAULT = false; - -/** Program argument default value of the serial rx channel. */ -static const char PRG_ARG_SERIAL_RX_CH_DEFAULT[] = "1"; - -/** Program argument default value of the serial tx channel. */ -static const char PRG_ARG_SERIAL_TX_CH_DEFAULT[] = "2"; - -/** Program argument default value of the verbose flag. */ -static bool PRG_ARG_VERBOSE_DEFAULT = false; - -#endif /* UNIT_TEST */ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -#ifdef UNIT_TEST - -extern int main(int argc, char** argv) -{ - setup(); /* Prepare test */ - loop(); /* Run test once */ - - return 0; -} - -extern unsigned long millis() -{ - clock_t now = clock(); - - return (now * 1000UL) / CLOCKS_PER_SEC; -} - -extern void delay(unsigned long ms) -{ - unsigned long timestamp = millis(); - - while ((millis() - timestamp) < ms) - { - ; - } -} - -#else /* UNIT_TEST */ - -extern int main(int argc, char** argv) -{ - int status = 0; - Board& board = Board::getInstance(); - Keyboard& keyboard = board.getKeyboard(); - WebotsSerialDrv* simSerial = board.getSimSerial(); - PrgArguments prgArguments; - - printf("\n*** Radon Ulzer ***\n"); - - /* Remove any buffering from stout and stderr to get the printed information immediately. */ - (void)setvbuf(stdout, NULL, _IONBF, 0); - (void)setvbuf(stderr, NULL, _IONBF, 0); - - status = handleCommandLineArguments(prgArguments, argc, argv); - - if (0 == status) - { - /* Show used arguments only in verbose mode. */ - if (true == prgArguments.verbose) - { - showPrgArguments(prgArguments); - } - - /* It might happen that the user enables the ZumoComSystem, but the - * choosen application doesn't support it. - */ - if ((nullptr == simSerial) && (true == prgArguments.isZumoComSystemEnabled)) - { - printf("Warning: The application doesn't support the ZumoComSystem.\n"); - - prgArguments.isZumoComSystemEnabled = false; - } - - /* Is ZumoComSystem enabled? */ - if ((nullptr != simSerial) && (true == prgArguments.isZumoComSystemEnabled)) - { - /* Set serial rx/tx channels for communication with the ZumoComSystem. */ - simSerial->setRxChannel(atoi(prgArguments.serialRxChannel)); - simSerial->setTxChannel(atoi(prgArguments.serialTxChannel)); - - /* Use the serial interface for ZumoComSystem communication and not for - * logging. - */ - Serial.setStream(*simSerial); - Logging::disable(); - } - - /* Get simulation time handler. It will be used by millis() and delay(). */ - gSimTime = &board.getSimTime(); - } - - if (0 != status) - { - /* Something went wrong previously and was already notified.*/ - ; - } - else if ((0 == gSimTime->getTimeStep()) || (MAX_TIME_STEP < gSimTime->getTimeStep())) - { - printf("Simulation time step is too high!\n"); - printf("This would cause missbehaviour in the application.\n"); - - status = -1; - } - else - { - /** - * Synchronization between the simulation steps and the control steps is done automatically - * by Webots (If the synchronization field in the robot node is set to TRUE). - * For a more detailed explanation see: - * https://cyberbotics.com/doc/reference/robot#synchronous-versus-asynchronous-controllers - */ - - /* Enable all simulation devices. Must be done before any other access to the devices. */ - board.enableSimulationDevices(); - - /* Do one single simulation step to force that all sensors will deliver already data. - * Otherwise e.g. the position sensor will provide NaN. - * This must be done before setup() is called! - * - * Prerequisite: The sensors must be enabled! - */ - if (false == gSimTime->step()) - { - printf("Very first simulation step failed.\n"); - status = -1; - } - else - { - /* Call the setup function, which is the entry point of the Arduino application. */ - setup(); - - while (true == gSimTime->step()) - { - keyboard.getPressedButtons(); - loop(); - } - } - } - - return status; -} - -extern unsigned long millis() -{ - return gSimTime->getElapsedTimeSinceReset(); -} - -extern void delay(unsigned long ms) -{ - unsigned long timestamp = millis(); - - while ((millis() - timestamp) < ms) - { - if (false == gSimTime->step()) - { - break; - } - } -} - -/****************************************************************************** - * Local Functions - *****************************************************************************/ - -/** - * Handle the arguments passed to the programm. - * If a argument is not given via command line interface, its default value will be used. - * - * @param[out] prgArguments Parsed program arguments - * @param[in] argc Program argument count - * @param[in] argv Program argument vector - * - * @returns 0 if handling was succesful. Otherwise, -1 - */ -static int handleCommandLineArguments(PrgArguments& prgArguments, int argc, char** argv) -{ - int status = 0; - const char* availableOptions = "n:cnvh"; - const char* programName = argv[0]; - int optionIndex = 0; - int option = getopt_long(argc, argv, availableOptions, LONG_OPTIONS, &optionIndex); - - /* Set default values */ - prgArguments.robotName = PRG_ARG_ROBOT_NAME_DEFAULT; - prgArguments.isZumoComSystemEnabled = PRG_ARG_IS_ZUMO_COM_SYSTEM_ENABLED_DEFAULT; - prgArguments.serialRxChannel = PRG_ARG_SERIAL_RX_CH_DEFAULT; - prgArguments.serialTxChannel = PRG_ARG_SERIAL_TX_CH_DEFAULT; - prgArguments.verbose = PRG_ARG_VERBOSE_DEFAULT; - - while ((-1 != option) && (0 == status)) - { - switch (option) - { - case 0: /* Long option */ - - if (0 == strcmp(LONG_OPTIONS[optionIndex].name, "help")) - { - status = -1; - } - else if (0 == strcmp(LONG_OPTIONS[optionIndex].name, "serialRxCh")) - { - prgArguments.serialRxChannel = optarg; - } - else if (0 == strcmp(LONG_OPTIONS[optionIndex].name, "serialTxCh")) - { - prgArguments.serialTxChannel = optarg; - } - else - { - status = -1; - } - break; - - case 'c': /* Is ZumoComSystem enabled? */ - prgArguments.isZumoComSystemEnabled = true; - break; - - case 'n': /* Name */ - prgArguments.robotName = optarg; - break; - - case 'v': /* Verbose */ - prgArguments.verbose = true; - break; - - case '?': /* Unknown */ - /* fallthrough */ - - case 'h': /* Help */ - /* fallthrough */ - - default: /* Default */ - status = -1; - break; - } - - option = getopt_long(argc, argv, availableOptions, LONG_OPTIONS, &optionIndex); - } - - /* Does the user need help? */ - if (0 > status) - { - printf("Usage: %s \nOptions:\n", programName); - printf("\t-h\t\t\tShow this help message.\n"); /* Help */ - printf("\t-n \t\tSet robot name.\n"); /* Robot Name */ - printf("\t-c\t\t\tEnable ZumoComSystem. Default: Disabled\n"); /* Flag */ - printf("\t--serialRxCh \t\tSet serial rx channel (ZumoComSystem)."); /* Serial rx channel */ - printf(" Default: %s\n", PRG_ARG_SERIAL_RX_CH_DEFAULT); /* Serial rx channel default value */ - printf("\t--serialTxCh \t\tSet serial tx channel (ZumoComSystem)."); /* Serial txchannel */ - printf(" Default: %s\n", PRG_ARG_SERIAL_TX_CH_DEFAULT); /* Serial tx channel default value */ - printf("\t-v\t\t\tVerbose mode. Default: Disabled\n"); /* Flag */ - } - - return status; -} - -/** - * Show program arguments on the console. - * - * @param[in] prgArgs Program arguments - */ -static void showPrgArguments(const PrgArguments& prgArgs) -{ - printf("Robot name : %s\n", prgArgs.robotName); - printf("ZumoComSystem : %s\n", (false == prgArgs.isZumoComSystemEnabled) ? "disabled" : "enabled"); - printf("Serial rx channel: %s\n", prgArgs.serialRxChannel); - printf("Serial tx channel: %s\n", prgArgs.serialTxChannel); - /* Skip verbose flag. */ -} - +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Arduino native + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include "Terminal.h" + +#ifdef UNIT_TEST + +#include + +#else /* UNIT_TEST */ + +#include +#include +#include +#include +#include + +#endif /* UNIT_TEST */ + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +#ifndef UNIT_TEST + +/** This type defines the possible program arguments. */ +typedef struct +{ + const char* robotName; /**< Robot name */ + bool isZumoComSystemEnabled; /**< Is the ZumoComSystem enabled? */ + const char* serialRxChannel; /**< Serial Rx channel */ + const char* serialTxChannel; /**< Serial Tx channel */ + bool verbose; /**< Show verbose information */ + +} PrgArguments; + +#endif /* UNIT_TEST */ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +extern void setup(); +extern void loop(); + +#ifndef UNIT_TEST + +static int handleCommandLineArguments(PrgArguments& prgArguments, int argc, char** argv); +static void showPrgArguments(const PrgArguments& prgArgs); + +#endif /* UNIT_TEST */ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/** Terminal/Console stream. */ +static Terminal gTerminalStream; + +/** Serial driver, used by Arduino applications. */ +Serial_ Serial(gTerminalStream); + +#ifndef UNIT_TEST + +/** + * The maximum duration a simulated time step can have. + * Everything above would cause missbehaviour in the application. + */ +static const int MAX_TIME_STEP = 10; + +/** + * Simulation time handler, used by Arduino functions. + */ +static SimTime* gSimTime = nullptr; + +/** Supported long program arguments. */ +static const struct option LONG_OPTIONS[] = {{"help", no_argument, nullptr, 0}, + {"serialRxCh", required_argument, nullptr, 0}, + {"serialTxCh", required_argument, nullptr, 0}, + {nullptr, no_argument, nullptr, 0}}; /* Marks the end. */ + +/** Program argument default value of the robot name. */ +static const char* PRG_ARG_ROBOT_NAME_DEFAULT = ""; + +/** Program argument default value of the flag whether the ZumoComSystem is enabled or not. */ +static bool PRG_ARG_IS_ZUMO_COM_SYSTEM_ENABLED_DEFAULT = false; + +/** Program argument default value of the serial rx channel. */ +static const char PRG_ARG_SERIAL_RX_CH_DEFAULT[] = "1"; + +/** Program argument default value of the serial tx channel. */ +static const char PRG_ARG_SERIAL_TX_CH_DEFAULT[] = "2"; + +/** Program argument default value of the verbose flag. */ +static bool PRG_ARG_VERBOSE_DEFAULT = false; + +#endif /* UNIT_TEST */ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +#ifdef UNIT_TEST + +extern int main(int argc, char** argv) +{ + setup(); /* Prepare test */ + loop(); /* Run test once */ + + return 0; +} + +extern unsigned long millis() +{ + clock_t now = clock(); + + return (now * 1000UL) / CLOCKS_PER_SEC; +} + +extern void delay(unsigned long ms) +{ + unsigned long timestamp = millis(); + + while ((millis() - timestamp) < ms) + { + ; + } +} + +#else /* UNIT_TEST */ + +extern int main(int argc, char** argv) +{ + int status = 0; + Board& board = Board::getInstance(); + Keyboard& keyboard = board.getKeyboard(); + WebotsSerialDrv* simSerial = board.getSimSerial(); + PrgArguments prgArguments; + + printf("\n*** Radon Ulzer ***\n"); + + /* Remove any buffering from stout and stderr to get the printed information immediately. */ + (void)setvbuf(stdout, NULL, _IONBF, 0); + (void)setvbuf(stderr, NULL, _IONBF, 0); + + status = handleCommandLineArguments(prgArguments, argc, argv); + + if (0 == status) + { + /* Show used arguments only in verbose mode. */ + if (true == prgArguments.verbose) + { + showPrgArguments(prgArguments); + } + + /* It might happen that the user enables the ZumoComSystem, but the + * choosen application doesn't support it. + */ + if ((nullptr == simSerial) && (true == prgArguments.isZumoComSystemEnabled)) + { + printf("Warning: The application doesn't support the ZumoComSystem.\n"); + + prgArguments.isZumoComSystemEnabled = false; + } + + /* Is ZumoComSystem enabled? */ + if ((nullptr != simSerial) && (true == prgArguments.isZumoComSystemEnabled)) + { + /* Set serial rx/tx channels for communication with the ZumoComSystem. */ + simSerial->setRxChannel(atoi(prgArguments.serialRxChannel)); + simSerial->setTxChannel(atoi(prgArguments.serialTxChannel)); + + /* Use the serial interface for ZumoComSystem communication and not for + * logging. + */ + Serial.setStream(*simSerial); + Logging::disable(); + } + + /* Get simulation time handler. It will be used by millis() and delay(). */ + gSimTime = &board.getSimTime(); + } + + if (0 != status) + { + /* Something went wrong previously and was already notified.*/ + ; + } + else if ((0 == gSimTime->getTimeStep()) || (MAX_TIME_STEP < gSimTime->getTimeStep())) + { + printf("Simulation time step is too high!\n"); + printf("This would cause missbehaviour in the application.\n"); + + status = -1; + } + else + { + /** + * Synchronization between the simulation steps and the control steps is done automatically + * by Webots (If the synchronization field in the robot node is set to TRUE). + * For a more detailed explanation see: + * https://cyberbotics.com/doc/reference/robot#synchronous-versus-asynchronous-controllers + */ + + /* Enable all simulation devices. Must be done before any other access to the devices. */ + board.enableSimulationDevices(); + + /* Do one single simulation step to force that all sensors will deliver already data. + * Otherwise e.g. the position sensor will provide NaN. + * This must be done before setup() is called! + * + * Prerequisite: The sensors must be enabled! + */ + if (false == gSimTime->step()) + { + printf("Very first simulation step failed.\n"); + status = -1; + } + else + { + /* Call the setup function, which is the entry point of the Arduino application. */ + setup(); + + while (true == gSimTime->step()) + { + keyboard.getPressedButtons(); + loop(); + } + } + } + + return status; +} + +extern unsigned long millis() +{ + return gSimTime->getElapsedTimeSinceReset(); +} + +extern void delay(unsigned long ms) +{ + unsigned long timestamp = millis(); + + while ((millis() - timestamp) < ms) + { + if (false == gSimTime->step()) + { + break; + } + } +} + +/****************************************************************************** + * Local Functions + *****************************************************************************/ + +/** + * Handle the arguments passed to the programm. + * If a argument is not given via command line interface, its default value will be used. + * + * @param[out] prgArguments Parsed program arguments + * @param[in] argc Program argument count + * @param[in] argv Program argument vector + * + * @returns 0 if handling was succesful. Otherwise, -1 + */ +static int handleCommandLineArguments(PrgArguments& prgArguments, int argc, char** argv) +{ + int status = 0; + const char* availableOptions = "n:cnvh"; + const char* programName = argv[0]; + int optionIndex = 0; + int option = getopt_long(argc, argv, availableOptions, LONG_OPTIONS, &optionIndex); + + /* Set default values */ + prgArguments.robotName = PRG_ARG_ROBOT_NAME_DEFAULT; + prgArguments.isZumoComSystemEnabled = PRG_ARG_IS_ZUMO_COM_SYSTEM_ENABLED_DEFAULT; + prgArguments.serialRxChannel = PRG_ARG_SERIAL_RX_CH_DEFAULT; + prgArguments.serialTxChannel = PRG_ARG_SERIAL_TX_CH_DEFAULT; + prgArguments.verbose = PRG_ARG_VERBOSE_DEFAULT; + + while ((-1 != option) && (0 == status)) + { + switch (option) + { + case 0: /* Long option */ + + if (0 == strcmp(LONG_OPTIONS[optionIndex].name, "help")) + { + status = -1; + } + else if (0 == strcmp(LONG_OPTIONS[optionIndex].name, "serialRxCh")) + { + prgArguments.serialRxChannel = optarg; + } + else if (0 == strcmp(LONG_OPTIONS[optionIndex].name, "serialTxCh")) + { + prgArguments.serialTxChannel = optarg; + } + else + { + status = -1; + } + break; + + case 'c': /* Is ZumoComSystem enabled? */ + prgArguments.isZumoComSystemEnabled = true; + break; + + case 'n': /* Name */ + prgArguments.robotName = optarg; + break; + + case 'v': /* Verbose */ + prgArguments.verbose = true; + break; + + case '?': /* Unknown */ + /* fallthrough */ + + case 'h': /* Help */ + /* fallthrough */ + + default: /* Default */ + status = -1; + break; + } + + option = getopt_long(argc, argv, availableOptions, LONG_OPTIONS, &optionIndex); + } + + /* Does the user need help? */ + if (0 > status) + { + printf("Usage: %s \nOptions:\n", programName); + printf("\t-h\t\t\tShow this help message.\n"); /* Help */ + printf("\t-n \t\tSet robot name.\n"); /* Robot Name */ + printf("\t-c\t\t\tEnable ZumoComSystem. Default: Disabled\n"); /* Flag */ + printf("\t--serialRxCh \t\tSet serial rx channel (ZumoComSystem)."); /* Serial rx channel */ + printf(" Default: %s\n", PRG_ARG_SERIAL_RX_CH_DEFAULT); /* Serial rx channel default value */ + printf("\t--serialTxCh \t\tSet serial tx channel (ZumoComSystem)."); /* Serial txchannel */ + printf(" Default: %s\n", PRG_ARG_SERIAL_TX_CH_DEFAULT); /* Serial tx channel default value */ + printf("\t-v\t\t\tVerbose mode. Default: Disabled\n"); /* Flag */ + } + + return status; +} + +/** + * Show program arguments on the console. + * + * @param[in] prgArgs Program arguments + */ +static void showPrgArguments(const PrgArguments& prgArgs) +{ + printf("Robot name : %s\n", prgArgs.robotName); + printf("ZumoComSystem : %s\n", (false == prgArgs.isZumoComSystemEnabled) ? "disabled" : "enabled"); + printf("Serial rx channel: %s\n", prgArgs.serialRxChannel); + printf("Serial tx channel: %s\n", prgArgs.serialTxChannel); + /* Skip verbose flag. */ +} + #endif /* UNIT_TEST */ \ No newline at end of file diff --git a/lib/ArduinoNative/Print.h b/lib/ArduinoNative/Print.h index 2a64c814..2f8e2f50 100644 --- a/lib/ArduinoNative/Print.h +++ b/lib/ArduinoNative/Print.h @@ -1,168 +1,168 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Print class definition - * @author Gabryel Reyes - */ - -#ifndef PRINT_H -#define PRINT_H - -/****************************************************************************** - * Includes - *****************************************************************************/ - -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** - * Class for definition of Print - */ -class Print -{ -public: - /** Destruct Print */ - virtual ~Print() - { - } - - /** - * Print argument. - * @param[in] str Argument to print. - */ - virtual void print(const char str[]) = 0; - - /** - * Print argument. - * @param[in] value Argument to print. - */ - virtual void print(uint8_t value) = 0; - - /** - * Print argument. - * @param[in] value Argument to print. - */ - virtual void print(uint16_t value) = 0; - - /** - * Print argument. - * @param[in] value Argument to print. - */ - virtual void print(uint32_t value) = 0; - - /** - * Print argument. - * @param[in] value Argument to print. - */ - virtual void print(int8_t value) = 0; - - /** - * Print argument. - * @param[in] value Argument to print. - */ - virtual void print(int16_t value) = 0; - - /** - * Print argument. - * @param[in] value Argument to print. - */ - virtual void print(int32_t value) = 0; - - /** - * Print argument. - * Appends Carriage Return at the end of the argument. - * @param[in] str Argument to print. - */ - virtual void println(const char str[]) = 0; - - /** - * Print argument. - * Appends Carriage Return at the end of the argument. - * @param[in] value Argument to print. - */ - virtual void println(uint8_t value) = 0; - - /** - * Print argument. - * Appends Carriage Return at the end of the argument. - * @param[in] value Argument to print. - */ - virtual void println(uint16_t value) = 0; - - /** - * Print argument. - * Appends Carriage Return at the end of the argument. - * @param[in] value Argument to print. - */ - virtual void println(uint32_t value) = 0; - - /** - * Print argument. - * Appends Carriage Return at the end of the argument. - * @param[in] value Argument to print. - */ - virtual void println(int8_t value) = 0; - - /** - * Print argument. - * Appends Carriage Return at the end of the argument. - * @param[in] value Argument to print. - */ - virtual void println(int16_t value) = 0; - - /** - * Print argument. - * Appends Carriage Return at the end of the argument. - * @param[in] value Argument to print. - */ - virtual void println(int32_t value) = 0; - - /** - * Write bytes. - * @param[in] buffer Byte Array to send. - * @param[in] length Length of Buffer. - * @returns Number of bytes written - */ - virtual size_t write(const uint8_t* buffer, size_t length) = 0; - -protected: - /** Construct Print */ - Print() - { - } -}; - -#endif /* PRINT_H_ */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Print class definition + * @author Gabryel Reyes + */ + +#ifndef PRINT_H +#define PRINT_H + +/****************************************************************************** + * Includes + *****************************************************************************/ + +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** + * Class for definition of Print + */ +class Print +{ +public: + /** Destruct Print */ + virtual ~Print() + { + } + + /** + * Print argument. + * @param[in] str Argument to print. + */ + virtual void print(const char str[]) = 0; + + /** + * Print argument. + * @param[in] value Argument to print. + */ + virtual void print(uint8_t value) = 0; + + /** + * Print argument. + * @param[in] value Argument to print. + */ + virtual void print(uint16_t value) = 0; + + /** + * Print argument. + * @param[in] value Argument to print. + */ + virtual void print(uint32_t value) = 0; + + /** + * Print argument. + * @param[in] value Argument to print. + */ + virtual void print(int8_t value) = 0; + + /** + * Print argument. + * @param[in] value Argument to print. + */ + virtual void print(int16_t value) = 0; + + /** + * Print argument. + * @param[in] value Argument to print. + */ + virtual void print(int32_t value) = 0; + + /** + * Print argument. + * Appends Carriage Return at the end of the argument. + * @param[in] str Argument to print. + */ + virtual void println(const char str[]) = 0; + + /** + * Print argument. + * Appends Carriage Return at the end of the argument. + * @param[in] value Argument to print. + */ + virtual void println(uint8_t value) = 0; + + /** + * Print argument. + * Appends Carriage Return at the end of the argument. + * @param[in] value Argument to print. + */ + virtual void println(uint16_t value) = 0; + + /** + * Print argument. + * Appends Carriage Return at the end of the argument. + * @param[in] value Argument to print. + */ + virtual void println(uint32_t value) = 0; + + /** + * Print argument. + * Appends Carriage Return at the end of the argument. + * @param[in] value Argument to print. + */ + virtual void println(int8_t value) = 0; + + /** + * Print argument. + * Appends Carriage Return at the end of the argument. + * @param[in] value Argument to print. + */ + virtual void println(int16_t value) = 0; + + /** + * Print argument. + * Appends Carriage Return at the end of the argument. + * @param[in] value Argument to print. + */ + virtual void println(int32_t value) = 0; + + /** + * Write bytes. + * @param[in] buffer Byte Array to send. + * @param[in] length Length of Buffer. + * @returns Number of bytes written + */ + virtual size_t write(const uint8_t* buffer, size_t length) = 0; + +protected: + /** Construct Print */ + Print() + { + } +}; + +#endif /* PRINT_H_ */ +/** @} */ diff --git a/lib/ArduinoNative/Serial.cpp b/lib/ArduinoNative/Serial.cpp index b1a2176e..38001a6f 100644 --- a/lib/ArduinoNative/Serial.cpp +++ b/lib/ArduinoNative/Serial.cpp @@ -1,171 +1,171 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Implementation of Arduino Serial - * @author Gabryel Reyes - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ - -#include "Serial.h" - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -Serial_::Serial_(Stream& stream) : Stream(), m_stream(&stream) -{ -} - -Serial_::~Serial_() -{ -} - -void Serial_::setStream(Stream& stream) -{ - m_stream = &stream; -} - -void Serial_::begin(unsigned long baudrate) -{ - (void)baudrate; -} - -void Serial_::end() -{ -} - -void Serial_::print(const char str[]) -{ - m_stream->print(str); -} - -void Serial_::print(uint8_t value) -{ - m_stream->print(value); -} - -void Serial_::print(uint16_t value) -{ - m_stream->print(value); -} - -void Serial_::print(uint32_t value) -{ - m_stream->print(value); -} - -void Serial_::print(int8_t value) -{ - m_stream->print(value); -} - -void Serial_::print(int16_t value) -{ - m_stream->print(value); -} - -void Serial_::print(int32_t value) -{ - m_stream->print(value); -} - -void Serial_::println(const char str[]) -{ - m_stream->println(str); -} - -void Serial_::println(uint8_t value) -{ - m_stream->println(value); -} - -void Serial_::println(uint16_t value) -{ - m_stream->println(value); -} - -void Serial_::println(uint32_t value) -{ - m_stream->println(value); -} - -void Serial_::println(int8_t value) -{ - m_stream->println(value); -} - -void Serial_::println(int16_t value) -{ - m_stream->println(value); -} - -void Serial_::println(int32_t value) -{ - m_stream->println(value); -} - -size_t Serial_::write(const uint8_t* buffer, size_t length) -{ - return m_stream->write(buffer, length); -} - -int Serial_::available() const -{ - return m_stream->available(); -} - -size_t Serial_::readBytes(uint8_t* buffer, size_t length) -{ - return m_stream->readBytes(buffer, length); -} - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Implementation of Arduino Serial + * @author Gabryel Reyes + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ + +#include "Serial.h" + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +Serial_::Serial_(Stream& stream) : Stream(), m_stream(&stream) +{ +} + +Serial_::~Serial_() +{ +} + +void Serial_::setStream(Stream& stream) +{ + m_stream = &stream; +} + +void Serial_::begin(unsigned long baudrate) +{ + (void)baudrate; +} + +void Serial_::end() +{ +} + +void Serial_::print(const char str[]) +{ + m_stream->print(str); +} + +void Serial_::print(uint8_t value) +{ + m_stream->print(value); +} + +void Serial_::print(uint16_t value) +{ + m_stream->print(value); +} + +void Serial_::print(uint32_t value) +{ + m_stream->print(value); +} + +void Serial_::print(int8_t value) +{ + m_stream->print(value); +} + +void Serial_::print(int16_t value) +{ + m_stream->print(value); +} + +void Serial_::print(int32_t value) +{ + m_stream->print(value); +} + +void Serial_::println(const char str[]) +{ + m_stream->println(str); +} + +void Serial_::println(uint8_t value) +{ + m_stream->println(value); +} + +void Serial_::println(uint16_t value) +{ + m_stream->println(value); +} + +void Serial_::println(uint32_t value) +{ + m_stream->println(value); +} + +void Serial_::println(int8_t value) +{ + m_stream->println(value); +} + +void Serial_::println(int16_t value) +{ + m_stream->println(value); +} + +void Serial_::println(int32_t value) +{ + m_stream->println(value); +} + +size_t Serial_::write(const uint8_t* buffer, size_t length) +{ + return m_stream->write(buffer, length); +} + +int Serial_::available() const +{ + return m_stream->available(); +} + +size_t Serial_::readBytes(uint8_t* buffer, size_t length) +{ + return m_stream->readBytes(buffer, length); +} + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/ArduinoNative/Serial.h b/lib/ArduinoNative/Serial.h index 29a90514..8427e59c 100644 --- a/lib/ArduinoNative/Serial.h +++ b/lib/ArduinoNative/Serial.h @@ -1,242 +1,242 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Implementation of Arduino Serial - * @author Gabryel Reyes - * - * @addtogroup HAL - * - * @{ - */ - -#ifndef SERIAL_H -#define SERIAL_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ - -#include "Stream.h" - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** Serial driver, used by Arduino applications. */ -class Serial_ : public Stream -{ -public: - /** - * Construct Serial_. - * - * @param[in] stream Output stream. - */ - Serial_(Stream& stream); - - /** - * Destroy Serial_. - */ - ~Serial_(); - - /** - * Set stream where serial data shall be sent to. - * - * @param[in] stream Output stream. - */ - void setStream(Stream& stream); - - /** - * Begin Serial_ communication. - * - * @param[in] baudrate Comm. speed in bits per second - */ - void begin(unsigned long baudrate); - - /** - * End the Serial_ communication. - */ - void end(); - - /** - * Print argument to the output stream. - * - * @param[in] str Argument to print. - */ - void print(const char str[]) final; - - /** - * Print argument to the output stream. - * - * @param[in] value Argument to print. - */ - void print(uint8_t value) final; - - /** - * Print argument to the output stream. - * - * @param[in] value Argument to print. - */ - void print(uint16_t value) final; - - /** - * Print argument to the output stream. - * - * @param[in] value Argument to print. - */ - void print(uint32_t value) final; - - /** - * Print argument to the output stream. - * - * @param[in] value Argument to print. - */ - void print(int8_t value) final; - - /** - * Print argument to the output stream. - * - * @param[in] value Argument to print. - */ - void print(int16_t value) final; - - /** - * Print argument to the output stream. - * - * @param[in] value Argument to print. - */ - void print(int32_t value) final; - - /** - * Print argument to the output stream. - * Appends Carriage Return at the end of the argument. - * - * @param[in] str Argument to print. - */ - void println(const char str[]) final; - - /** - * Print argument to the output stream. - * Appends Carriage Return at the end of the argument. - * - * @param[in] value Argument to print. - */ - void println(uint8_t value) final; - - /** - * Print argument to the output stream. - * Appends Carriage Return at the end of the argument. - * - * @param[in] value Argument to print. - */ - void println(uint16_t value) final; - - /** - * Print argument to the output stream. - * Appends Carriage Return at the end of the argument. - * - * @param[in] value Argument to print. - */ - void println(uint32_t value) final; - - /** - * Print argument to the output stream. - * Appends Carriage Return at the end of the argument. - * - * @param[in] value Argument to print. - */ - void println(int8_t value) final; - - /** - * Print argument to the output stream. - * Appends Carriage Return at the end of the argument. - * - * @param[in] value Argument to print. - */ - void println(int16_t value) final; - - /** - * Print argument to the output stream. - * Appends Carriage Return at the end of the argument. - * - * @param[in] value Argument to print. - */ - void println(int32_t value) final; - - /** - * Write bytes to stream. - * - * @param[in] buffer Byte Array to send. - * @param[in] length Length of Buffer. - * - * @returns Number of bytes written - */ - size_t write(const uint8_t* buffer, size_t length) final; - - /** - * Check if there are available bytes in the Stream. - * - * @returns Number of available bytes. - */ - int available() const final; - - /** - * Read bytes into a buffer. - * - * @param[in] buffer Array to write bytes to. - * @param[in] length number of bytes to be read. - * - * @returns Number of bytes read from Stream. - */ - size_t readBytes(uint8_t* buffer, size_t length) final; - -private: - /** - * Stream for input and output of data. - */ - Stream* m_stream; - - /* Prevent empty constructor*/ - Serial_(); -}; - -/* Serial driver, used by Arduino applications. */ -extern Serial_ Serial; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* SERIAL_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Implementation of Arduino Serial + * @author Gabryel Reyes + * + * @addtogroup HAL + * + * @{ + */ + +#ifndef SERIAL_H +#define SERIAL_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ + +#include "Stream.h" + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** Serial driver, used by Arduino applications. */ +class Serial_ : public Stream +{ +public: + /** + * Construct Serial_. + * + * @param[in] stream Output stream. + */ + Serial_(Stream& stream); + + /** + * Destroy Serial_. + */ + ~Serial_(); + + /** + * Set stream where serial data shall be sent to. + * + * @param[in] stream Output stream. + */ + void setStream(Stream& stream); + + /** + * Begin Serial_ communication. + * + * @param[in] baudrate Comm. speed in bits per second + */ + void begin(unsigned long baudrate); + + /** + * End the Serial_ communication. + */ + void end(); + + /** + * Print argument to the output stream. + * + * @param[in] str Argument to print. + */ + void print(const char str[]) final; + + /** + * Print argument to the output stream. + * + * @param[in] value Argument to print. + */ + void print(uint8_t value) final; + + /** + * Print argument to the output stream. + * + * @param[in] value Argument to print. + */ + void print(uint16_t value) final; + + /** + * Print argument to the output stream. + * + * @param[in] value Argument to print. + */ + void print(uint32_t value) final; + + /** + * Print argument to the output stream. + * + * @param[in] value Argument to print. + */ + void print(int8_t value) final; + + /** + * Print argument to the output stream. + * + * @param[in] value Argument to print. + */ + void print(int16_t value) final; + + /** + * Print argument to the output stream. + * + * @param[in] value Argument to print. + */ + void print(int32_t value) final; + + /** + * Print argument to the output stream. + * Appends Carriage Return at the end of the argument. + * + * @param[in] str Argument to print. + */ + void println(const char str[]) final; + + /** + * Print argument to the output stream. + * Appends Carriage Return at the end of the argument. + * + * @param[in] value Argument to print. + */ + void println(uint8_t value) final; + + /** + * Print argument to the output stream. + * Appends Carriage Return at the end of the argument. + * + * @param[in] value Argument to print. + */ + void println(uint16_t value) final; + + /** + * Print argument to the output stream. + * Appends Carriage Return at the end of the argument. + * + * @param[in] value Argument to print. + */ + void println(uint32_t value) final; + + /** + * Print argument to the output stream. + * Appends Carriage Return at the end of the argument. + * + * @param[in] value Argument to print. + */ + void println(int8_t value) final; + + /** + * Print argument to the output stream. + * Appends Carriage Return at the end of the argument. + * + * @param[in] value Argument to print. + */ + void println(int16_t value) final; + + /** + * Print argument to the output stream. + * Appends Carriage Return at the end of the argument. + * + * @param[in] value Argument to print. + */ + void println(int32_t value) final; + + /** + * Write bytes to stream. + * + * @param[in] buffer Byte Array to send. + * @param[in] length Length of Buffer. + * + * @returns Number of bytes written + */ + size_t write(const uint8_t* buffer, size_t length) final; + + /** + * Check if there are available bytes in the Stream. + * + * @returns Number of available bytes. + */ + int available() const final; + + /** + * Read bytes into a buffer. + * + * @param[in] buffer Array to write bytes to. + * @param[in] length number of bytes to be read. + * + * @returns Number of bytes read from Stream. + */ + size_t readBytes(uint8_t* buffer, size_t length) final; + +private: + /** + * Stream for input and output of data. + */ + Stream* m_stream; + + /* Prevent empty constructor*/ + Serial_(); +}; + +/* Serial driver, used by Arduino applications. */ +extern Serial_ Serial; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* SERIAL_H */ +/** @} */ diff --git a/lib/ArduinoNative/Stream.h b/lib/ArduinoNative/Stream.h index 75a6a136..de1f3cb7 100644 --- a/lib/ArduinoNative/Stream.h +++ b/lib/ArduinoNative/Stream.h @@ -1,83 +1,83 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Data Stream class definition - * @author Gabryel Reyes - */ - -#ifndef STREAM_H -#define STREAM_H - -/****************************************************************************** - * Includes - *****************************************************************************/ - -#include -#include "Print.h" - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** - * Class for definition of data Streams - */ -class Stream : public Print -{ -public: - /** Destruct Stream */ - virtual ~Stream() - { - } - - /** - * Check if there are available bytes in the Stream. - * @returns Number of available bytes. - */ - virtual int available() const = 0; - - /** - * Read bytes into a buffer. - * @param[in] buffer Array to write bytes to. - * @param[in] length number of bytes to be read. - * @returns Number of bytes read from Stream. - */ - virtual size_t readBytes(uint8_t* buffer, size_t length) = 0; - -protected: - /** Construct Stream */ - Stream() : Print() - { - } -}; - -#endif /* STREAM_H_ */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Data Stream class definition + * @author Gabryel Reyes + */ + +#ifndef STREAM_H +#define STREAM_H + +/****************************************************************************** + * Includes + *****************************************************************************/ + +#include +#include "Print.h" + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** + * Class for definition of data Streams + */ +class Stream : public Print +{ +public: + /** Destruct Stream */ + virtual ~Stream() + { + } + + /** + * Check if there are available bytes in the Stream. + * @returns Number of available bytes. + */ + virtual int available() const = 0; + + /** + * Read bytes into a buffer. + * @param[in] buffer Array to write bytes to. + * @param[in] length number of bytes to be read. + * @returns Number of bytes read from Stream. + */ + virtual size_t readBytes(uint8_t* buffer, size_t length) = 0; + +protected: + /** Construct Stream */ + Stream() : Print() + { + } +}; + +#endif /* STREAM_H_ */ +/** @} */ diff --git a/lib/ArduinoNative/Terminal.cpp b/lib/ArduinoNative/Terminal.cpp index 205475d3..5f43f2eb 100644 --- a/lib/ArduinoNative/Terminal.cpp +++ b/lib/ArduinoNative/Terminal.cpp @@ -1,172 +1,172 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Implementation of the Terminal/Console Stream - * @author Gabryel Reyes - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ - -#include -#include "Terminal.h" - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -Terminal::Terminal() : Stream() -{ -} - -Terminal::~Terminal() -{ -} - -void Terminal::print(const char str[]) -{ - printf("%s", str); -} - -void Terminal::print(uint8_t value) -{ - printf("%u", value); -} - -void Terminal::print(uint16_t value) -{ - printf("%u", value); -} - -void Terminal::print(uint32_t value) -{ - printf("%u", value); -} - -void Terminal::print(int8_t value) -{ - printf("%d", value); -} - -void Terminal::print(int16_t value) -{ - printf("%d", value); -} - -void Terminal::print(int32_t value) -{ - printf("%d", value); -} - -void Terminal::println(const char str[]) -{ - printf("%s\n", str); -} - -void Terminal::println(uint8_t value) -{ - printf("%u\n", value); -} - -void Terminal::println(uint16_t value) -{ - printf("%u\n", value); -} - -void Terminal::println(uint32_t value) -{ - printf("%u\n", value); -} - -void Terminal::println(int8_t value) -{ - printf("%d\n", value); -} - -void Terminal::println(int16_t value) -{ - printf("%d\n", value); -} - -void Terminal::println(int32_t value) -{ - printf("%d\n", value); -} - -size_t Terminal::write(const uint8_t* buffer, size_t length) -{ - size_t count = 0; - - if ((nullptr != buffer) && (0U != length)) - { - for (count = 0; count < length; count++) - { - printf("%u", buffer[count]); - } - } - - return count; -} - -int Terminal::available() const -{ - /* Not implemented*/ - return 0; -} - -size_t Terminal::readBytes(uint8_t* buffer, size_t length) -{ - /* Not implemented*/ - (void) buffer; - (void) length; - return 0; -} - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Implementation of the Terminal/Console Stream + * @author Gabryel Reyes + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ + +#include +#include "Terminal.h" + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +Terminal::Terminal() : Stream() +{ +} + +Terminal::~Terminal() +{ +} + +void Terminal::print(const char str[]) +{ + printf("%s", str); +} + +void Terminal::print(uint8_t value) +{ + printf("%u", value); +} + +void Terminal::print(uint16_t value) +{ + printf("%u", value); +} + +void Terminal::print(uint32_t value) +{ + printf("%u", value); +} + +void Terminal::print(int8_t value) +{ + printf("%d", value); +} + +void Terminal::print(int16_t value) +{ + printf("%d", value); +} + +void Terminal::print(int32_t value) +{ + printf("%d", value); +} + +void Terminal::println(const char str[]) +{ + printf("%s\n", str); +} + +void Terminal::println(uint8_t value) +{ + printf("%u\n", value); +} + +void Terminal::println(uint16_t value) +{ + printf("%u\n", value); +} + +void Terminal::println(uint32_t value) +{ + printf("%u\n", value); +} + +void Terminal::println(int8_t value) +{ + printf("%d\n", value); +} + +void Terminal::println(int16_t value) +{ + printf("%d\n", value); +} + +void Terminal::println(int32_t value) +{ + printf("%d\n", value); +} + +size_t Terminal::write(const uint8_t* buffer, size_t length) +{ + size_t count = 0; + + if ((nullptr != buffer) && (0U != length)) + { + for (count = 0; count < length; count++) + { + printf("%u", buffer[count]); + } + } + + return count; +} + +int Terminal::available() const +{ + /* Not implemented*/ + return 0; +} + +size_t Terminal::readBytes(uint8_t* buffer, size_t length) +{ + /* Not implemented*/ + (void) buffer; + (void) length; + return 0; +} + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/ArduinoNative/Terminal.h b/lib/ArduinoNative/Terminal.h index 92cf2901..7f71efb6 100644 --- a/lib/ArduinoNative/Terminal.h +++ b/lib/ArduinoNative/Terminal.h @@ -1,189 +1,189 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Implementation of the Terminal/Console Stream - * @author Gabryel Reyes - * - * @addtogroup HAL - * - * @{ - */ - -#ifndef TERMINAL_H -#define TERMINAL_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ - -#include "Stream.h" - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** Implementation of the Terminal/Console Stream. */ -class Terminal : public Stream -{ -public: - /** - * Construct Terminal. - */ - Terminal(); - - /** - * Destroy Terminal. - */ - ~Terminal(); - - /** - * Print argument to the Output Stream. - * @param[in] str Argument to print. - */ - void print(const char str[]) final; - - /** - * Print argument to the Output Stream. - * @param[in] value Argument to print. - */ - void print(uint8_t value) final; - - /** - * Print argument to the Output Stream. - * @param[in] value Argument to print. - */ - void print(uint16_t value) final; - - /** - * Print argument to the Output Stream. - * @param[in] value Argument to print. - */ - void print(uint32_t value) final; - - /** - * Print argument to the Output Stream. - * @param[in] value Argument to print. - */ - void print(int8_t value) final; - - /** - * Print argument to the Output Stream. - * @param[in] value Argument to print. - */ - void print(int16_t value) final; - - /** - * Print argument to the Output Stream. - * @param[in] value Argument to print. - */ - void print(int32_t value) final; - - /** - * Print argument to the Output Stream. - * Appends Carriage Return at the end of the argument. - * @param[in] str Argument to print. - */ - void println(const char str[]) final; - - /** - * Print argument to the Output Stream. - * Appends Carriage Return at the end of the argument. - * @param[in] value Argument to print. - */ - void println(uint8_t value) final; - - /** - * Print argument to the Output Stream. - * Appends Carriage Return at the end of the argument. - * @param[in] value Argument to print. - */ - void println(uint16_t value) final; - - /** - * Print argument to the Output Stream. - * Appends Carriage Return at the end of the argument. - * @param[in] value Argument to print. - */ - void println(uint32_t value) final; - - /** - * Print argument to the Output Stream. - * Appends Carriage Return at the end of the argument. - * @param[in] value Argument to print. - */ - void println(int8_t value) final; - - /** - * Print argument to the Output Stream. - * Appends Carriage Return at the end of the argument. - * @param[in] value Argument to print. - */ - void println(int16_t value) final; - - /** - * Print argument to the Output Stream. - * Appends Carriage Return at the end of the argument. - * @param[in] value Argument to print. - */ - void println(int32_t value) final; - - /** - * Write bytes to stream. - * @param[in] buffer Byte Array to send. - * @param[in] length Length of Buffer. - * @returns Number of bytes written - */ - size_t write(const uint8_t* buffer, size_t length) final; - - /** - * Check if there are available bytes in the Stream. - * @returns Number of available bytes. - */ - int available() const final; - - /** - * Read bytes into a buffer. - * @param[in] buffer Array to write bytes to. - * @param[in] length number of bytes to be read. - * @returns Number of bytes read from Stream. - */ - size_t readBytes(uint8_t* buffer, size_t length) final; -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Implementation of the Terminal/Console Stream + * @author Gabryel Reyes + * + * @addtogroup HAL + * + * @{ + */ + +#ifndef TERMINAL_H +#define TERMINAL_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ + +#include "Stream.h" + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** Implementation of the Terminal/Console Stream. */ +class Terminal : public Stream +{ +public: + /** + * Construct Terminal. + */ + Terminal(); + + /** + * Destroy Terminal. + */ + ~Terminal(); + + /** + * Print argument to the Output Stream. + * @param[in] str Argument to print. + */ + void print(const char str[]) final; + + /** + * Print argument to the Output Stream. + * @param[in] value Argument to print. + */ + void print(uint8_t value) final; + + /** + * Print argument to the Output Stream. + * @param[in] value Argument to print. + */ + void print(uint16_t value) final; + + /** + * Print argument to the Output Stream. + * @param[in] value Argument to print. + */ + void print(uint32_t value) final; + + /** + * Print argument to the Output Stream. + * @param[in] value Argument to print. + */ + void print(int8_t value) final; + + /** + * Print argument to the Output Stream. + * @param[in] value Argument to print. + */ + void print(int16_t value) final; + + /** + * Print argument to the Output Stream. + * @param[in] value Argument to print. + */ + void print(int32_t value) final; + + /** + * Print argument to the Output Stream. + * Appends Carriage Return at the end of the argument. + * @param[in] str Argument to print. + */ + void println(const char str[]) final; + + /** + * Print argument to the Output Stream. + * Appends Carriage Return at the end of the argument. + * @param[in] value Argument to print. + */ + void println(uint8_t value) final; + + /** + * Print argument to the Output Stream. + * Appends Carriage Return at the end of the argument. + * @param[in] value Argument to print. + */ + void println(uint16_t value) final; + + /** + * Print argument to the Output Stream. + * Appends Carriage Return at the end of the argument. + * @param[in] value Argument to print. + */ + void println(uint32_t value) final; + + /** + * Print argument to the Output Stream. + * Appends Carriage Return at the end of the argument. + * @param[in] value Argument to print. + */ + void println(int8_t value) final; + + /** + * Print argument to the Output Stream. + * Appends Carriage Return at the end of the argument. + * @param[in] value Argument to print. + */ + void println(int16_t value) final; + + /** + * Print argument to the Output Stream. + * Appends Carriage Return at the end of the argument. + * @param[in] value Argument to print. + */ + void println(int32_t value) final; + + /** + * Write bytes to stream. + * @param[in] buffer Byte Array to send. + * @param[in] length Length of Buffer. + * @returns Number of bytes written + */ + size_t write(const uint8_t* buffer, size_t length) final; + + /** + * Check if there are available bytes in the Stream. + * @returns Number of available bytes. + */ + int available() const final; + + /** + * Read bytes into a buffer. + * @param[in] buffer Array to write bytes to. + * @param[in] length number of bytes to be read. + * @returns Number of bytes read from Stream. + */ + size_t readBytes(uint8_t* buffer, size_t length) final; +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + #endif /* TERMINAL_H */ \ No newline at end of file diff --git a/lib/ArduinoNative/library.json b/lib/ArduinoNative/library.json index c6e7ff08..f976272d 100644 --- a/lib/ArduinoNative/library.json +++ b/lib/ArduinoNative/library.json @@ -1,15 +1,15 @@ -{ - "name": "ArduinoNative", - "version": "0.1.0", - "description": "...", - "authors": [{ - "name": "Andreas Merkle", - "email": "web@blue-andi.de", - "url": "https://github.com/BlueAndi", - "maintainer": true - }], - "license": "MIT", - "dependencies": [], - "frameworks": "*", - "platforms": "*" +{ + "name": "ArduinoNative", + "version": "0.1.0", + "description": "...", + "authors": [{ + "name": "Andreas Merkle", + "email": "web@blue-andi.de", + "url": "https://github.com/BlueAndi", + "maintainer": true + }], + "license": "MIT", + "dependencies": [], + "frameworks": "*", + "platforms": "*" } \ No newline at end of file diff --git a/lib/HALCalibSim/Board.cpp b/lib/HALCalibSim/Board.cpp index 21c95b4b..02e594db 100644 --- a/lib/HALCalibSim/Board.cpp +++ b/lib/HALCalibSim/Board.cpp @@ -1,133 +1,133 @@ -/* MIT License - * - * Copyright (c) 2019 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief The simulation robot board realization. - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void Board::init() -{ - m_encoders.init(); - m_lineSensors.init(); - m_motors.init(); - m_proximitySensors.initFrontSensor(); - m_settings.init(); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -Board::Board() : - IBoard(), - 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(RobotDeviceNames::SPEAKER_NAME)), - m_display(m_robot.getDisplay(RobotDeviceNames::DISPLAY_NAME)), - m_encoders(m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME), - m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)), - m_lineSensors( - m_robot.getEmitter(RobotDeviceNames::EMITTER_0_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_1_NAME), - m_robot.getEmitter(RobotDeviceNames::EMITTER_2_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_3_NAME), - m_robot.getEmitter(RobotDeviceNames::EMITTER_4_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)), - m_motors(m_robot.getMotor(RobotDeviceNames::LEFT_MOTOR_NAME), m_robot.getMotor(RobotDeviceNames::RIGHT_MOTOR_NAME)), - m_ledRed(m_robot.getLED(RobotDeviceNames::LED_RED_NAME)), - m_ledYellow(m_robot.getLED(RobotDeviceNames::LED_YELLOW_NAME)), - m_ledGreen(m_robot.getLED(RobotDeviceNames::LED_GREEN_NAME)), - m_proximitySensors(m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_LEFT_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_RIGHT_NAME)), - m_sender(m_robot.getEmitter(RobotDeviceNames::EMITTER_NAME_SERIAL)), - m_settings() -{ -} - -void Board::enableSimulationDevices() -{ - const int timeStep = m_simTime.getTimeStep(); - - m_robot.getKeyboard()->enable(timeStep); - m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME)->enable(timeStep); - m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_LEFT_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_RIGHT_NAME)->enable(timeStep); -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2019 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief The simulation robot board realization. + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void Board::init() +{ + m_encoders.init(); + m_lineSensors.init(); + m_motors.init(); + m_proximitySensors.initFrontSensor(); + m_settings.init(); +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +Board::Board() : + IBoard(), + 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(RobotDeviceNames::SPEAKER_NAME)), + m_display(m_robot.getDisplay(RobotDeviceNames::DISPLAY_NAME)), + m_encoders(m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME), + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)), + m_lineSensors( + m_robot.getEmitter(RobotDeviceNames::EMITTER_0_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_1_NAME), + m_robot.getEmitter(RobotDeviceNames::EMITTER_2_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_3_NAME), + m_robot.getEmitter(RobotDeviceNames::EMITTER_4_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)), + m_motors(m_robot.getMotor(RobotDeviceNames::LEFT_MOTOR_NAME), m_robot.getMotor(RobotDeviceNames::RIGHT_MOTOR_NAME)), + m_ledRed(m_robot.getLED(RobotDeviceNames::LED_RED_NAME)), + m_ledYellow(m_robot.getLED(RobotDeviceNames::LED_YELLOW_NAME)), + m_ledGreen(m_robot.getLED(RobotDeviceNames::LED_GREEN_NAME)), + m_proximitySensors(m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_LEFT_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_RIGHT_NAME)), + m_sender(m_robot.getEmitter(RobotDeviceNames::EMITTER_NAME_SERIAL)), + m_settings() +{ +} + +void Board::enableSimulationDevices() +{ + const int timeStep = m_simTime.getTimeStep(); + + m_robot.getKeyboard()->enable(timeStep); + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME)->enable(timeStep); + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_LEFT_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_RIGHT_NAME)->enable(timeStep); +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALCalibSim/Board.h b/lib/HALCalibSim/Board.h index 7b0100d6..4d31b79f 100644 --- a/lib/HALCalibSim/Board.h +++ b/lib/HALCalibSim/Board.h @@ -1,369 +1,369 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief The simulation robot board realization. - * @author Andreas Merkle - * - * @addtogroup HALSim - * - * @{ - */ -#ifndef BOARD_H -#define BOARD_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** - * The concrete simulation robot board. - */ -class Board : public IBoard -{ -public: - /** - * Get board instance. - * - * @return Board instance - */ - static Board& getInstance() - { - static Board instance; /* idiom */ - - return instance; - } - - /** - * Initialize the hardware. - */ - void init() final; - - /** - * Get button A driver. - * - * @return Button A driver. - */ - IButton& getButtonA() final - { - return m_buttonA; - } - - /** - * Get button B driver. - * - * @return Button B driver. - */ - IButton& getButtonB() final - { - return m_buttonB; - } - - /** - * Get button C driver. - * - * @return Button C driver. - */ - IButton& getButtonC() final - { - return m_buttonC; - } - - /** - * Get buzzer driver. - * - * @return Buzzer driver. - */ - IBuzzer& getBuzzer() final - { - return m_buzzer; - } - - /** - * Get LCD driver. - * - * @return LCD driver. - */ - IDisplay& getDisplay() final - { - return m_display; - } - - /** - * Get encoders. - * - * @return Encoders driver. - */ - IEncoders& getEncoders() final - { - return m_encoders; - } - - /** - * Get line sensors driver. - * - * @return Line sensor driver. - */ - ILineSensors& getLineSensors() final - { - return m_lineSensors; - } - - /** - * Get motor driver. - * - * @return Motor driver. - */ - IMotors& getMotors() final - { - return m_motors; - } - - /** - * Get red LED driver. - * - * @return Red LED driver. - */ - ILed& getRedLed() final - { - return m_ledRed; - } - - /** - * Get yellow LED driver. - * - * @return Yellow LED driver. - */ - ILed& getYellowLed() final - { - return m_ledYellow; - } - - /** - * Get green LED driver. - * - * @return Green LED driver. - */ - ILed& getGreenLed() final - { - return m_ledGreen; - } - - /** - * Get proximity sensors driver. - * - * @return Proximity sensors driver - */ - IProximitySensors& getProximitySensors() final - { - return m_proximitySensors; - } - - /** - * Get sender driver. - * - * @return Sender driver - */ - ISender& getSender() final - { - return m_sender; - } - - /** - * Get settings instance. - * - * @return Settings - */ - ISettings& getSettings() final - { - return m_settings; - } - - /** - * Process actators and sensors. - */ - void process() final - { - m_buzzer.process(); - } - -private: - - /** Simulated roboter instance. */ - webots::Robot m_robot; - - /** Simulation time handler */ - SimTime m_simTime; - - /** Own keyboard that wraps the webots keyboard. */ - Keyboard m_keyboard; - - /** Button A driver */ - ButtonA m_buttonA; - - /** Button B driver */ - ButtonB m_buttonB; - - /** Button C driver */ - ButtonC m_buttonC; - - /** Buzzer driver */ - Buzzer m_buzzer; - - /** Display driver */ - Display m_display; - - /** Encoders driver */ - Encoders m_encoders; - - /** Line sensors driver */ - LineSensors m_lineSensors; - - /** Motors driver */ - Motors m_motors; - - /** Red LED driver */ - LedRed m_ledRed; - - /** Red LED driver */ - LedYellow m_ledYellow; - - /** Red LED driver */ - LedGreen m_ledGreen; - - /** Proximity sensors */ - ProximitySensors m_proximitySensors; - - /** Sender */ - Sender m_sender; - - /** Settings */ - Settings m_settings; - - /** - * Constructs the concrete board. - */ - Board(); - - /** - * Destroys the concrete board. - */ - ~Board() - { - } - - /** - * Get the simulation time handler. - * - * @return Simulation time handler - */ - SimTime& getSimTime() - { - return m_simTime; - } - - /** - * Get the keyboard instance of the simulation. - * - * @return The keyboard. - */ - Keyboard& getKeyboard() - { - return m_keyboard; - } - - /** - * Get the simulation serial driver, which is connected within Webots. - * - * @return If serial driver is available, it will return a pointer to it, otherwise nullptr. - */ - WebotsSerialDrv* getSimSerial() - { - /* Not supported. */ - return nullptr; - } - - /** - * Enable all simulation devices. - * It is called by the main entry only. - * Devices must be enabled before they can be used, and a simulation step must be performed before the application - * initialization. - */ - void enableSimulationDevices(); - - /** - * The main entry needs access to the simulation robot instance. - * But all other application parts shall have no access, which is - * solved by this friend. - * - * @param[in] argc Number of arguments - * @param[in] argv Arguments - * - * @return Exit code - */ - friend int main(int argc, char** argv); -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* BOARD_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief The simulation robot board realization. + * @author Andreas Merkle + * + * @addtogroup HALSim + * + * @{ + */ +#ifndef BOARD_H +#define BOARD_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** + * The concrete simulation robot board. + */ +class Board : public IBoard +{ +public: + /** + * Get board instance. + * + * @return Board instance + */ + static Board& getInstance() + { + static Board instance; /* idiom */ + + return instance; + } + + /** + * Initialize the hardware. + */ + void init() final; + + /** + * Get button A driver. + * + * @return Button A driver. + */ + IButton& getButtonA() final + { + return m_buttonA; + } + + /** + * Get button B driver. + * + * @return Button B driver. + */ + IButton& getButtonB() final + { + return m_buttonB; + } + + /** + * Get button C driver. + * + * @return Button C driver. + */ + IButton& getButtonC() final + { + return m_buttonC; + } + + /** + * Get buzzer driver. + * + * @return Buzzer driver. + */ + IBuzzer& getBuzzer() final + { + return m_buzzer; + } + + /** + * Get LCD driver. + * + * @return LCD driver. + */ + IDisplay& getDisplay() final + { + return m_display; + } + + /** + * Get encoders. + * + * @return Encoders driver. + */ + IEncoders& getEncoders() final + { + return m_encoders; + } + + /** + * Get line sensors driver. + * + * @return Line sensor driver. + */ + ILineSensors& getLineSensors() final + { + return m_lineSensors; + } + + /** + * Get motor driver. + * + * @return Motor driver. + */ + IMotors& getMotors() final + { + return m_motors; + } + + /** + * Get red LED driver. + * + * @return Red LED driver. + */ + ILed& getRedLed() final + { + return m_ledRed; + } + + /** + * Get yellow LED driver. + * + * @return Yellow LED driver. + */ + ILed& getYellowLed() final + { + return m_ledYellow; + } + + /** + * Get green LED driver. + * + * @return Green LED driver. + */ + ILed& getGreenLed() final + { + return m_ledGreen; + } + + /** + * Get proximity sensors driver. + * + * @return Proximity sensors driver + */ + IProximitySensors& getProximitySensors() final + { + return m_proximitySensors; + } + + /** + * Get sender driver. + * + * @return Sender driver + */ + ISender& getSender() final + { + return m_sender; + } + + /** + * Get settings instance. + * + * @return Settings + */ + ISettings& getSettings() final + { + return m_settings; + } + + /** + * Process actators and sensors. + */ + void process() final + { + m_buzzer.process(); + } + +private: + + /** Simulated roboter instance. */ + webots::Robot m_robot; + + /** Simulation time handler */ + SimTime m_simTime; + + /** Own keyboard that wraps the webots keyboard. */ + Keyboard m_keyboard; + + /** Button A driver */ + ButtonA m_buttonA; + + /** Button B driver */ + ButtonB m_buttonB; + + /** Button C driver */ + ButtonC m_buttonC; + + /** Buzzer driver */ + Buzzer m_buzzer; + + /** Display driver */ + Display m_display; + + /** Encoders driver */ + Encoders m_encoders; + + /** Line sensors driver */ + LineSensors m_lineSensors; + + /** Motors driver */ + Motors m_motors; + + /** Red LED driver */ + LedRed m_ledRed; + + /** Red LED driver */ + LedYellow m_ledYellow; + + /** Red LED driver */ + LedGreen m_ledGreen; + + /** Proximity sensors */ + ProximitySensors m_proximitySensors; + + /** Sender */ + Sender m_sender; + + /** Settings */ + Settings m_settings; + + /** + * Constructs the concrete board. + */ + Board(); + + /** + * Destroys the concrete board. + */ + ~Board() + { + } + + /** + * Get the simulation time handler. + * + * @return Simulation time handler + */ + SimTime& getSimTime() + { + return m_simTime; + } + + /** + * Get the keyboard instance of the simulation. + * + * @return The keyboard. + */ + Keyboard& getKeyboard() + { + return m_keyboard; + } + + /** + * Get the simulation serial driver, which is connected within Webots. + * + * @return If serial driver is available, it will return a pointer to it, otherwise nullptr. + */ + WebotsSerialDrv* getSimSerial() + { + /* Not supported. */ + return nullptr; + } + + /** + * Enable all simulation devices. + * It is called by the main entry only. + * Devices must be enabled before they can be used, and a simulation step must be performed before the application + * initialization. + */ + void enableSimulationDevices(); + + /** + * The main entry needs access to the simulation robot instance. + * But all other application parts shall have no access, which is + * solved by this friend. + * + * @param[in] argc Number of arguments + * @param[in] argv Arguments + * + * @return Exit code + */ + friend int main(int argc, char** argv); +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* BOARD_H */ +/** @} */ diff --git a/lib/HALCalibSim/library.json b/lib/HALCalibSim/library.json index bfa78835..54bd5cc4 100644 --- a/lib/HALCalibSim/library.json +++ b/lib/HALCalibSim/library.json @@ -1,21 +1,21 @@ -{ - "name": "HALCalibSim", - "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": "HALCalibSim", + "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/HALCalibTarget/Board.cpp b/lib/HALCalibTarget/Board.cpp index f48e94e1..033b203f 100644 --- a/lib/HALCalibTarget/Board.cpp +++ b/lib/HALCalibTarget/Board.cpp @@ -1,103 +1,103 @@ -/* MIT License - * - * Copyright (c) 2019 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief The physical robot board realization. - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void Board::init() -{ - m_encoders.init(); - m_lineSensors.init(); - m_motors.init(); - m_proximitySensors.initFrontSensor(); - m_settings.init(); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -Board::Board() : - IBoard(), - m_buttonA(), - m_buttonB(), - m_buttonC(), - m_buzzer(), - m_display(), - m_encoders(), - m_lineSensors(), - m_motors(), - m_ledRed(), - m_ledYellow(), - m_ledGreen(), - m_proximitySensors(), - m_sender(), - m_settings() -{ -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2019 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief The physical robot board realization. + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void Board::init() +{ + m_encoders.init(); + m_lineSensors.init(); + m_motors.init(); + m_proximitySensors.initFrontSensor(); + m_settings.init(); +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +Board::Board() : + IBoard(), + m_buttonA(), + m_buttonB(), + m_buttonC(), + m_buzzer(), + m_display(), + m_encoders(), + m_lineSensors(), + m_motors(), + m_ledRed(), + m_ledYellow(), + m_ledGreen(), + m_proximitySensors(), + m_sender(), + m_settings() +{ +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALCalibTarget/Board.h b/lib/HALCalibTarget/Board.h index 0afecf74..a5814960 100644 --- a/lib/HALCalibTarget/Board.h +++ b/lib/HALCalibTarget/Board.h @@ -1,303 +1,303 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief The 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 -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** - * The concrete physical board. - */ -class Board : public IBoard -{ -public: - /** - * Get board instance. - * - * @return Board instance - */ - static Board& getInstance() - { - static Board instance; /* idiom */ - - return instance; - } - - /** - * Initialize the hardware. - */ - void init() final; - - /** - * Get button A driver. - * - * @return Button A driver. - */ - IButton& getButtonA() final - { - return m_buttonA; - } - - /** - * Get button B driver. - * - * @return Button B driver. - */ - IButton& getButtonB() final - { - return m_buttonB; - } - - /** - * Get button C driver. - * - * @return Button C driver. - */ - IButton& getButtonC() final - { - return m_buttonC; - } - - /** - * Get buzzer driver. - * - * @return Buzzer driver. - */ - IBuzzer& getBuzzer() final - { - return m_buzzer; - } - - /** - * Get LCD driver. - * - * @return LCD driver. - */ - IDisplay& getDisplay() final - { - return m_display; - } - - /** - * Get encoders. - * - * @return Encoders driver. - */ - IEncoders& getEncoders() final - { - return m_encoders; - } - - /** - * Get line sensors driver. - * - * @return Line sensor driver. - */ - ILineSensors& getLineSensors() final - { - return m_lineSensors; - } - - /** - * Get motor driver. - * - * @return Motor driver. - */ - IMotors& getMotors() final - { - return m_motors; - } - - /** - * Get red LED driver. - * - * @return Red LED driver. - */ - ILed& getRedLed() final - { - return m_ledRed; - } - - /** - * Get yellow LED driver. - * - * @return Yellow LED driver. - */ - ILed& getYellowLed() final - { - return m_ledYellow; - } - - /** - * Get green LED driver. - * - * @return Green LED driver. - */ - ILed& getGreenLed() final - { - return m_ledGreen; - } - - /** - * Get proximity sensors driver. - * - * @return Proximity sensors driver - */ - IProximitySensors& getProximitySensors() final - { - return m_proximitySensors; - } - - /** - * Get sender driver. - * - * @return Sender driver - */ - ISender& getSender() final - { - return m_sender; - } - - /** - * Get settings instance. - * - * @return Settings - */ - ISettings& getSettings() final - { - return m_settings; - } - - /** - * Process actuators and sensors. - */ - void process() final - { - m_buzzer.process(); - } - -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; - - /** Red LED driver */ - LedYellow m_ledYellow; - - /** Red LED driver */ - LedGreen m_ledGreen; - - /** Proximity sensors */ - ProximitySensors m_proximitySensors; - - /** Sender */ - Sender m_sender; - - /** Settings */ - Settings m_settings; - - /** - * Constructs the concrete board. - */ - Board(); - - /** - * Destroys the concrete board. - */ - ~Board() - { - } -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* BOARD_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief 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 +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** + * The concrete physical board. + */ +class Board : public IBoard +{ +public: + /** + * Get board instance. + * + * @return Board instance + */ + static Board& getInstance() + { + static Board instance; /* idiom */ + + return instance; + } + + /** + * Initialize the hardware. + */ + void init() final; + + /** + * Get button A driver. + * + * @return Button A driver. + */ + IButton& getButtonA() final + { + return m_buttonA; + } + + /** + * Get button B driver. + * + * @return Button B driver. + */ + IButton& getButtonB() final + { + return m_buttonB; + } + + /** + * Get button C driver. + * + * @return Button C driver. + */ + IButton& getButtonC() final + { + return m_buttonC; + } + + /** + * Get buzzer driver. + * + * @return Buzzer driver. + */ + IBuzzer& getBuzzer() final + { + return m_buzzer; + } + + /** + * Get LCD driver. + * + * @return LCD driver. + */ + IDisplay& getDisplay() final + { + return m_display; + } + + /** + * Get encoders. + * + * @return Encoders driver. + */ + IEncoders& getEncoders() final + { + return m_encoders; + } + + /** + * Get line sensors driver. + * + * @return Line sensor driver. + */ + ILineSensors& getLineSensors() final + { + return m_lineSensors; + } + + /** + * Get motor driver. + * + * @return Motor driver. + */ + IMotors& getMotors() final + { + return m_motors; + } + + /** + * Get red LED driver. + * + * @return Red LED driver. + */ + ILed& getRedLed() final + { + return m_ledRed; + } + + /** + * Get yellow LED driver. + * + * @return Yellow LED driver. + */ + ILed& getYellowLed() final + { + return m_ledYellow; + } + + /** + * Get green LED driver. + * + * @return Green LED driver. + */ + ILed& getGreenLed() final + { + return m_ledGreen; + } + + /** + * Get proximity sensors driver. + * + * @return Proximity sensors driver + */ + IProximitySensors& getProximitySensors() final + { + return m_proximitySensors; + } + + /** + * Get sender driver. + * + * @return Sender driver + */ + ISender& getSender() final + { + return m_sender; + } + + /** + * Get settings instance. + * + * @return Settings + */ + ISettings& getSettings() final + { + return m_settings; + } + + /** + * Process actuators and sensors. + */ + void process() final + { + m_buzzer.process(); + } + +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; + + /** Red LED driver */ + LedYellow m_ledYellow; + + /** Red LED driver */ + LedGreen m_ledGreen; + + /** Proximity sensors */ + ProximitySensors m_proximitySensors; + + /** Sender */ + Sender m_sender; + + /** Settings */ + Settings m_settings; + + /** + * Constructs the concrete board. + */ + Board(); + + /** + * Destroys the concrete board. + */ + ~Board() + { + } +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* BOARD_H */ +/** @} */ diff --git a/lib/HALCalibTarget/library.json b/lib/HALCalibTarget/library.json index 34bd4ec7..3e36b565 100644 --- a/lib/HALCalibTarget/library.json +++ b/lib/HALCalibTarget/library.json @@ -1,23 +1,23 @@ -{ - "name": "HALCalibTarget", - "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": "HALCalibTarget", + "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/HALConvoyFollowerSim/Board.cpp b/lib/HALConvoyFollowerSim/Board.cpp index 11ce3e23..00a22737 100644 --- a/lib/HALConvoyFollowerSim/Board.cpp +++ b/lib/HALConvoyFollowerSim/Board.cpp @@ -1,134 +1,134 @@ -/* MIT License - * - * Copyright (c) 2019 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief The simulation robot board realization. - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void Board::init() -{ - m_encoders.init(); - m_lineSensors.init(); - m_motors.init(); - m_proximitySensors.initFrontSensor(); - m_settings.init(); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -Board::Board() : - IBoard(), - 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(RobotDeviceNames::SPEAKER_NAME)), - m_display(m_robot.getDisplay(RobotDeviceNames::DISPLAY_NAME)), - m_encoders(m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME), - m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)), - m_lineSensors( - m_robot.getEmitter(RobotDeviceNames::EMITTER_0_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_1_NAME), - m_robot.getEmitter(RobotDeviceNames::EMITTER_2_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_3_NAME), - m_robot.getEmitter(RobotDeviceNames::EMITTER_4_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)), - m_motors(m_robot.getMotor(RobotDeviceNames::LEFT_MOTOR_NAME), m_robot.getMotor(RobotDeviceNames::RIGHT_MOTOR_NAME)), - m_ledRed(m_robot.getLED(RobotDeviceNames::LED_RED_NAME)), - m_ledYellow(m_robot.getLED(RobotDeviceNames::LED_YELLOW_NAME)), - m_ledGreen(m_robot.getLED(RobotDeviceNames::LED_GREEN_NAME)), - m_proximitySensors(m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_LEFT_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_RIGHT_NAME)), - m_serialDrv(m_robot.getEmitter(RobotDeviceNames::EMITTER_NAME_SERIAL), - m_robot.getReceiver(RobotDeviceNames::RECEIVER_NAME_SERIAL)), - m_settings() -{ -} - -void Board::enableSimulationDevices() -{ - const int timeStep = m_simTime.getTimeStep(); - - m_robot.getKeyboard()->enable(timeStep); - m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME)->enable(timeStep); - m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_LEFT_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_RIGHT_NAME)->enable(timeStep); - m_robot.getReceiver(RobotDeviceNames::RECEIVER_NAME_SERIAL)->enable(timeStep); -} -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions +/* MIT License + * + * Copyright (c) 2019 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief The simulation robot board realization. + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void Board::init() +{ + m_encoders.init(); + m_lineSensors.init(); + m_motors.init(); + m_proximitySensors.initFrontSensor(); + m_settings.init(); +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +Board::Board() : + IBoard(), + 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(RobotDeviceNames::SPEAKER_NAME)), + m_display(m_robot.getDisplay(RobotDeviceNames::DISPLAY_NAME)), + m_encoders(m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME), + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)), + m_lineSensors( + m_robot.getEmitter(RobotDeviceNames::EMITTER_0_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_1_NAME), + m_robot.getEmitter(RobotDeviceNames::EMITTER_2_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_3_NAME), + m_robot.getEmitter(RobotDeviceNames::EMITTER_4_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)), + m_motors(m_robot.getMotor(RobotDeviceNames::LEFT_MOTOR_NAME), m_robot.getMotor(RobotDeviceNames::RIGHT_MOTOR_NAME)), + m_ledRed(m_robot.getLED(RobotDeviceNames::LED_RED_NAME)), + m_ledYellow(m_robot.getLED(RobotDeviceNames::LED_YELLOW_NAME)), + m_ledGreen(m_robot.getLED(RobotDeviceNames::LED_GREEN_NAME)), + m_proximitySensors(m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_LEFT_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_RIGHT_NAME)), + m_serialDrv(m_robot.getEmitter(RobotDeviceNames::EMITTER_NAME_SERIAL), + m_robot.getReceiver(RobotDeviceNames::RECEIVER_NAME_SERIAL)), + m_settings() +{ +} + +void Board::enableSimulationDevices() +{ + const int timeStep = m_simTime.getTimeStep(); + + m_robot.getKeyboard()->enable(timeStep); + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME)->enable(timeStep); + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_LEFT_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_RIGHT_NAME)->enable(timeStep); + m_robot.getReceiver(RobotDeviceNames::RECEIVER_NAME_SERIAL)->enable(timeStep); +} +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions *****************************************************************************/ \ No newline at end of file diff --git a/lib/HALConvoyFollowerSim/library.json b/lib/HALConvoyFollowerSim/library.json index 0fb64394..8f088bee 100644 --- a/lib/HALConvoyFollowerSim/library.json +++ b/lib/HALConvoyFollowerSim/library.json @@ -1,21 +1,21 @@ -{ - "name": "HALConvoyFollowerSim", - "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": "HALConvoyFollowerSim", + "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/HALConvoyFollowerTarget/Board.cpp b/lib/HALConvoyFollowerTarget/Board.cpp index a30cb8e0..39fbe102 100644 --- a/lib/HALConvoyFollowerTarget/Board.cpp +++ b/lib/HALConvoyFollowerTarget/Board.cpp @@ -1,102 +1,102 @@ -/* MIT License - * - * Copyright (c) 2019 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief The physical robot board realization. - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void Board::init() -{ - m_encoders.init(); - m_lineSensors.init(); - m_motors.init(); - m_proximitySensors.initFrontSensor(); - m_settings.init(); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -Board::Board() : - IBoard(), - m_buttonA(), - m_buttonB(), - m_buttonC(), - m_buzzer(), - m_display(), - m_encoders(), - m_lineSensors(), - m_motors(), - m_ledRed(), - m_ledYellow(), - m_ledGreen(), - m_proximitySensors(), - m_settings() -{ -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions +/* MIT License + * + * Copyright (c) 2019 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief The physical robot board realization. + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void Board::init() +{ + m_encoders.init(); + m_lineSensors.init(); + m_motors.init(); + m_proximitySensors.initFrontSensor(); + m_settings.init(); +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +Board::Board() : + IBoard(), + m_buttonA(), + m_buttonB(), + m_buttonC(), + m_buzzer(), + m_display(), + m_encoders(), + m_lineSensors(), + m_motors(), + m_ledRed(), + m_ledYellow(), + m_ledGreen(), + m_proximitySensors(), + m_settings() +{ +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions *****************************************************************************/ \ No newline at end of file diff --git a/lib/HALConvoyFollowerTarget/library.json b/lib/HALConvoyFollowerTarget/library.json index 1fb2be71..33e217af 100644 --- a/lib/HALConvoyFollowerTarget/library.json +++ b/lib/HALConvoyFollowerTarget/library.json @@ -1,23 +1,23 @@ -{ - "name": "HALConvoyFollowerTarget", - "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": "HALConvoyFollowerTarget", + "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/HALConvoyLeaderSim/Board.cpp b/lib/HALConvoyLeaderSim/Board.cpp index b6721de9..3663425d 100644 --- a/lib/HALConvoyLeaderSim/Board.cpp +++ b/lib/HALConvoyLeaderSim/Board.cpp @@ -1,135 +1,135 @@ -/* MIT License - * - * Copyright (c) 2019 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief The simulation robot board realization. - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void Board::init() -{ - m_encoders.init(); - m_lineSensors.init(); - m_motors.init(); - m_proximitySensors.initFrontSensor(); - m_settings.init(); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -Board::Board() : - IBoard(), - 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(RobotDeviceNames::SPEAKER_NAME)), - m_display(m_robot.getDisplay(RobotDeviceNames::DISPLAY_NAME)), - m_encoders(m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME), - m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)), - m_lineSensors( - m_robot.getEmitter(RobotDeviceNames::EMITTER_0_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_1_NAME), - m_robot.getEmitter(RobotDeviceNames::EMITTER_2_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_3_NAME), - m_robot.getEmitter(RobotDeviceNames::EMITTER_4_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)), - m_motors(m_robot.getMotor(RobotDeviceNames::LEFT_MOTOR_NAME), m_robot.getMotor(RobotDeviceNames::RIGHT_MOTOR_NAME)), - m_ledRed(m_robot.getLED(RobotDeviceNames::LED_RED_NAME)), - m_ledYellow(m_robot.getLED(RobotDeviceNames::LED_YELLOW_NAME)), - m_ledGreen(m_robot.getLED(RobotDeviceNames::LED_GREEN_NAME)), - m_proximitySensors(m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_LEFT_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_RIGHT_NAME)), - m_serialDrv(m_robot.getEmitter(RobotDeviceNames::EMITTER_NAME_SERIAL), - m_robot.getReceiver(RobotDeviceNames::RECEIVER_NAME_SERIAL)), - m_settings() -{ -} - -void Board::enableSimulationDevices() -{ - const int timeStep = m_simTime.getTimeStep(); - - m_robot.getKeyboard()->enable(timeStep); - m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME)->enable(timeStep); - m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_LEFT_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_RIGHT_NAME)->enable(timeStep); - m_robot.getReceiver(RobotDeviceNames::RECEIVER_NAME_SERIAL)->enable(timeStep); -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions +/* MIT License + * + * Copyright (c) 2019 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief The simulation robot board realization. + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void Board::init() +{ + m_encoders.init(); + m_lineSensors.init(); + m_motors.init(); + m_proximitySensors.initFrontSensor(); + m_settings.init(); +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +Board::Board() : + IBoard(), + 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(RobotDeviceNames::SPEAKER_NAME)), + m_display(m_robot.getDisplay(RobotDeviceNames::DISPLAY_NAME)), + m_encoders(m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME), + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)), + m_lineSensors( + m_robot.getEmitter(RobotDeviceNames::EMITTER_0_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_1_NAME), + m_robot.getEmitter(RobotDeviceNames::EMITTER_2_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_3_NAME), + m_robot.getEmitter(RobotDeviceNames::EMITTER_4_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)), + m_motors(m_robot.getMotor(RobotDeviceNames::LEFT_MOTOR_NAME), m_robot.getMotor(RobotDeviceNames::RIGHT_MOTOR_NAME)), + m_ledRed(m_robot.getLED(RobotDeviceNames::LED_RED_NAME)), + m_ledYellow(m_robot.getLED(RobotDeviceNames::LED_YELLOW_NAME)), + m_ledGreen(m_robot.getLED(RobotDeviceNames::LED_GREEN_NAME)), + m_proximitySensors(m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_LEFT_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_RIGHT_NAME)), + m_serialDrv(m_robot.getEmitter(RobotDeviceNames::EMITTER_NAME_SERIAL), + m_robot.getReceiver(RobotDeviceNames::RECEIVER_NAME_SERIAL)), + m_settings() +{ +} + +void Board::enableSimulationDevices() +{ + const int timeStep = m_simTime.getTimeStep(); + + m_robot.getKeyboard()->enable(timeStep); + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME)->enable(timeStep); + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_LEFT_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_RIGHT_NAME)->enable(timeStep); + m_robot.getReceiver(RobotDeviceNames::RECEIVER_NAME_SERIAL)->enable(timeStep); +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions *****************************************************************************/ \ No newline at end of file diff --git a/lib/HALConvoyLeaderSim/library.json b/lib/HALConvoyLeaderSim/library.json index e3386ecf..cf950c12 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 a30cb8e0..39fbe102 100644 --- a/lib/HALConvoyLeaderTarget/Board.cpp +++ b/lib/HALConvoyLeaderTarget/Board.cpp @@ -1,102 +1,102 @@ -/* MIT License - * - * Copyright (c) 2019 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief The physical robot board realization. - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void Board::init() -{ - m_encoders.init(); - m_lineSensors.init(); - m_motors.init(); - m_proximitySensors.initFrontSensor(); - m_settings.init(); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -Board::Board() : - IBoard(), - m_buttonA(), - m_buttonB(), - m_buttonC(), - m_buzzer(), - m_display(), - m_encoders(), - m_lineSensors(), - m_motors(), - m_ledRed(), - m_ledYellow(), - m_ledGreen(), - m_proximitySensors(), - m_settings() -{ -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions +/* MIT License + * + * Copyright (c) 2019 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief The physical robot board realization. + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void Board::init() +{ + m_encoders.init(); + m_lineSensors.init(); + m_motors.init(); + m_proximitySensors.initFrontSensor(); + m_settings.init(); +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +Board::Board() : + IBoard(), + m_buttonA(), + m_buttonB(), + m_buttonC(), + m_buzzer(), + m_display(), + m_encoders(), + m_lineSensors(), + m_motors(), + m_ledRed(), + m_ledYellow(), + m_ledGreen(), + m_proximitySensors(), + m_settings() +{ +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions *****************************************************************************/ \ No newline at end of file diff --git a/lib/HALConvoyLeaderTarget/library.json b/lib/HALConvoyLeaderTarget/library.json index 5177e08f..78ecc094 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/IButton.h b/lib/HALInterfaces/IButton.h index ad852a21..11d92b05 100644 --- a/lib/HALInterfaces/IButton.h +++ b/lib/HALInterfaces/IButton.h @@ -1,95 +1,95 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Abstract button interface - * @author Andreas Merkle - * - * @addtogroup HALInterfaces - * - * @{ - */ - -#ifndef IBUTTON_H -#define IBUTTON_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The abstract button interface. */ -class IButton -{ -public: - /** - * Destroys the interface. - */ - virtual ~IButton() - { - } - - /** - * Is button pressed or not - * - * @return If button is pressed, returns true otherwise false. - */ - virtual bool isPressed() = 0; - - /** - * Wait until button is released. - */ - virtual void waitForRelease() = 0; - -protected: - /** - * Constructs the interface. - */ - IButton() - { - } - -private: -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* IBUTTON_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Abstract button interface + * @author Andreas Merkle + * + * @addtogroup HALInterfaces + * + * @{ + */ + +#ifndef IBUTTON_H +#define IBUTTON_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The abstract button interface. */ +class IButton +{ +public: + /** + * Destroys the interface. + */ + virtual ~IButton() + { + } + + /** + * Is button pressed or not + * + * @return If button is pressed, returns true otherwise false. + */ + virtual bool isPressed() = 0; + + /** + * Wait until button is released. + */ + virtual void waitForRelease() = 0; + +protected: + /** + * Constructs the interface. + */ + IButton() + { + } + +private: +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* IBUTTON_H */ +/** @} */ diff --git a/lib/HALInterfaces/IDisplay.h b/lib/HALInterfaces/IDisplay.h index 9b0a8530..d5b9ff78 100644 --- a/lib/HALInterfaces/IDisplay.h +++ b/lib/HALInterfaces/IDisplay.h @@ -1,158 +1,158 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Abstract display interface - * @author Andreas Merkle - * - * @addtogroup HALInterfaces - * - * @{ - */ -#ifndef IDISPLAY_H -#define IDISPLAY_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The abstract display interface. */ -class IDisplay -{ -public: - /** - * Destroys the interface. - */ - virtual ~IDisplay() - { - } - - /** - * Clear the display and set the cursor to the upper left corner. - */ - virtual void clear() = 0; - - /** - * Set the cursor to the given position. - * - * @param[in] xCoord x-coordinate, 0 is the most left position. - * @param[in] yCoord y-coordinate, 0 is the most upper position. - */ - virtual void gotoXY(uint8_t xCoord, uint8_t yCoord) = 0; - - /** - * Print the string to the display at the current cursor position. - * - * @param[in] str String - * - * @return Printed number of characters - */ - virtual size_t print(const char str[]) = 0; - - /** - * Print the unsigned 8-bit value to the display at the current cursor position. - * - * @param[in] value Value - * - * @return Printed number of characters - */ - virtual size_t print(uint8_t value) = 0; - - /** - * Print the unsigned 16-bit value to the display at the current cursor position. - * - * @param[in] value Value - * - * @return Printed number of characters - */ - virtual size_t print(uint16_t value) = 0; - - /** - * Print the unsigned 32-bit value to the display at the current cursor position. - * - * @param[in] value Value - * - * @return Printed number of characters - */ - virtual size_t print(uint32_t value) = 0; - - /** - * Print the signed 8-bit value to the display at the current cursor position. - * - * @param[in] value Value - * - * @return Printed number of characters - */ - virtual size_t print(int8_t value) = 0; - - /** - * Print the signed 16-bit value to the display at the current cursor position. - * - * @param[in] value Value - * - * @return Printed number of characters - */ - virtual size_t print(int16_t value) = 0; - - /** - * Print the signed 32-bit value to the display at the current cursor position. - * - * @param[in] value Value - * - * @return Printed number of characters - */ - virtual size_t print(int32_t value) = 0; - -protected: - /** - * Constructs the interface. - */ - IDisplay() - { - } - -private: -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* IDISPLAY_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Abstract display interface + * @author Andreas Merkle + * + * @addtogroup HALInterfaces + * + * @{ + */ +#ifndef IDISPLAY_H +#define IDISPLAY_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The abstract display interface. */ +class IDisplay +{ +public: + /** + * Destroys the interface. + */ + virtual ~IDisplay() + { + } + + /** + * Clear the display and set the cursor to the upper left corner. + */ + virtual void clear() = 0; + + /** + * Set the cursor to the given position. + * + * @param[in] xCoord x-coordinate, 0 is the most left position. + * @param[in] yCoord y-coordinate, 0 is the most upper position. + */ + virtual void gotoXY(uint8_t xCoord, uint8_t yCoord) = 0; + + /** + * Print the string to the display at the current cursor position. + * + * @param[in] str String + * + * @return Printed number of characters + */ + virtual size_t print(const char str[]) = 0; + + /** + * Print the unsigned 8-bit value to the display at the current cursor position. + * + * @param[in] value Value + * + * @return Printed number of characters + */ + virtual size_t print(uint8_t value) = 0; + + /** + * Print the unsigned 16-bit value to the display at the current cursor position. + * + * @param[in] value Value + * + * @return Printed number of characters + */ + virtual size_t print(uint16_t value) = 0; + + /** + * Print the unsigned 32-bit value to the display at the current cursor position. + * + * @param[in] value Value + * + * @return Printed number of characters + */ + virtual size_t print(uint32_t value) = 0; + + /** + * Print the signed 8-bit value to the display at the current cursor position. + * + * @param[in] value Value + * + * @return Printed number of characters + */ + virtual size_t print(int8_t value) = 0; + + /** + * Print the signed 16-bit value to the display at the current cursor position. + * + * @param[in] value Value + * + * @return Printed number of characters + */ + virtual size_t print(int16_t value) = 0; + + /** + * Print the signed 32-bit value to the display at the current cursor position. + * + * @param[in] value Value + * + * @return Printed number of characters + */ + virtual size_t print(int32_t value) = 0; + +protected: + /** + * Constructs the interface. + */ + IDisplay() + { + } + +private: +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* IDISPLAY_H */ +/** @} */ diff --git a/lib/HALInterfaces/IEncoders.h b/lib/HALInterfaces/IEncoders.h index 171e475a..1f787e8e 100644 --- a/lib/HALInterfaces/IEncoders.h +++ b/lib/HALInterfaces/IEncoders.h @@ -1,133 +1,133 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Abstract encoders interface - * @author Andreas Merkle - * - * @addtogroup HALInterfaces - * - * @{ - */ -#ifndef IENCODERS_H -#define IENCODERS_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The abstract encoder interface. */ -class IEncoders -{ -public: - /** - * Destroys the interface. - */ - virtual ~IEncoders() - { - } - - /** - * Initializes the encoders. - */ - virtual void init() = 0; - - /** - * Returns the number of counts that have been detected from the left-side - * encoder. These counts start at 0. Positive counts correspond to forward - * movement of the left side of the robot, while negative counts correspond - * to backwards movement. - * - * The count is returned as a signed 16-bit integer. When the count goes - * over 32767, it will overflow down to -32768. When the count goes below - * -32768, it will overflow up to 32767. - * - * @return Encoder steps left - */ - virtual int16_t getCountsLeft() = 0; - - /** - * Returns the number of counts that have been detected from the right-side - * encoder. These counts start at 0. Positive counts correspond to forward - * movement of the left side of the robot, while negative counts correspond - * to backwards movement. - * - * The count is returned as a signed 16-bit integer. When the count goes - * over 32767, it will overflow down to -32768. When the count goes below - * -32768, it will overflow up to 32767. - * - * @return Encoder steps right - */ - virtual int16_t getCountsRight() = 0; - - /** - * This function is just like getCountsLeft() except it also clears the - * counts before returning. If you call this frequently enough, you will - * not have to worry about the count overflowing. - * - * @return Encoder steps left - */ - virtual int16_t getCountsAndResetLeft() = 0; - - /** - * This function is just like getCountsRight() except it also clears the - * counts before returning. If you call this frequently enough, you will - * not have to worry about the count overflowing. - * - * @return Encoder steps right - */ - virtual int16_t getCountsAndResetRight() = 0; - -protected: - /** - * Constructs the interface. - */ - IEncoders() - { - } - -private: -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* IENCODERS_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Abstract encoders interface + * @author Andreas Merkle + * + * @addtogroup HALInterfaces + * + * @{ + */ +#ifndef IENCODERS_H +#define IENCODERS_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The abstract encoder interface. */ +class IEncoders +{ +public: + /** + * Destroys the interface. + */ + virtual ~IEncoders() + { + } + + /** + * Initializes the encoders. + */ + virtual void init() = 0; + + /** + * Returns the number of counts that have been detected from the left-side + * encoder. These counts start at 0. Positive counts correspond to forward + * movement of the left side of the robot, while negative counts correspond + * to backwards movement. + * + * The count is returned as a signed 16-bit integer. When the count goes + * over 32767, it will overflow down to -32768. When the count goes below + * -32768, it will overflow up to 32767. + * + * @return Encoder steps left + */ + virtual int16_t getCountsLeft() = 0; + + /** + * Returns the number of counts that have been detected from the right-side + * encoder. These counts start at 0. Positive counts correspond to forward + * movement of the left side of the robot, while negative counts correspond + * to backwards movement. + * + * The count is returned as a signed 16-bit integer. When the count goes + * over 32767, it will overflow down to -32768. When the count goes below + * -32768, it will overflow up to 32767. + * + * @return Encoder steps right + */ + virtual int16_t getCountsRight() = 0; + + /** + * This function is just like getCountsLeft() except it also clears the + * counts before returning. If you call this frequently enough, you will + * not have to worry about the count overflowing. + * + * @return Encoder steps left + */ + virtual int16_t getCountsAndResetLeft() = 0; + + /** + * This function is just like getCountsRight() except it also clears the + * counts before returning. If you call this frequently enough, you will + * not have to worry about the count overflowing. + * + * @return Encoder steps right + */ + virtual int16_t getCountsAndResetRight() = 0; + +protected: + /** + * Constructs the interface. + */ + IEncoders() + { + } + +private: +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* IENCODERS_H */ +/** @} */ diff --git a/lib/HALInterfaces/IIMU.h b/lib/HALInterfaces/IIMU.h index f2db5568..032500d5 100644 --- a/lib/HALInterfaces/IIMU.h +++ b/lib/HALInterfaces/IIMU.h @@ -1,178 +1,178 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief 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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief 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/HALInterfaces/ILed.h b/lib/HALInterfaces/ILed.h index 6dac9a05..6a6dca4b 100644 --- a/lib/HALInterfaces/ILed.h +++ b/lib/HALInterfaces/ILed.h @@ -1,89 +1,89 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Abstract LED interface - * @author Andreas Merkle - * - * @addtogroup HALInterfaces - * - * @{ - */ -#ifndef ILED_H -#define ILED_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The abstract LED interface. */ -class ILed -{ -public: - /** - * Destroys the interface. - */ - virtual ~ILed() - { - } - - /** - * Enables/Disables the LED. - * - * @param[in] enableIt Enable LED with true, disable it with false. - */ - virtual void enable(bool enableIt) = 0; - -protected: - /** - * Constructs the interface. - */ - ILed() - { - } - -private: -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* ILED_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Abstract LED interface + * @author Andreas Merkle + * + * @addtogroup HALInterfaces + * + * @{ + */ +#ifndef ILED_H +#define ILED_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The abstract LED interface. */ +class ILed +{ +public: + /** + * Destroys the interface. + */ + virtual ~ILed() + { + } + + /** + * Enables/Disables the LED. + * + * @param[in] enableIt Enable LED with true, disable it with false. + */ + virtual void enable(bool enableIt) = 0; + +protected: + /** + * Constructs the interface. + */ + ILed() + { + } + +private: +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* ILED_H */ +/** @} */ diff --git a/lib/HALInterfaces/ILineSensors.h b/lib/HALInterfaces/ILineSensors.h index 6613e2e2..a01d7df7 100644 --- a/lib/HALInterfaces/ILineSensors.h +++ b/lib/HALInterfaces/ILineSensors.h @@ -1,170 +1,170 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Abstract line sensors array interface - * @author Andreas Merkle - * - * @addtogroup HALInterfaces - * - * @{ - */ -#ifndef ILINESENSORS_H -#define ILINESENSORS_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The abstract line sensors interface. */ -class ILineSensors -{ -public: - /** - * Destroys the interface. - */ - virtual ~ILineSensors() - { - } - - /** - * Initializes the line sensors. - */ - virtual void init() = 0; - - /** - * Reads the sensors for calibration. Call this method several times during - * turning the sensors over the line to determine the minimum and maximum - * values. - * - * The calibration factors are stored internally. - */ - virtual void calibrate() = 0; - - /** - * Determines the deviation and returns an estimated position of the robot - * with respect to a line. The estimate is made using a weighted average of - * the sensor indices multiplied by 1000, so that a return value of 0 - * indicates that the line is directly below sensor 0, a return value of - * 1000 indicates that the line is directly below sensor 1, 2000 - * indicates that it's below sensor 2000, etc. Intermediate values - * indicate that the line is between two sensors. The formula is: - * - * 0*value0 + 1000*value1 + 2000*value2 + ... - * -------------------------------------------- - * value0 + value1 + value2 + ... - * - * This function assumes a dark line (high values) surrounded by white - * (low values). - * - * @return Estimated position with respect to track. - */ - virtual int16_t readLine() = 0; - - /** - * Get last line sensor values. - * - * @return Line sensor values - */ - virtual const uint16_t* getSensorValues() = 0; - - /** - * Checks whether the calibration was successful or not. - * It assumes that the environment brightness compensation is active. - * - * @return If successful, it will return true otherwise false. - */ - virtual bool isCalibrationSuccessful() = 0; - - /** - * It will return the index of the sensor, which caused to fail the calibration. - * If calibration was successful, it will return 0xFF. - * If calibration was not not done yet, it will return 0xFE. - * - * @return Sensor index, starting with 0. Note the other cases in description. - */ - virtual uint8_t getCalibErrorInfo() const = 0; - - /** - * Get number of used line sensors. - * - * @return Number of used line sensors - */ - virtual uint8_t getNumLineSensors() const = 0; - - /** - * Get max. value of a single line sensor in digits. - * The sensor value is indirect proportional to the reflectance. - * - * @return Max. line sensor value - */ - virtual int16_t getSensorValueMax() const = 0; - - /** - * Resets the maximum and minimum values measured by each sensor. - */ - virtual void resetCalibration() = 0; - - /** - * Calibration error information: Calibration successful. - */ - static const uint8_t CALIB_ERROR_OK = 0xFF; - - /** - * Calibration error information: Calibration not done yet. - */ - static const uint8_t CALIB_ERROR_NOT_CALIBRATED = 0xFE; - -protected: - /** - * Constructs the interface. - */ - ILineSensors() - { - } - -private: -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* ILINESENSORS_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Abstract line sensors array interface + * @author Andreas Merkle + * + * @addtogroup HALInterfaces + * + * @{ + */ +#ifndef ILINESENSORS_H +#define ILINESENSORS_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The abstract line sensors interface. */ +class ILineSensors +{ +public: + /** + * Destroys the interface. + */ + virtual ~ILineSensors() + { + } + + /** + * Initializes the line sensors. + */ + virtual void init() = 0; + + /** + * Reads the sensors for calibration. Call this method several times during + * turning the sensors over the line to determine the minimum and maximum + * values. + * + * The calibration factors are stored internally. + */ + virtual void calibrate() = 0; + + /** + * Determines the deviation and returns an estimated position of the robot + * with respect to a line. The estimate is made using a weighted average of + * the sensor indices multiplied by 1000, so that a return value of 0 + * indicates that the line is directly below sensor 0, a return value of + * 1000 indicates that the line is directly below sensor 1, 2000 + * indicates that it's below sensor 2000, etc. Intermediate values + * indicate that the line is between two sensors. The formula is: + * + * 0*value0 + 1000*value1 + 2000*value2 + ... + * -------------------------------------------- + * value0 + value1 + value2 + ... + * + * This function assumes a dark line (high values) surrounded by white + * (low values). + * + * @return Estimated position with respect to track. + */ + virtual int16_t readLine() = 0; + + /** + * Get last line sensor values. + * + * @return Line sensor values + */ + virtual const uint16_t* getSensorValues() = 0; + + /** + * Checks whether the calibration was successful or not. + * It assumes that the environment brightness compensation is active. + * + * @return If successful, it will return true otherwise false. + */ + virtual bool isCalibrationSuccessful() = 0; + + /** + * It will return the index of the sensor, which caused to fail the calibration. + * If calibration was successful, it will return 0xFF. + * If calibration was not not done yet, it will return 0xFE. + * + * @return Sensor index, starting with 0. Note the other cases in description. + */ + virtual uint8_t getCalibErrorInfo() const = 0; + + /** + * Get number of used line sensors. + * + * @return Number of used line sensors + */ + virtual uint8_t getNumLineSensors() const = 0; + + /** + * Get max. value of a single line sensor in digits. + * The sensor value is indirect proportional to the reflectance. + * + * @return Max. line sensor value + */ + virtual int16_t getSensorValueMax() const = 0; + + /** + * Resets the maximum and minimum values measured by each sensor. + */ + virtual void resetCalibration() = 0; + + /** + * Calibration error information: Calibration successful. + */ + static const uint8_t CALIB_ERROR_OK = 0xFF; + + /** + * Calibration error information: Calibration not done yet. + */ + static const uint8_t CALIB_ERROR_NOT_CALIBRATED = 0xFE; + +protected: + /** + * Constructs the interface. + */ + ILineSensors() + { + } + +private: +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* ILINESENSORS_H */ +/** @} */ diff --git a/lib/HALInterfaces/IMotors.h b/lib/HALInterfaces/IMotors.h index 8b2f8938..3648715b 100644 --- a/lib/HALInterfaces/IMotors.h +++ b/lib/HALInterfaces/IMotors.h @@ -1,120 +1,120 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Abstract motors interface - * @author Andreas Merkle - * - * @addtogroup HALInterfaces - * - * @{ - */ -#ifndef IMOTORS_H -#define IMOTORS_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The abstract motors interface. */ -class IMotors -{ -public: - /** - * Destroys the interface. - */ - virtual ~IMotors() - { - } - - /** - * Initializes the motors. - */ - virtual void init() = 0; - - /** - * Sets the speeds for both motors. - * - * @param[in] leftSpeed A number from -400 to 400 representing the speed and - * direction of the right motor. Values of -400 or less result in full speed - * reverse, and values of 400 or more result in full speed forward. - * @param[in] rightSpeed A number from -400 to 400 representing the speed and - * direction of the right motor. Values of -400 or less result in full speed - * reverse, and values of 400 or more result in full speed forward. - */ - virtual void setSpeeds(int16_t leftSpeed, int16_t rightSpeed) = 0; - - /** - * Get maximum speed of the motors in digits. - * - * @return Max. speed in digits - */ - virtual int16_t getMaxSpeed() const = 0; - - /** - * Get the current speed of the left motor. - * - * @return The speed of the left motor in digits. - */ - virtual int16_t getLeftSpeed() = 0; - - /** - * Get the current speed of the right motor. - * - * @return The speed of the right motor in digits. - */ - virtual int16_t getRightSpeed() = 0; - -protected: - /** - * Constructs the interface. - */ - IMotors() - { - } - -private: -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* IMOTORS_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Abstract motors interface + * @author Andreas Merkle + * + * @addtogroup HALInterfaces + * + * @{ + */ +#ifndef IMOTORS_H +#define IMOTORS_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The abstract motors interface. */ +class IMotors +{ +public: + /** + * Destroys the interface. + */ + virtual ~IMotors() + { + } + + /** + * Initializes the motors. + */ + virtual void init() = 0; + + /** + * Sets the speeds for both motors. + * + * @param[in] leftSpeed A number from -400 to 400 representing the speed and + * direction of the right motor. Values of -400 or less result in full speed + * reverse, and values of 400 or more result in full speed forward. + * @param[in] rightSpeed A number from -400 to 400 representing the speed and + * direction of the right motor. Values of -400 or less result in full speed + * reverse, and values of 400 or more result in full speed forward. + */ + virtual void setSpeeds(int16_t leftSpeed, int16_t rightSpeed) = 0; + + /** + * Get maximum speed of the motors in digits. + * + * @return Max. speed in digits + */ + virtual int16_t getMaxSpeed() const = 0; + + /** + * Get the current speed of the left motor. + * + * @return The speed of the left motor in digits. + */ + virtual int16_t getLeftSpeed() = 0; + + /** + * Get the current speed of the right motor. + * + * @return The speed of the right motor in digits. + */ + virtual int16_t getRightSpeed() = 0; + +protected: + /** + * Constructs the interface. + */ + IMotors() + { + } + +private: +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* IMOTORS_H */ +/** @} */ diff --git a/lib/HALInterfaces/IProximitySensors.h b/lib/HALInterfaces/IProximitySensors.h index 5884938a..4b0b2eb7 100644 --- a/lib/HALInterfaces/IProximitySensors.h +++ b/lib/HALInterfaces/IProximitySensors.h @@ -1,122 +1,122 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Abstract proximity sensors interface - * @author Andreas Merkle - * - * @addtogroup HALInterfaces - * - * @{ - */ -#ifndef IPROXIMITYSENSORS_H -#define IPROXIMITYSENSORS_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The abstract proximity sensors interface. */ -class IProximitySensors -{ -public: - /** - * Destroys the interface. - */ - virtual ~IProximitySensors() - { - } - - /** - * Initialize only the front proximity sensor. - */ - virtual void initFrontSensor() = 0; - - /** - * Returns the number of sensors. - * - * @return Number of sensors - */ - virtual uint8_t getNumSensors() const = 0; - - /** - * Emits IR pulses and gets readings from the sensors. - */ - virtual void read() = 0; - - /** - * Returns the number of brightness levels for the left LEDs that - * activated the front proximity sensor. - * - * @return Number of brightness levels - */ - virtual uint8_t countsFrontWithLeftLeds() const = 0; - - /** - * Returns the number of brightness levels for the right LEDs that - * activated the front proximity sensor. - * - * @return Number of brightness levels - */ - virtual uint8_t countsFrontWithRightLeds() const = 0; - - /** - * Returns the maximum number of brightness levels. - * - * @return Number of brightness levels. - */ - virtual uint8_t getNumBrightnessLevels() const = 0; - -protected: - /** - * Constructs the interface. - */ - IProximitySensors() - { - } - -private: -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* IPROXIMITYSENSORS_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Abstract proximity sensors interface + * @author Andreas Merkle + * + * @addtogroup HALInterfaces + * + * @{ + */ +#ifndef IPROXIMITYSENSORS_H +#define IPROXIMITYSENSORS_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The abstract proximity sensors interface. */ +class IProximitySensors +{ +public: + /** + * Destroys the interface. + */ + virtual ~IProximitySensors() + { + } + + /** + * Initialize only the front proximity sensor. + */ + virtual void initFrontSensor() = 0; + + /** + * Returns the number of sensors. + * + * @return Number of sensors + */ + virtual uint8_t getNumSensors() const = 0; + + /** + * Emits IR pulses and gets readings from the sensors. + */ + virtual void read() = 0; + + /** + * Returns the number of brightness levels for the left LEDs that + * activated the front proximity sensor. + * + * @return Number of brightness levels + */ + virtual uint8_t countsFrontWithLeftLeds() const = 0; + + /** + * Returns the number of brightness levels for the right LEDs that + * activated the front proximity sensor. + * + * @return Number of brightness levels + */ + virtual uint8_t countsFrontWithRightLeds() const = 0; + + /** + * Returns the maximum number of brightness levels. + * + * @return Number of brightness levels. + */ + virtual uint8_t getNumBrightnessLevels() const = 0; + +protected: + /** + * Constructs the interface. + */ + IProximitySensors() + { + } + +private: +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* IPROXIMITYSENSORS_H */ +/** @} */ diff --git a/lib/HALInterfaces/ISender.h b/lib/HALInterfaces/ISender.h index 0d6b630d..dc8acfd0 100644 --- a/lib/HALInterfaces/ISender.h +++ b/lib/HALInterfaces/ISender.h @@ -1,105 +1,105 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Abstract sender interface - * @author Andreas Merkle - * - * @addtogroup HALInterfaces - * - * @{ - */ -#ifndef ISENDER_H -#define ISENDER_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The abstract sender interface. */ -class ISender -{ -public: - /** - * Destroys the interface. - */ - virtual ~ISender() - { - } - - /** - * Set channel which to send data to. - * - * @param[in] channel The channel which to use. - */ - virtual void setChannel(int32_t channel) = 0; - - /** - * Sends data to the configured channel. - * - * @param[in] data Data buffer - * @param[in] size Data buffer size in bytes. - */ - virtual void send(const void* data, size_t size) const = 0; - - /** - * Sends string to the configured channel. - * - * @param[in] str String which to send. - */ - virtual void send(const char* str) const = 0; - -protected: - /** - * Constructs the interface. - */ - ISender() - { - } - -private: -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* ISENDER_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Abstract sender interface + * @author Andreas Merkle + * + * @addtogroup HALInterfaces + * + * @{ + */ +#ifndef ISENDER_H +#define ISENDER_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The abstract sender interface. */ +class ISender +{ +public: + /** + * Destroys the interface. + */ + virtual ~ISender() + { + } + + /** + * Set channel which to send data to. + * + * @param[in] channel The channel which to use. + */ + virtual void setChannel(int32_t channel) = 0; + + /** + * Sends data to the configured channel. + * + * @param[in] data Data buffer + * @param[in] size Data buffer size in bytes. + */ + virtual void send(const void* data, size_t size) const = 0; + + /** + * Sends string to the configured channel. + * + * @param[in] str String which to send. + */ + virtual void send(const char* str) const = 0; + +protected: + /** + * Constructs the interface. + */ + ISender() + { + } + +private: +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* ISENDER_H */ +/** @} */ diff --git a/lib/HALInterfaces/ISettings.h b/lib/HALInterfaces/ISettings.h index c9984697..7c2c0dff 100644 --- a/lib/HALInterfaces/ISettings.h +++ b/lib/HALInterfaces/ISettings.h @@ -1,103 +1,103 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Abstract settings interface - * @author Andreas Merkle - * - * @addtogroup HALInterfaces - * - * @{ - */ - -#ifndef ISETTINGS_H -#define ISETTINGS_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The abstract settings interface. */ -class ISettings -{ -public: - /** - * Destroys the interface. - */ - virtual ~ISettings() - { - } - - /** - * Initialize the settings. - */ - virtual void init() = 0; - - /** - * Get the max. speed. - * - * @return Max. speed in steps/s. - */ - virtual int16_t getMaxSpeed() const = 0; - - /** - * Set the max. speed. - * - * @param[in] maxSpeed Max. speed in steps/s. - */ - virtual void setMaxSpeed(int16_t maxSpeed) = 0; - -protected: - - /** - * Constructs the interface. - */ - ISettings() - { - } - -private: -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* ISETTINGS_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Abstract settings interface + * @author Andreas Merkle + * + * @addtogroup HALInterfaces + * + * @{ + */ + +#ifndef ISETTINGS_H +#define ISETTINGS_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The abstract settings interface. */ +class ISettings +{ +public: + /** + * Destroys the interface. + */ + virtual ~ISettings() + { + } + + /** + * Initialize the settings. + */ + virtual void init() = 0; + + /** + * Get the max. speed. + * + * @return Max. speed in steps/s. + */ + virtual int16_t getMaxSpeed() const = 0; + + /** + * Set the max. speed. + * + * @param[in] maxSpeed Max. speed in steps/s. + */ + virtual void setMaxSpeed(int16_t maxSpeed) = 0; + +protected: + + /** + * Constructs the interface. + */ + ISettings() + { + } + +private: +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* ISETTINGS_H */ +/** @} */ diff --git a/lib/HALInterfaces/library.json b/lib/HALInterfaces/library.json index f2b8f80e..bb5af12c 100644 --- a/lib/HALInterfaces/library.json +++ b/lib/HALInterfaces/library.json @@ -1,15 +1,15 @@ -{ - "name": "HALInterfaces", - "version": "0.1.0", - "description": "...", - "authors": [{ - "name": "Andreas Merkle", - "email": "web@blue-andi.de", - "url": "https://github.com/BlueAndi", - "maintainer": true - }], - "license": "MIT", - "dependencies": [], - "frameworks": "*", - "platforms": "*" +{ + "name": "HALInterfaces", + "version": "0.1.0", + "description": "...", + "authors": [{ + "name": "Andreas Merkle", + "email": "web@blue-andi.de", + "url": "https://github.com/BlueAndi", + "maintainer": true + }], + "license": "MIT", + "dependencies": [], + "frameworks": "*", + "platforms": "*" } \ No newline at end of file diff --git a/lib/HALInterfacesTest/IEncodersTest.h b/lib/HALInterfacesTest/IEncodersTest.h index 4d8054fd..eabe26b4 100644 --- a/lib/HALInterfacesTest/IEncodersTest.h +++ b/lib/HALInterfacesTest/IEncodersTest.h @@ -1,96 +1,96 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Abstract encoders test interface - * @author Andreas Merkle - * - * @addtogroup HALInterfaces - * - * @{ - */ -#ifndef IENCODERSTEST_H -#define IENCODERSTEST_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** - * Encoders test interface used to simulate the encoders. - */ -class IEncodersTest -{ -public: - /** - * Destroy the test interface. - */ - ~IEncodersTest() - { - } - - /** - * Set encoder steps left. - * - * @param[in] steps Encoder steps - */ - virtual void setCountsLeft(int16_t steps) = 0; - - /** - * Set encoder steps right. - * - * @param[in] steps Encoder steps - */ - virtual void setCountsRight(int16_t steps) = 0; - -protected: - /** - * Construct the test interface. - */ - IEncodersTest() - { - } -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* IENCODERSTEST_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Abstract encoders test interface + * @author Andreas Merkle + * + * @addtogroup HALInterfaces + * + * @{ + */ +#ifndef IENCODERSTEST_H +#define IENCODERSTEST_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** + * Encoders test interface used to simulate the encoders. + */ +class IEncodersTest +{ +public: + /** + * Destroy the test interface. + */ + ~IEncodersTest() + { + } + + /** + * Set encoder steps left. + * + * @param[in] steps Encoder steps + */ + virtual void setCountsLeft(int16_t steps) = 0; + + /** + * Set encoder steps right. + * + * @param[in] steps Encoder steps + */ + virtual void setCountsRight(int16_t steps) = 0; + +protected: + /** + * Construct the test interface. + */ + IEncodersTest() + { + } +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* IENCODERSTEST_H */ +/** @} */ diff --git a/lib/HALInterfacesTest/IMotorsTest.h b/lib/HALInterfacesTest/IMotorsTest.h index 0dfc932c..0f28e1d3 100644 --- a/lib/HALInterfacesTest/IMotorsTest.h +++ b/lib/HALInterfacesTest/IMotorsTest.h @@ -1,96 +1,96 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Abstract motors test interface - * @author Andreas Merkle - * - * @addtogroup HALInterfaces - * - * @{ - */ -#ifndef IMOTORSTEST_H -#define IMOTORSTEST_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The abstract motors test interface. */ -class IMotorsTest -{ -public: - /** - * Destroys the interface. - */ - virtual ~IMotorsTest() - { - } - - /** - * Set speed of the left motor. - * - * @param[in] speed Speed in digits. - */ - virtual void setLeftSpeed(int16_t speed) = 0; - - /** - * Set speed of the right motor. - * - * @param[in] speed Speed in digits. - */ - virtual void setRightSpeed(int16_t speed) = 0; - -protected: - /** - * Constructs the interface. - */ - IMotorsTest() - { - } - -private: -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* IMOTORSTEST_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Abstract motors test interface + * @author Andreas Merkle + * + * @addtogroup HALInterfaces + * + * @{ + */ +#ifndef IMOTORSTEST_H +#define IMOTORSTEST_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The abstract motors test interface. */ +class IMotorsTest +{ +public: + /** + * Destroys the interface. + */ + virtual ~IMotorsTest() + { + } + + /** + * Set speed of the left motor. + * + * @param[in] speed Speed in digits. + */ + virtual void setLeftSpeed(int16_t speed) = 0; + + /** + * Set speed of the right motor. + * + * @param[in] speed Speed in digits. + */ + virtual void setRightSpeed(int16_t speed) = 0; + +protected: + /** + * Constructs the interface. + */ + IMotorsTest() + { + } + +private: +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* IMOTORSTEST_H */ +/** @} */ diff --git a/lib/HALInterfacesTest/library.json b/lib/HALInterfacesTest/library.json index 94208a3a..7fea4c1e 100644 --- a/lib/HALInterfacesTest/library.json +++ b/lib/HALInterfacesTest/library.json @@ -1,15 +1,15 @@ -{ - "name": "HALInterfacesTest", - "version": "0.1.0", - "description": "...", - "authors": [{ - "name": "Andreas Merkle", - "email": "web@blue-andi.de", - "url": "https://github.com/BlueAndi", - "maintainer": true - }], - "license": "MIT", - "dependencies": [], - "frameworks": "*", - "platforms": "*" +{ + "name": "HALInterfacesTest", + "version": "0.1.0", + "description": "...", + "authors": [{ + "name": "Andreas Merkle", + "email": "web@blue-andi.de", + "url": "https://github.com/BlueAndi", + "maintainer": true + }], + "license": "MIT", + "dependencies": [], + "frameworks": "*", + "platforms": "*" } \ No newline at end of file diff --git a/lib/HALLineFollowerSim/Board.cpp b/lib/HALLineFollowerSim/Board.cpp index d70999ad..b1fd5e5d 100644 --- a/lib/HALLineFollowerSim/Board.cpp +++ b/lib/HALLineFollowerSim/Board.cpp @@ -1,131 +1,131 @@ -/* MIT License - * - * Copyright (c) 2019 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief The simulation robot board realization. - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void Board::init() -{ - m_encoders.init(); - m_lineSensors.init(); - m_motors.init(); - m_settings.init(); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -Board::Board() : - IBoard(), - m_robot(), - m_simTime(m_robot), - m_keyboard(m_simTime, m_robot.getKeyboard()), - m_buttonA(m_keyboard), - m_buttonB(m_keyboard), - m_buttonC(m_keyboard), - m_buzzer(m_robot.getSpeaker(RobotDeviceNames::SPEAKER_NAME)), - m_display(m_robot.getDisplay(RobotDeviceNames::DISPLAY_NAME)), - m_encoders(m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME), - m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)), - m_lineSensors( - m_robot.getEmitter(RobotDeviceNames::EMITTER_0_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_1_NAME), - m_robot.getEmitter(RobotDeviceNames::EMITTER_2_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_3_NAME), - m_robot.getEmitter(RobotDeviceNames::EMITTER_4_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)), - m_motors(m_robot.getMotor(RobotDeviceNames::LEFT_MOTOR_NAME), m_robot.getMotor(RobotDeviceNames::RIGHT_MOTOR_NAME)), - m_ledRed(m_robot.getLED(RobotDeviceNames::LED_RED_NAME)), - m_ledYellow(m_robot.getLED(RobotDeviceNames::LED_YELLOW_NAME)), - m_ledGreen(m_robot.getLED(RobotDeviceNames::LED_GREEN_NAME)), - m_settings() -#ifdef DEBUG_ODOMETRY - , - m_sender(m_robot.getEmitter(SENDER_NAME)) -#endif /* DEBUG_ODOMETRY */ -{ -} - -void Board::enableSimulationDevices() -{ - const int timeStep = m_simTime.getTimeStep(); - - m_robot.getKeyboard()->enable(timeStep); - m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME)->enable(timeStep); - m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)->enable(timeStep); -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions +/* MIT License + * + * Copyright (c) 2019 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief The simulation robot board realization. + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void Board::init() +{ + m_encoders.init(); + m_lineSensors.init(); + m_motors.init(); + m_settings.init(); +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +Board::Board() : + IBoard(), + m_robot(), + m_simTime(m_robot), + m_keyboard(m_simTime, m_robot.getKeyboard()), + m_buttonA(m_keyboard), + m_buttonB(m_keyboard), + m_buttonC(m_keyboard), + m_buzzer(m_robot.getSpeaker(RobotDeviceNames::SPEAKER_NAME)), + m_display(m_robot.getDisplay(RobotDeviceNames::DISPLAY_NAME)), + m_encoders(m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME), + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)), + m_lineSensors( + m_robot.getEmitter(RobotDeviceNames::EMITTER_0_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_1_NAME), + m_robot.getEmitter(RobotDeviceNames::EMITTER_2_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_3_NAME), + m_robot.getEmitter(RobotDeviceNames::EMITTER_4_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)), + m_motors(m_robot.getMotor(RobotDeviceNames::LEFT_MOTOR_NAME), m_robot.getMotor(RobotDeviceNames::RIGHT_MOTOR_NAME)), + m_ledRed(m_robot.getLED(RobotDeviceNames::LED_RED_NAME)), + m_ledYellow(m_robot.getLED(RobotDeviceNames::LED_YELLOW_NAME)), + m_ledGreen(m_robot.getLED(RobotDeviceNames::LED_GREEN_NAME)), + m_settings() +#ifdef DEBUG_ODOMETRY + , + m_sender(m_robot.getEmitter(SENDER_NAME)) +#endif /* DEBUG_ODOMETRY */ +{ +} + +void Board::enableSimulationDevices() +{ + const int timeStep = m_simTime.getTimeStep(); + + m_robot.getKeyboard()->enable(timeStep); + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME)->enable(timeStep); + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)->enable(timeStep); +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions *****************************************************************************/ \ No newline at end of file diff --git a/lib/HALLineFollowerSim/library.json b/lib/HALLineFollowerSim/library.json index 1db98687..fd925552 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 4b265980..97427cc2 100644 --- a/lib/HALLineFollowerTarget/Board.cpp +++ b/lib/HALLineFollowerTarget/Board.cpp @@ -1,100 +1,100 @@ -/* MIT License - * - * Copyright (c) 2019 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief The physical robot board realization. - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void Board::init() -{ - m_encoders.init(); - m_lineSensors.init(); - m_motors.init(); - m_settings.init(); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -Board::Board() : - IBoard(), - m_buttonA(), - m_buttonB(), - m_buttonC(), - m_buzzer(), - m_display(), - m_encoders(), - m_lineSensors(), - m_motors(), - m_ledRed(), - m_ledYellow(), - m_ledGreen(), - m_settings() -{ -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions +/* MIT License + * + * Copyright (c) 2019 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief The physical robot board realization. + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void Board::init() +{ + m_encoders.init(); + m_lineSensors.init(); + m_motors.init(); + m_settings.init(); +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +Board::Board() : + IBoard(), + m_buttonA(), + m_buttonB(), + m_buttonC(), + m_buzzer(), + m_display(), + m_encoders(), + m_lineSensors(), + m_motors(), + m_ledRed(), + m_ledYellow(), + m_ledGreen(), + m_settings() +{ +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions *****************************************************************************/ \ No newline at end of file diff --git a/lib/HALLineFollowerTarget/library.json b/lib/HALLineFollowerTarget/library.json index 00083a5d..bf6ef9c3 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 11ce3e23..00a22737 100644 --- a/lib/HALRemoteControlSim/Board.cpp +++ b/lib/HALRemoteControlSim/Board.cpp @@ -1,134 +1,134 @@ -/* MIT License - * - * Copyright (c) 2019 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief The simulation robot board realization. - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void Board::init() -{ - m_encoders.init(); - m_lineSensors.init(); - m_motors.init(); - m_proximitySensors.initFrontSensor(); - m_settings.init(); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -Board::Board() : - IBoard(), - 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(RobotDeviceNames::SPEAKER_NAME)), - m_display(m_robot.getDisplay(RobotDeviceNames::DISPLAY_NAME)), - m_encoders(m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME), - m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)), - m_lineSensors( - m_robot.getEmitter(RobotDeviceNames::EMITTER_0_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_1_NAME), - m_robot.getEmitter(RobotDeviceNames::EMITTER_2_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_3_NAME), - m_robot.getEmitter(RobotDeviceNames::EMITTER_4_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)), - m_motors(m_robot.getMotor(RobotDeviceNames::LEFT_MOTOR_NAME), m_robot.getMotor(RobotDeviceNames::RIGHT_MOTOR_NAME)), - m_ledRed(m_robot.getLED(RobotDeviceNames::LED_RED_NAME)), - m_ledYellow(m_robot.getLED(RobotDeviceNames::LED_YELLOW_NAME)), - m_ledGreen(m_robot.getLED(RobotDeviceNames::LED_GREEN_NAME)), - m_proximitySensors(m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_LEFT_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_RIGHT_NAME)), - m_serialDrv(m_robot.getEmitter(RobotDeviceNames::EMITTER_NAME_SERIAL), - m_robot.getReceiver(RobotDeviceNames::RECEIVER_NAME_SERIAL)), - m_settings() -{ -} - -void Board::enableSimulationDevices() -{ - const int timeStep = m_simTime.getTimeStep(); - - m_robot.getKeyboard()->enable(timeStep); - m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME)->enable(timeStep); - m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_LEFT_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_RIGHT_NAME)->enable(timeStep); - m_robot.getReceiver(RobotDeviceNames::RECEIVER_NAME_SERIAL)->enable(timeStep); -} -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions +/* MIT License + * + * Copyright (c) 2019 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief The simulation robot board realization. + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void Board::init() +{ + m_encoders.init(); + m_lineSensors.init(); + m_motors.init(); + m_proximitySensors.initFrontSensor(); + m_settings.init(); +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +Board::Board() : + IBoard(), + 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(RobotDeviceNames::SPEAKER_NAME)), + m_display(m_robot.getDisplay(RobotDeviceNames::DISPLAY_NAME)), + m_encoders(m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME), + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)), + m_lineSensors( + m_robot.getEmitter(RobotDeviceNames::EMITTER_0_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_1_NAME), + m_robot.getEmitter(RobotDeviceNames::EMITTER_2_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_3_NAME), + m_robot.getEmitter(RobotDeviceNames::EMITTER_4_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)), + m_motors(m_robot.getMotor(RobotDeviceNames::LEFT_MOTOR_NAME), m_robot.getMotor(RobotDeviceNames::RIGHT_MOTOR_NAME)), + m_ledRed(m_robot.getLED(RobotDeviceNames::LED_RED_NAME)), + m_ledYellow(m_robot.getLED(RobotDeviceNames::LED_YELLOW_NAME)), + m_ledGreen(m_robot.getLED(RobotDeviceNames::LED_GREEN_NAME)), + m_proximitySensors(m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_LEFT_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_RIGHT_NAME)), + m_serialDrv(m_robot.getEmitter(RobotDeviceNames::EMITTER_NAME_SERIAL), + m_robot.getReceiver(RobotDeviceNames::RECEIVER_NAME_SERIAL)), + m_settings() +{ +} + +void Board::enableSimulationDevices() +{ + const int timeStep = m_simTime.getTimeStep(); + + m_robot.getKeyboard()->enable(timeStep); + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME)->enable(timeStep); + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_LEFT_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_RIGHT_NAME)->enable(timeStep); + m_robot.getReceiver(RobotDeviceNames::RECEIVER_NAME_SERIAL)->enable(timeStep); +} +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions *****************************************************************************/ \ No newline at end of file diff --git a/lib/HALRemoteControlSim/library.json b/lib/HALRemoteControlSim/library.json index 29af2800..d3159427 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 a30cb8e0..39fbe102 100644 --- a/lib/HALRemoteControlTarget/Board.cpp +++ b/lib/HALRemoteControlTarget/Board.cpp @@ -1,102 +1,102 @@ -/* MIT License - * - * Copyright (c) 2019 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief The physical robot board realization. - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void Board::init() -{ - m_encoders.init(); - m_lineSensors.init(); - m_motors.init(); - m_proximitySensors.initFrontSensor(); - m_settings.init(); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -Board::Board() : - IBoard(), - m_buttonA(), - m_buttonB(), - m_buttonC(), - m_buzzer(), - m_display(), - m_encoders(), - m_lineSensors(), - m_motors(), - m_ledRed(), - m_ledYellow(), - m_ledGreen(), - m_proximitySensors(), - m_settings() -{ -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions +/* MIT License + * + * Copyright (c) 2019 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief The physical robot board realization. + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void Board::init() +{ + m_encoders.init(); + m_lineSensors.init(); + m_motors.init(); + m_proximitySensors.initFrontSensor(); + m_settings.init(); +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +Board::Board() : + IBoard(), + m_buttonA(), + m_buttonB(), + m_buttonC(), + m_buzzer(), + m_display(), + m_encoders(), + m_lineSensors(), + m_motors(), + m_ledRed(), + m_ledYellow(), + m_ledGreen(), + m_proximitySensors(), + m_settings() +{ +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions *****************************************************************************/ \ No newline at end of file diff --git a/lib/HALRemoteControlTarget/library.json b/lib/HALRemoteControlTarget/library.json index 62907680..7ebd7947 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 e55cc70e..4fec1342 100644 --- a/lib/HALSensorFusionSim/Board.cpp +++ b/lib/HALSensorFusionSim/Board.cpp @@ -1,134 +1,134 @@ -/* MIT License - * - * Copyright (c) 2019 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief The simulation robot board realization. - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void Board::init() -{ - m_encoders.init(); - m_lineSensors.init(); - m_motors.init(); - /* TODO: TD084 React if IMU initialization fails */ - (void)m_imu.init(); - m_imu.enableDefault(); - m_imu.configureForTurnSensing(); - m_imu.calibrate(); - m_settings.init(); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -Board::Board() : - m_robot(), - m_simTime(m_robot), - m_keyboard(m_simTime, m_robot.getKeyboard()), - m_buttonA(m_keyboard), - m_buzzer(m_robot.getSpeaker(RobotDeviceNames::SPEAKER_NAME)), - m_encoders(m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME), - m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)), - m_lineSensors( - m_robot.getEmitter(RobotDeviceNames::EMITTER_0_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_1_NAME), - m_robot.getEmitter(RobotDeviceNames::EMITTER_2_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_3_NAME), - m_robot.getEmitter(RobotDeviceNames::EMITTER_4_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME), - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)), - m_motors(m_robot.getMotor(RobotDeviceNames::LEFT_MOTOR_NAME), m_robot.getMotor(RobotDeviceNames::RIGHT_MOTOR_NAME)), - m_ledYellow(m_robot.getLED(RobotDeviceNames::LED_YELLOW_NAME)), - m_imu(m_robot.getAccelerometer(RobotDeviceNames::ACCELEROMETER_NAME), m_robot.getGyro(RobotDeviceNames::GYRO_NAME), - m_robot.getCompass(RobotDeviceNames::MAGNETOMETER_NAME)), - m_serialDrv(m_robot.getEmitter(RobotDeviceNames::EMITTER_NAME_SERIAL), - m_robot.getReceiver(RobotDeviceNames::RECEIVER_NAME_SERIAL)), - m_settings() -{ -} - -void Board::enableSimulationDevices() -{ - const int timeStep = m_simTime.getTimeStep(); - - m_robot.getKeyboard()->enable(timeStep); - m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME)->enable(timeStep); - m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME)->enable(timeStep); - m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)->enable(timeStep); - m_robot.getAccelerometer(RobotDeviceNames::ACCELEROMETER_NAME)->enable(timeStep); - m_robot.getGyro(RobotDeviceNames::GYRO_NAME)->enable(timeStep); - m_robot.getCompass(RobotDeviceNames::MAGNETOMETER_NAME)->enable(timeStep); - m_robot.getReceiver(RobotDeviceNames::RECEIVER_NAME_SERIAL)->enable(timeStep); -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2019 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief The simulation robot board realization. + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void Board::init() +{ + m_encoders.init(); + m_lineSensors.init(); + m_motors.init(); + /* TODO: TD084 React if IMU initialization fails */ + (void)m_imu.init(); + m_imu.enableDefault(); + m_imu.configureForTurnSensing(); + m_imu.calibrate(); + m_settings.init(); +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +Board::Board() : + m_robot(), + m_simTime(m_robot), + m_keyboard(m_simTime, m_robot.getKeyboard()), + m_buttonA(m_keyboard), + m_buzzer(m_robot.getSpeaker(RobotDeviceNames::SPEAKER_NAME)), + m_encoders(m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME), + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)), + m_lineSensors( + m_robot.getEmitter(RobotDeviceNames::EMITTER_0_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_1_NAME), + m_robot.getEmitter(RobotDeviceNames::EMITTER_2_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_3_NAME), + m_robot.getEmitter(RobotDeviceNames::EMITTER_4_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)), + m_motors(m_robot.getMotor(RobotDeviceNames::LEFT_MOTOR_NAME), m_robot.getMotor(RobotDeviceNames::RIGHT_MOTOR_NAME)), + m_ledYellow(m_robot.getLED(RobotDeviceNames::LED_YELLOW_NAME)), + m_imu(m_robot.getAccelerometer(RobotDeviceNames::ACCELEROMETER_NAME), m_robot.getGyro(RobotDeviceNames::GYRO_NAME), + m_robot.getCompass(RobotDeviceNames::MAGNETOMETER_NAME)), + m_serialDrv(m_robot.getEmitter(RobotDeviceNames::EMITTER_NAME_SERIAL), + m_robot.getReceiver(RobotDeviceNames::RECEIVER_NAME_SERIAL)), + m_settings() +{ +} + +void Board::enableSimulationDevices() +{ + const int timeStep = m_simTime.getTimeStep(); + + m_robot.getKeyboard()->enable(timeStep); + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME)->enable(timeStep); + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)->enable(timeStep); + m_robot.getAccelerometer(RobotDeviceNames::ACCELEROMETER_NAME)->enable(timeStep); + m_robot.getGyro(RobotDeviceNames::GYRO_NAME)->enable(timeStep); + m_robot.getCompass(RobotDeviceNames::MAGNETOMETER_NAME)->enable(timeStep); + m_robot.getReceiver(RobotDeviceNames::RECEIVER_NAME_SERIAL)->enable(timeStep); +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALSensorFusionSim/Board.h b/lib/HALSensorFusionSim/Board.h index f9f714b0..6b5c3d32 100644 --- a/lib/HALSensorFusionSim/Board.h +++ b/lib/HALSensorFusionSim/Board.h @@ -1,287 +1,287 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief The simulation robot board realization. - * @author Andreas Merkle - * - * @addtogroup HALSim - * - * @{ - */ -#ifndef BOARD_H -#define BOARD_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** - * The concrete simulation robot board. - */ -class Board : public IBoard -{ -public: - /** - * Get board instance. - * - * @return Board instance - */ - static Board& getInstance() - { - static Board instance; /* idiom */ - - return instance; - } - - /** - * Initialize the hardware. - */ - void init() final; - - /** - * Get button A driver. - * - * @return Button A driver. - */ - IButton& getButtonA() final - { - return m_buttonA; - } - - /** - * Get buzzer driver. - * - * @return Buzzer driver. - */ - IBuzzer& getBuzzer() final - { - return m_buzzer; - } - - /** - * Get encoders. - * - * @return Encoders driver. - */ - IEncoders& getEncoders() final - { - return m_encoders; - } - - /** - * Get line sensors driver. - * - * @return Line sensor driver. - */ - ILineSensors& getLineSensors() final - { - return m_lineSensors; - } - - /** - * Get motor driver. - * - * @return Motor driver. - */ - IMotors& getMotors() final - { - return m_motors; - } - - /** - * Get yellow LED driver. - * - * @return Yellow LED driver. - */ - ILed& getYellowLed() final - { - return m_ledYellow; - } - - /** - * Get IMU (Inertial Measurement Unit) driver. - * - * @return IMU driver - */ - IIMU& getIMU() final - { - return m_imu; - } - - /** - * Get settings instance. - * - * @return Settings - */ - ISettings& getSettings() final - { - return m_settings; - } - - /** - * Process actuators and sensors. - */ - void process() final - { - m_buzzer.process(); - } - -private: - - /** Simulated roboter instance. */ - webots::Robot m_robot; - - /** Simulation time handler */ - SimTime m_simTime; - - /** Own keyboard that wraps the webots keyboard. */ - Keyboard m_keyboard; - - /** Button A driver */ - ButtonA m_buttonA; - - /** 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; - - /** Simulation serial driver */ - WebotsSerialDrv m_serialDrv; - - /** Settings */ - Settings m_settings; - - /** - * Constructs the concrete board. - */ - Board(); - - /** - * Destroys the concrete board. - */ - ~Board() - { - } - - /** - * Get the simulation time handler. - * - * @return Simulation time handler - */ - SimTime& getSimTime() - { - return m_simTime; - } - - /** - * Get the keyboard instance of the simulation. - * - * @return The keyboard. - */ - Keyboard& getKeyboard() - { - return m_keyboard; - } - - /** - * Get the simulation serial driver, which is connected within Webots. - * - * @return If serial driver is available, it will return a pointer to it, otherwise nullptr. - */ - WebotsSerialDrv* getSimSerial() - { - return &m_serialDrv; - } - - /** - * Enable all simulation devices. - * It is called by the main entry only. - * Devices must be enabled before they can be used, and a simulation step must be performed before the application - * initialization. - */ - void enableSimulationDevices(); - - /** - * The main entry needs access to the simulation robot instance. - * But all other application parts shall have no access, which is - * solved by this friend. - * - * @param[in] argc Number of arguments - * @param[in] argv Arguments - * - * @return Exit code - */ - friend int main(int argc, char** argv); -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* BOARD_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief The simulation robot board realization. + * @author Andreas Merkle + * + * @addtogroup HALSim + * + * @{ + */ +#ifndef BOARD_H +#define BOARD_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** + * The concrete simulation robot board. + */ +class Board : public IBoard +{ +public: + /** + * Get board instance. + * + * @return Board instance + */ + static Board& getInstance() + { + static Board instance; /* idiom */ + + return instance; + } + + /** + * Initialize the hardware. + */ + void init() final; + + /** + * Get button A driver. + * + * @return Button A driver. + */ + IButton& getButtonA() final + { + return m_buttonA; + } + + /** + * Get buzzer driver. + * + * @return Buzzer driver. + */ + IBuzzer& getBuzzer() final + { + return m_buzzer; + } + + /** + * Get encoders. + * + * @return Encoders driver. + */ + IEncoders& getEncoders() final + { + return m_encoders; + } + + /** + * Get line sensors driver. + * + * @return Line sensor driver. + */ + ILineSensors& getLineSensors() final + { + return m_lineSensors; + } + + /** + * Get motor driver. + * + * @return Motor driver. + */ + IMotors& getMotors() final + { + return m_motors; + } + + /** + * Get yellow LED driver. + * + * @return Yellow LED driver. + */ + ILed& getYellowLed() final + { + return m_ledYellow; + } + + /** + * Get IMU (Inertial Measurement Unit) driver. + * + * @return IMU driver + */ + IIMU& getIMU() final + { + return m_imu; + } + + /** + * Get settings instance. + * + * @return Settings + */ + ISettings& getSettings() final + { + return m_settings; + } + + /** + * Process actuators and sensors. + */ + void process() final + { + m_buzzer.process(); + } + +private: + + /** Simulated roboter instance. */ + webots::Robot m_robot; + + /** Simulation time handler */ + SimTime m_simTime; + + /** Own keyboard that wraps the webots keyboard. */ + Keyboard m_keyboard; + + /** Button A driver */ + ButtonA m_buttonA; + + /** 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; + + /** Simulation serial driver */ + WebotsSerialDrv m_serialDrv; + + /** Settings */ + Settings m_settings; + + /** + * Constructs the concrete board. + */ + Board(); + + /** + * Destroys the concrete board. + */ + ~Board() + { + } + + /** + * Get the simulation time handler. + * + * @return Simulation time handler + */ + SimTime& getSimTime() + { + return m_simTime; + } + + /** + * Get the keyboard instance of the simulation. + * + * @return The keyboard. + */ + Keyboard& getKeyboard() + { + return m_keyboard; + } + + /** + * Get the simulation serial driver, which is connected within Webots. + * + * @return If serial driver is available, it will return a pointer to it, otherwise nullptr. + */ + WebotsSerialDrv* getSimSerial() + { + return &m_serialDrv; + } + + /** + * Enable all simulation devices. + * It is called by the main entry only. + * Devices must be enabled before they can be used, and a simulation step must be performed before the application + * initialization. + */ + void enableSimulationDevices(); + + /** + * The main entry needs access to the simulation robot instance. + * But all other application parts shall have no access, which is + * solved by this friend. + * + * @param[in] argc Number of arguments + * @param[in] argv Arguments + * + * @return Exit code + */ + friend int main(int argc, char** argv); +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* BOARD_H */ +/** @} */ diff --git a/lib/HALSensorFusionSim/library.json b/lib/HALSensorFusionSim/library.json index 5444939d..789a7f54 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 fa9b7e62..7528e766 100644 --- a/lib/HALSensorFusionTarget/Board.cpp +++ b/lib/HALSensorFusionTarget/Board.cpp @@ -1,101 +1,101 @@ -/* MIT License - * - * Copyright (c) 2019 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief The physical robot board realization. - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -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(); - m_settings.init(); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -Board::Board() : - IBoard(), - m_buttonA(), - m_buzzer(), - m_encoders(), - m_lineSensors(), - m_motors(), - m_ledYellow(), - m_imu(), - m_settings() -{ -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2019 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief The physical robot board realization. + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +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(); + m_settings.init(); +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +Board::Board() : + IBoard(), + m_buttonA(), + m_buzzer(), + m_encoders(), + m_lineSensors(), + m_motors(), + m_ledYellow(), + m_imu(), + m_settings() +{ +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALSensorFusionTarget/Board.h b/lib/HALSensorFusionTarget/Board.h index 46e8867f..3b063273 100644 --- a/lib/HALSensorFusionTarget/Board.h +++ b/lib/HALSensorFusionTarget/Board.h @@ -1,218 +1,218 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief The 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 - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** - * The concrete physical board. - */ -class Board : public IBoard -{ -public: - /** - * Get board instance. - * - * @return Board instance - */ - static Board& getInstance() - { - static Board instance; /* idiom */ - - return instance; - } - - /** - * Initialize the hardware. - */ - void init() final; - - /** - * Get button A driver. - * - * @return Button A driver. - */ - IButton& getButtonA() final - { - return m_buttonA; - } - - /** - * Get buzzer driver. - * - * @return Buzzer driver. - */ - IBuzzer& getBuzzer() final - { - return m_buzzer; - } - - /** - * Get encoders. - * - * @return Encoders driver. - */ - IEncoders& getEncoders() final - { - return m_encoders; - } - - /** - * Get line sensors driver. - * - * @return Line sensor driver. - */ - ILineSensors& getLineSensors() final - { - return m_lineSensors; - } - - /** - * Get motor driver. - * - * @return Motor driver. - */ - IMotors& getMotors() final - { - return m_motors; - } - - /** - * Get yellow LED driver. - * - * @return Yellow LED driver. - */ - ILed& getYellowLed() final - { - return m_ledYellow; - } - - /** - * Get IMU (Inertial Measurement Unit) driver. - * - * @return IMU driver - */ - IIMU& getIMU() final - { - return m_imu; - } - - /** - * Get settings instance. - * - * @return Settings - */ - ISettings& getSettings() final - { - return m_settings; - } - - /** - * Process actuators and sensors. - */ - void process() final - { - m_buzzer.process(); - } - -private: - /** Button A driver */ - ButtonA m_buttonA; - - /** Buzzer driver as a Dummy */ - NoBuzzer 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; - - /** Settings */ - Settings m_settings; - - /** - * Constructs the concrete board. - */ - Board(); - - /** - * Destroys the concrete board. - */ - ~Board() - { - } -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* BOARD_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief 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 + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** + * The concrete physical board. + */ +class Board : public IBoard +{ +public: + /** + * Get board instance. + * + * @return Board instance + */ + static Board& getInstance() + { + static Board instance; /* idiom */ + + return instance; + } + + /** + * Initialize the hardware. + */ + void init() final; + + /** + * Get button A driver. + * + * @return Button A driver. + */ + IButton& getButtonA() final + { + return m_buttonA; + } + + /** + * Get buzzer driver. + * + * @return Buzzer driver. + */ + IBuzzer& getBuzzer() final + { + return m_buzzer; + } + + /** + * Get encoders. + * + * @return Encoders driver. + */ + IEncoders& getEncoders() final + { + return m_encoders; + } + + /** + * Get line sensors driver. + * + * @return Line sensor driver. + */ + ILineSensors& getLineSensors() final + { + return m_lineSensors; + } + + /** + * Get motor driver. + * + * @return Motor driver. + */ + IMotors& getMotors() final + { + return m_motors; + } + + /** + * Get yellow LED driver. + * + * @return Yellow LED driver. + */ + ILed& getYellowLed() final + { + return m_ledYellow; + } + + /** + * Get IMU (Inertial Measurement Unit) driver. + * + * @return IMU driver + */ + IIMU& getIMU() final + { + return m_imu; + } + + /** + * Get settings instance. + * + * @return Settings + */ + ISettings& getSettings() final + { + return m_settings; + } + + /** + * Process actuators and sensors. + */ + void process() final + { + m_buzzer.process(); + } + +private: + /** Button A driver */ + ButtonA m_buttonA; + + /** Buzzer driver as a Dummy */ + NoBuzzer 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; + + /** Settings */ + Settings m_settings; + + /** + * Constructs the concrete board. + */ + Board(); + + /** + * Destroys the concrete board. + */ + ~Board() + { + } +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* BOARD_H */ +/** @} */ diff --git a/lib/HALSensorFusionTarget/library.json b/lib/HALSensorFusionTarget/library.json index 569f7ad4..5c4faca5 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/ButtonA.cpp b/lib/HALSim/ButtonA.cpp index b2e648ee..1a2b2979 100644 --- a/lib/HALSim/ButtonA.cpp +++ b/lib/HALSim/ButtonA.cpp @@ -1,85 +1,85 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Button A realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ButtonA.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -bool ButtonA::isPressed() -{ - return m_keyboard.buttonAPressed(); -} - -void ButtonA::waitForRelease() -{ - m_keyboard.waitForReleaseA(); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Button A realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ButtonA.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +bool ButtonA::isPressed() +{ + return m_keyboard.buttonAPressed(); +} + +void ButtonA::waitForRelease() +{ + m_keyboard.waitForReleaseA(); +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALSim/ButtonA.h b/lib/HALSim/ButtonA.h index 774825d8..f049a4c3 100644 --- a/lib/HALSim/ButtonA.h +++ b/lib/HALSim/ButtonA.h @@ -1,102 +1,102 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Button A realization - * @author Andreas Merkle - * - * @addtogroup HALSim - * - * @{ - */ - -#ifndef BUTTONA_H -#define BUTTONA_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "IButton.h" - -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class provides access to the robot simulation button A. */ -class ButtonA : public IButton -{ -public: - /** - * Constructs the button A adapter. - * - * @param[in] keyboard The robot keyboard. - */ - ButtonA(Keyboard& keyboard) : IButton(), m_keyboard(keyboard) - { - } - - /** - * Destroys the button A adapter. - */ - ~ButtonA() - { - } - - /** - * Indicates whether the button A is pressed or not. - * - * @return Returns true if button is pressed, otherwise returns false. - */ - bool isPressed() final; - - /** - * Wait until button A is released. - */ - void waitForRelease() final; - -private: - Keyboard& m_keyboard; /**< Robot keyboard */ - - /* Default constructor not allowed. */ - ButtonA(); -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* BUTTONA_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Button A realization + * @author Andreas Merkle + * + * @addtogroup HALSim + * + * @{ + */ + +#ifndef BUTTONA_H +#define BUTTONA_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "IButton.h" + +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** This class provides access to the robot simulation button A. */ +class ButtonA : public IButton +{ +public: + /** + * Constructs the button A adapter. + * + * @param[in] keyboard The robot keyboard. + */ + ButtonA(Keyboard& keyboard) : IButton(), m_keyboard(keyboard) + { + } + + /** + * Destroys the button A adapter. + */ + ~ButtonA() + { + } + + /** + * Indicates whether the button A is pressed or not. + * + * @return Returns true if button is pressed, otherwise returns false. + */ + bool isPressed() final; + + /** + * Wait until button A is released. + */ + void waitForRelease() final; + +private: + Keyboard& m_keyboard; /**< Robot keyboard */ + + /* Default constructor not allowed. */ + ButtonA(); +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* BUTTONA_H */ +/** @} */ diff --git a/lib/HALSim/ButtonB.cpp b/lib/HALSim/ButtonB.cpp index f4ac9acf..7fad22a3 100644 --- a/lib/HALSim/ButtonB.cpp +++ b/lib/HALSim/ButtonB.cpp @@ -1,85 +1,85 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Button B realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ButtonB.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -bool ButtonB::isPressed() -{ - return m_keyboard.buttonBPressed(); -} - -void ButtonB::waitForRelease() -{ - m_keyboard.waitForReleaseB(); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Button B realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ButtonB.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +bool ButtonB::isPressed() +{ + return m_keyboard.buttonBPressed(); +} + +void ButtonB::waitForRelease() +{ + m_keyboard.waitForReleaseB(); +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALSim/ButtonB.h b/lib/HALSim/ButtonB.h index 9a515bc2..a0d180d8 100644 --- a/lib/HALSim/ButtonB.h +++ b/lib/HALSim/ButtonB.h @@ -1,102 +1,102 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Button B realization - * @author Andreas Merkle - * - * @addtogroup HALSim - * - * @{ - */ - -#ifndef BUTTONB_H -#define BUTTONB_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "IButton.h" - -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class provides access to the robot simulation button B. */ -class ButtonB : public IButton -{ -public: - /** - * Constructs the button B adapter. - * - * @param[in] keyboard The robot keyboard. - */ - ButtonB(Keyboard& keyboard) : IButton(), m_keyboard(keyboard) - { - } - - /** - * Destroys the button B adapter. - */ - ~ButtonB() - { - } - - /** - * Indicates whether button B is pressed or not. - * - * @return Returns true if button B is pressed, otherwise returns false. - */ - bool isPressed() final; - - /** - * Wait until button B is released. - */ - void waitForRelease() final; - -private: - Keyboard& m_keyboard; /**< Robot keyboard */ - - /* Default constructor not allowed. */ - ButtonB(); -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* BUTTONB_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Button B realization + * @author Andreas Merkle + * + * @addtogroup HALSim + * + * @{ + */ + +#ifndef BUTTONB_H +#define BUTTONB_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "IButton.h" + +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** This class provides access to the robot simulation button B. */ +class ButtonB : public IButton +{ +public: + /** + * Constructs the button B adapter. + * + * @param[in] keyboard The robot keyboard. + */ + ButtonB(Keyboard& keyboard) : IButton(), m_keyboard(keyboard) + { + } + + /** + * Destroys the button B adapter. + */ + ~ButtonB() + { + } + + /** + * Indicates whether button B is pressed or not. + * + * @return Returns true if button B is pressed, otherwise returns false. + */ + bool isPressed() final; + + /** + * Wait until button B is released. + */ + void waitForRelease() final; + +private: + Keyboard& m_keyboard; /**< Robot keyboard */ + + /* Default constructor not allowed. */ + ButtonB(); +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* BUTTONB_H */ +/** @} */ diff --git a/lib/HALSim/ButtonC.cpp b/lib/HALSim/ButtonC.cpp index 2906488d..692e5fab 100644 --- a/lib/HALSim/ButtonC.cpp +++ b/lib/HALSim/ButtonC.cpp @@ -1,85 +1,85 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Button C realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ButtonC.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -bool ButtonC::isPressed() -{ - return m_keyboard.buttonCPressed(); -} - -void ButtonC::waitForRelease() -{ - m_keyboard.waitForReleaseC(); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Button C realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ButtonC.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +bool ButtonC::isPressed() +{ + return m_keyboard.buttonCPressed(); +} + +void ButtonC::waitForRelease() +{ + m_keyboard.waitForReleaseC(); +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALSim/ButtonC.h b/lib/HALSim/ButtonC.h index 6921514d..befc6654 100644 --- a/lib/HALSim/ButtonC.h +++ b/lib/HALSim/ButtonC.h @@ -1,102 +1,102 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Button C realization - * @author Andreas Merkle - * - * @addtogroup HALSim - * - * @{ - */ - -#ifndef BUTTONC_H -#define BUTTONC_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "IButton.h" - -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class provides access to the robot simulation button C. */ -class ButtonC : public IButton -{ -public: - /** - * Constructs the button C adapter. - * - * @param[in] keyboard The robot keyboard. - */ - ButtonC(Keyboard& keyboard) : IButton(), m_keyboard(keyboard) - { - } - - /** - * Destroys the button C adapter. - */ - ~ButtonC() - { - } - - /** - * Indicates whether button C is pressed or not. - * - * @return Returns true if button C is pressed, otherwise returns false. - */ - bool isPressed() final; - - /** - * Wait until button is released. - */ - void waitForRelease() final; - -private: - Keyboard& m_keyboard; /**< Robot keyboard */ - - /* Default constructor not allowed. */ - ButtonC(); -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* BUTTONC_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Button C realization + * @author Andreas Merkle + * + * @addtogroup HALSim + * + * @{ + */ + +#ifndef BUTTONC_H +#define BUTTONC_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "IButton.h" + +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** This class provides access to the robot simulation button C. */ +class ButtonC : public IButton +{ +public: + /** + * Constructs the button C adapter. + * + * @param[in] keyboard The robot keyboard. + */ + ButtonC(Keyboard& keyboard) : IButton(), m_keyboard(keyboard) + { + } + + /** + * Destroys the button C adapter. + */ + ~ButtonC() + { + } + + /** + * Indicates whether button C is pressed or not. + * + * @return Returns true if button C is pressed, otherwise returns false. + */ + bool isPressed() final; + + /** + * Wait until button is released. + */ + void waitForRelease() final; + +private: + Keyboard& m_keyboard; /**< Robot keyboard */ + + /* Default constructor not allowed. */ + ButtonC(); +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* BUTTONC_H */ +/** @} */ diff --git a/lib/HALSim/Display.cpp b/lib/HALSim/Display.cpp index a3628897..6468b6cb 100644 --- a/lib/HALSim/Display.cpp +++ b/lib/HALSim/Display.cpp @@ -1,75 +1,75 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Display realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "Display.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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Display realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "Display.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALSim/Display.h b/lib/HALSim/Display.h index 5af85864..cffb8130 100644 --- a/lib/HALSim/Display.h +++ b/lib/HALSim/Display.h @@ -1,304 +1,304 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Display realization - * @author Andreas Merkle - * - * @addtogroup HALSim - * - * @{ - */ - -#ifndef DISPLAY_H -#define DISPLAY_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "IDisplay.h" - -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class provides access to the roboto simulation LCD. */ -class Display : public IDisplay -{ -public: - /** - * Constructs the display adapter. - * - * @param[in] display The robot display. - */ - Display(webots::Display* display) : - IDisplay(), - m_display(display), - m_height(0), - m_width(0), - m_currX(0), - m_currY(0), - m_lastX(0), - m_lastY(0) - { - if (nullptr != display) - { - m_height = display->getHeight(); - m_width = display->getWidth(); - } - } - - /** - * Destroys the display adapter. - */ - ~Display() - { - } - - /** - * Clear the display and set the cursor to the upper left corner. - */ - void clear() final - { - if (nullptr != m_display) - { - m_display->setColor(BLACK); - m_display->fillRectangle(0, 0, m_width, m_height); - } - - gotoXY(0, 0); - } - - /** - * Set the cursor to the given position. - * - * @param[in] xCoord x-coordinate, 0 is the most left position. - * @param[in] yCoord y-coordinate, 0 is the most upper position. - */ - void gotoXY(uint8_t xCoord, uint8_t yCoord) final - { - /* The one gets added to have a better central allignment of the text. */ - m_currY = static_cast((m_height / (DISPLAYLINES + 1)) * (1 + yCoord)); - m_currX = static_cast((m_width / DISPLAYCHARS) * xCoord); - - m_lastX = xCoord; - m_lastY = yCoord; - } - - /** - * Print the string to the display at the current cursor position. - * - * @param[in] str String - * - * @return Printed number of characters - */ - size_t print(const char str[]) final - { - std::string text; - text = str; - - if (nullptr != m_display) - { - m_display->setColor(WHITE); - m_display->drawText(text, m_currX, m_currY); - } - - gotoXY((m_lastX + text.length()), m_lastY); - - return text.length(); - } - - /** - * Print the unsigned 8-bit value to the display at the current cursor position. - * - * @param[in] value Value - * - * @return Printed number of characters - */ - size_t print(uint8_t value) final - { - std::string text; - text = std::to_string(value); - - if (nullptr != m_display) - { - m_display->setColor(WHITE); - m_display->drawText(text, m_currX, m_currY); - } - - gotoXY((m_lastX + text.length()), m_lastY); - - return text.length(); - } - - /** - * Print the unsigned 16-bit value to the display at the current cursor position. - * - * @param[in] value Value - * - * @return Printed number of characters - */ - size_t print(uint16_t value) final - { - std::string text; - text = std::to_string(value); - - if (nullptr != m_display) - { - m_display->setColor(WHITE); - m_display->drawText(text, m_currX, m_currY); - } - - gotoXY((m_lastX + text.length()), m_lastY); - - return text.length(); - } - - /** - * Print the unsigned 32-bit value to the display at the current cursor position. - * - * @param[in] value Value - * - * @return Printed number of characters - */ - size_t print(uint32_t value) final - { - std::string text; - text = std::to_string(value); - - if (nullptr != m_display) - { - m_display->setColor(WHITE); - m_display->drawText(text, m_currX, m_currY); - } - - gotoXY((m_lastX + text.length()), m_lastY); - - return text.length(); - } - - /** - * Print the signed 8-bit value to the display at the current cursor position. - * - * @param[in] value Value - * - * @return Printed number of characters - */ - size_t print(int8_t value) final - { - std::string text; - text = std::to_string(value); - - if (nullptr != m_display) - { - m_display->setColor(WHITE); - m_display->drawText(text, m_currX, m_currY); - } - - gotoXY((m_lastX + text.length()), m_lastY); - - return text.length(); - } - - /** - * Print the signed 16-bit value to the display at the current cursor position. - * - * @param[in] value Value - * - * @return Printed number of characters - */ - size_t print(int16_t value) final - { - std::string text; - text = std::to_string(value); - - if (nullptr != m_display) - { - m_display->setColor(WHITE); - m_display->drawText(text, m_currX, m_currY); - } - - gotoXY((m_lastX + text.length()), m_lastY); - - return text.length(); - } - - /** - * Print the signed 32-bit value to the display at the current cursor position. - * - * @param[in] value Value - * - * @return Printed number of characters - */ - size_t print(int32_t value) final - { - std::string text; - text = std::to_string(value); - - if (nullptr != m_display) - { - m_display->setColor(WHITE); - m_display->drawText(text, m_currX, m_currY); - } - - gotoXY((m_lastX + text.length()), m_lastY); - - return text.length(); - } - -private: - - webots::Display* m_display; /**< The robot simulated display. */ - uint8_t m_height; /**< The display height. */ - uint8_t m_width; /**< The display width. */ - uint8_t m_currX; /**< Internal variable for the current pixel position on the simulated display. */ - uint8_t m_currY; /**< Internal variable for the current pixel position on the simulated display. */ - uint8_t m_lastX; /**< The last x-value passed to the goto function. */ - uint8_t m_lastY; /**< The last y-value passed to the goto function. */ - static const uint32_t WHITE = 0xFFFFFF; /**< The color white for the display text. */ - static const uint32_t BLACK = 0x000000; /**< The color black for the display background. */ - static const uint8_t DISPLAYLINES = 2; /**< The amount of lines the display has. */ - static const uint8_t DISPLAYCHARS = 8; /**< The characters per line the display has. */ - -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* DISPLAY_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Display realization + * @author Andreas Merkle + * + * @addtogroup HALSim + * + * @{ + */ + +#ifndef DISPLAY_H +#define DISPLAY_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "IDisplay.h" + +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** This class provides access to the roboto simulation LCD. */ +class Display : public IDisplay +{ +public: + /** + * Constructs the display adapter. + * + * @param[in] display The robot display. + */ + Display(webots::Display* display) : + IDisplay(), + m_display(display), + m_height(0), + m_width(0), + m_currX(0), + m_currY(0), + m_lastX(0), + m_lastY(0) + { + if (nullptr != display) + { + m_height = display->getHeight(); + m_width = display->getWidth(); + } + } + + /** + * Destroys the display adapter. + */ + ~Display() + { + } + + /** + * Clear the display and set the cursor to the upper left corner. + */ + void clear() final + { + if (nullptr != m_display) + { + m_display->setColor(BLACK); + m_display->fillRectangle(0, 0, m_width, m_height); + } + + gotoXY(0, 0); + } + + /** + * Set the cursor to the given position. + * + * @param[in] xCoord x-coordinate, 0 is the most left position. + * @param[in] yCoord y-coordinate, 0 is the most upper position. + */ + void gotoXY(uint8_t xCoord, uint8_t yCoord) final + { + /* The one gets added to have a better central allignment of the text. */ + m_currY = static_cast((m_height / (DISPLAYLINES + 1)) * (1 + yCoord)); + m_currX = static_cast((m_width / DISPLAYCHARS) * xCoord); + + m_lastX = xCoord; + m_lastY = yCoord; + } + + /** + * Print the string to the display at the current cursor position. + * + * @param[in] str String + * + * @return Printed number of characters + */ + size_t print(const char str[]) final + { + std::string text; + text = str; + + if (nullptr != m_display) + { + m_display->setColor(WHITE); + m_display->drawText(text, m_currX, m_currY); + } + + gotoXY((m_lastX + text.length()), m_lastY); + + return text.length(); + } + + /** + * Print the unsigned 8-bit value to the display at the current cursor position. + * + * @param[in] value Value + * + * @return Printed number of characters + */ + size_t print(uint8_t value) final + { + std::string text; + text = std::to_string(value); + + if (nullptr != m_display) + { + m_display->setColor(WHITE); + m_display->drawText(text, m_currX, m_currY); + } + + gotoXY((m_lastX + text.length()), m_lastY); + + return text.length(); + } + + /** + * Print the unsigned 16-bit value to the display at the current cursor position. + * + * @param[in] value Value + * + * @return Printed number of characters + */ + size_t print(uint16_t value) final + { + std::string text; + text = std::to_string(value); + + if (nullptr != m_display) + { + m_display->setColor(WHITE); + m_display->drawText(text, m_currX, m_currY); + } + + gotoXY((m_lastX + text.length()), m_lastY); + + return text.length(); + } + + /** + * Print the unsigned 32-bit value to the display at the current cursor position. + * + * @param[in] value Value + * + * @return Printed number of characters + */ + size_t print(uint32_t value) final + { + std::string text; + text = std::to_string(value); + + if (nullptr != m_display) + { + m_display->setColor(WHITE); + m_display->drawText(text, m_currX, m_currY); + } + + gotoXY((m_lastX + text.length()), m_lastY); + + return text.length(); + } + + /** + * Print the signed 8-bit value to the display at the current cursor position. + * + * @param[in] value Value + * + * @return Printed number of characters + */ + size_t print(int8_t value) final + { + std::string text; + text = std::to_string(value); + + if (nullptr != m_display) + { + m_display->setColor(WHITE); + m_display->drawText(text, m_currX, m_currY); + } + + gotoXY((m_lastX + text.length()), m_lastY); + + return text.length(); + } + + /** + * Print the signed 16-bit value to the display at the current cursor position. + * + * @param[in] value Value + * + * @return Printed number of characters + */ + size_t print(int16_t value) final + { + std::string text; + text = std::to_string(value); + + if (nullptr != m_display) + { + m_display->setColor(WHITE); + m_display->drawText(text, m_currX, m_currY); + } + + gotoXY((m_lastX + text.length()), m_lastY); + + return text.length(); + } + + /** + * Print the signed 32-bit value to the display at the current cursor position. + * + * @param[in] value Value + * + * @return Printed number of characters + */ + size_t print(int32_t value) final + { + std::string text; + text = std::to_string(value); + + if (nullptr != m_display) + { + m_display->setColor(WHITE); + m_display->drawText(text, m_currX, m_currY); + } + + gotoXY((m_lastX + text.length()), m_lastY); + + return text.length(); + } + +private: + + webots::Display* m_display; /**< The robot simulated display. */ + uint8_t m_height; /**< The display height. */ + uint8_t m_width; /**< The display width. */ + uint8_t m_currX; /**< Internal variable for the current pixel position on the simulated display. */ + uint8_t m_currY; /**< Internal variable for the current pixel position on the simulated display. */ + uint8_t m_lastX; /**< The last x-value passed to the goto function. */ + uint8_t m_lastY; /**< The last y-value passed to the goto function. */ + static const uint32_t WHITE = 0xFFFFFF; /**< The color white for the display text. */ + static const uint32_t BLACK = 0x000000; /**< The color black for the display background. */ + static const uint8_t DISPLAYLINES = 2; /**< The amount of lines the display has. */ + static const uint8_t DISPLAYCHARS = 8; /**< The characters per line the display has. */ + +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* DISPLAY_H */ +/** @} */ diff --git a/lib/HALSim/Encoders.cpp b/lib/HALSim/Encoders.cpp index f4350f31..da2fec9b 100644 --- a/lib/HALSim/Encoders.cpp +++ b/lib/HALSim/Encoders.cpp @@ -1,192 +1,192 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Encoders realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "Encoders.h" -#include "RobotConstants.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void Encoders::init() -{ - if (nullptr != m_posSensorLeft) - { - /* If robot reconnets, the position sensor will provide its position. - * Ensure that the left encoder will start at 0. - */ - m_lastResetValueLeft = m_posSensorLeft->getValue(); - } - - if (nullptr != m_posSensorRight) - { - /* If robot reconnets, the position sensor will provide its position. - * Ensure that the right encoder will start at 0. - */ - m_lastResetValueRight = m_posSensorRight->getValue(); - } -} - -int16_t Encoders::getCountsLeft() -{ - int16_t steps = 0; - - /* The position sensor provides the driven distance in [m]. - * The encoder steps are dervived from it. - */ - if (nullptr != m_posSensorLeft) - { - double currentPos = m_posSensorLeft->getValue(); - - overflowProtection(m_lastResetValueLeft, currentPos); - steps = calculateSteps(m_lastResetValueLeft, currentPos); - } - - return steps; -} - -int16_t Encoders::getCountsRight() -{ - int16_t steps = 0; - - /* The position sensor provides the driven distance in [m]. - * The encoder steps are dervived from it. - */ - if (nullptr != m_posSensorRight) - { - double currentPos = m_posSensorRight->getValue(); - - overflowProtection(m_lastResetValueRight, currentPos); - steps = calculateSteps(m_lastResetValueRight, currentPos); - } - - return steps; -} - -int16_t Encoders::getCountsAndResetLeft() -{ - int16_t steps = getCountsLeft(); - - m_lastResetValueLeft = m_posSensorLeft->getValue(); - - return steps; -} - -int16_t Encoders::getCountsAndResetRight() -{ - int16_t steps = getCountsRight(); - - m_lastResetValueRight = m_posSensorRight->getValue(); - - return steps; -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -void Encoders::overflowProtection(double& lastPos, double pos) -{ - /* Position sensor provides NAN until the first simulation step was perfomed. */ - if (false == isnan(pos)) - { - const double OVERFLOW_DELTA_POS = - static_cast(UINT16_MAX) / static_cast(RobotConstants::ENCODER_STEPS_PER_M); /* [m] */ - double deltaPosM = pos - lastPos; /* [m] */ - - /* Protect against delta position overflow (16-bit). */ - if (0.0F <= deltaPosM) - { - if (OVERFLOW_DELTA_POS <= deltaPosM) - { - lastPos += OVERFLOW_DELTA_POS; - } - } - else - { - if (OVERFLOW_DELTA_POS <= (-deltaPosM)) - { - lastPos -= OVERFLOW_DELTA_POS; - } - } - } -} - -int16_t Encoders::calculateSteps(double lastPos, double pos) const -{ - int16_t steps = 0; /* [steps] */ - - /* Position sensor provides NAN until the first simulation step was perfomed. */ - if (false == isnan(pos)) - { - double deltaPos = pos - lastPos; /* [m] */ - double encoderSteps = (deltaPos * static_cast(RobotConstants::ENCODER_STEPS_PER_M)); /* [steps] */ - - steps = static_cast(encoderSteps); /* [steps] */ - } - - return steps; -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Encoders realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "Encoders.h" +#include "RobotConstants.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void Encoders::init() +{ + if (nullptr != m_posSensorLeft) + { + /* If robot reconnets, the position sensor will provide its position. + * Ensure that the left encoder will start at 0. + */ + m_lastResetValueLeft = m_posSensorLeft->getValue(); + } + + if (nullptr != m_posSensorRight) + { + /* If robot reconnets, the position sensor will provide its position. + * Ensure that the right encoder will start at 0. + */ + m_lastResetValueRight = m_posSensorRight->getValue(); + } +} + +int16_t Encoders::getCountsLeft() +{ + int16_t steps = 0; + + /* The position sensor provides the driven distance in [m]. + * The encoder steps are dervived from it. + */ + if (nullptr != m_posSensorLeft) + { + double currentPos = m_posSensorLeft->getValue(); + + overflowProtection(m_lastResetValueLeft, currentPos); + steps = calculateSteps(m_lastResetValueLeft, currentPos); + } + + return steps; +} + +int16_t Encoders::getCountsRight() +{ + int16_t steps = 0; + + /* The position sensor provides the driven distance in [m]. + * The encoder steps are dervived from it. + */ + if (nullptr != m_posSensorRight) + { + double currentPos = m_posSensorRight->getValue(); + + overflowProtection(m_lastResetValueRight, currentPos); + steps = calculateSteps(m_lastResetValueRight, currentPos); + } + + return steps; +} + +int16_t Encoders::getCountsAndResetLeft() +{ + int16_t steps = getCountsLeft(); + + m_lastResetValueLeft = m_posSensorLeft->getValue(); + + return steps; +} + +int16_t Encoders::getCountsAndResetRight() +{ + int16_t steps = getCountsRight(); + + m_lastResetValueRight = m_posSensorRight->getValue(); + + return steps; +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +void Encoders::overflowProtection(double& lastPos, double pos) +{ + /* Position sensor provides NAN until the first simulation step was perfomed. */ + if (false == isnan(pos)) + { + const double OVERFLOW_DELTA_POS = + static_cast(UINT16_MAX) / static_cast(RobotConstants::ENCODER_STEPS_PER_M); /* [m] */ + double deltaPosM = pos - lastPos; /* [m] */ + + /* Protect against delta position overflow (16-bit). */ + if (0.0F <= deltaPosM) + { + if (OVERFLOW_DELTA_POS <= deltaPosM) + { + lastPos += OVERFLOW_DELTA_POS; + } + } + else + { + if (OVERFLOW_DELTA_POS <= (-deltaPosM)) + { + lastPos -= OVERFLOW_DELTA_POS; + } + } + } +} + +int16_t Encoders::calculateSteps(double lastPos, double pos) const +{ + int16_t steps = 0; /* [steps] */ + + /* Position sensor provides NAN until the first simulation step was perfomed. */ + if (false == isnan(pos)) + { + double deltaPos = pos - lastPos; /* [m] */ + double encoderSteps = (deltaPos * static_cast(RobotConstants::ENCODER_STEPS_PER_M)); /* [steps] */ + + steps = static_cast(encoderSteps); /* [steps] */ + } + + return steps; +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALSim/Encoders.h b/lib/HALSim/Encoders.h index 7274db32..e71b59ca 100644 --- a/lib/HALSim/Encoders.h +++ b/lib/HALSim/Encoders.h @@ -1,171 +1,171 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Encoders realization - * @author Andreas Merkle - * - * @addtogroup HALSim - * - * @{ - */ - -#ifndef ENCODERS_H -#define ENCODERS_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "IEncoders.h" -#include "SimTime.h" - -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class provides access to the Zumo target encoders. */ -class Encoders : public IEncoders -{ -public: - /** - * Constructs the encoders adapter. - * - * @param[in] posSensorLeft The left position sensor - * @param[in] posSensorRight The right position sensor - */ - Encoders(webots::PositionSensor* posSensorLeft, webots::PositionSensor* posSensorRight) : - IEncoders(), - m_posSensorLeft(posSensorLeft), - m_posSensorRight(posSensorRight), - m_lastResetValueLeft(0.0f), - m_lastResetValueRight(0.0f) - { - } - - /** - * Destroys the encoders adapter. - */ - ~Encoders() - { - } - - /** - * Initialize or re-initialize the encoders. - * This is used e.g. to re-initialize the encoders after the simulation world - * was reset, but the ext. robot is still active. - */ - void init() final; - - /** - * Returns the counts of the encoder of the left-side motor. - * Internal calculation are done because the positionSensor - * in webots that acts as an encoder only returns the driven - * way in m/s and not the counted steps. - * - * @return Encoder steps left - */ - int16_t getCountsLeft() final; - - /** - * Returns the counts of the encoder of the right-side motor. - * Internal calculation are done because the positionSensor - * in webots that acts as an encoder only returns the driven - * way in m/s and not the counted steps. - * - * @return Encoder steps right - */ - int16_t getCountsRight() final; - - /** - * This function is just like getCountsLeft() except it also clears the - * counts before returning. If you call this frequently enough, you will - * not have to worry about the count overflowing. - * - * @return Encoder steps left - */ - int16_t getCountsAndResetLeft() final; - - /** - * This function is just like getCountsRight() except it also clears the - * counts before returning. If you call this frequently enough, you will - * not have to worry about the count overflowing. - * - * @return Encoder steps right - */ - int16_t getCountsAndResetRight() final; - -private: - /** The position sensor of the left motor in the robot simulation. */ - webots::PositionSensor* m_posSensorLeft; - - /** The position sensor of the right motor in the robot simulation. */ - webots::PositionSensor* m_posSensorRight; - - /** Last position value of the left sensor in [m], used as reference. */ - double m_lastResetValueLeft; - - /** Last position value of the right sensor in [m], used as reference. */ - double m_lastResetValueRight; - - /** - * The position sensor provides a distance as double and in [m]. - * The target system provides the distance as int16_t and in [steps]. - * Calling this method will prevent that the difference between the - * reference point and the current position will overflow the target - * data type. - * - * @param[in,out] lastPos Last position in [m] - * @param[in] pos Current position in [m] - */ - void overflowProtection(double& lastPos, double pos); - - /** - * Calculate the absolute number of encoder steps by position change. - * - * @param[in] lastPos Last position in [m] - * @param[in] pos Current position in [m] - * - * @return Absolute number of encoder steps - */ - int16_t calculateSteps(double lastPos, double pos) const; -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* ENCODERS_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Encoders realization + * @author Andreas Merkle + * + * @addtogroup HALSim + * + * @{ + */ + +#ifndef ENCODERS_H +#define ENCODERS_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "IEncoders.h" +#include "SimTime.h" + +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** This class provides access to the Zumo target encoders. */ +class Encoders : public IEncoders +{ +public: + /** + * Constructs the encoders adapter. + * + * @param[in] posSensorLeft The left position sensor + * @param[in] posSensorRight The right position sensor + */ + Encoders(webots::PositionSensor* posSensorLeft, webots::PositionSensor* posSensorRight) : + IEncoders(), + m_posSensorLeft(posSensorLeft), + m_posSensorRight(posSensorRight), + m_lastResetValueLeft(0.0f), + m_lastResetValueRight(0.0f) + { + } + + /** + * Destroys the encoders adapter. + */ + ~Encoders() + { + } + + /** + * Initialize or re-initialize the encoders. + * This is used e.g. to re-initialize the encoders after the simulation world + * was reset, but the ext. robot is still active. + */ + void init() final; + + /** + * Returns the counts of the encoder of the left-side motor. + * Internal calculation are done because the positionSensor + * in webots that acts as an encoder only returns the driven + * way in m/s and not the counted steps. + * + * @return Encoder steps left + */ + int16_t getCountsLeft() final; + + /** + * Returns the counts of the encoder of the right-side motor. + * Internal calculation are done because the positionSensor + * in webots that acts as an encoder only returns the driven + * way in m/s and not the counted steps. + * + * @return Encoder steps right + */ + int16_t getCountsRight() final; + + /** + * This function is just like getCountsLeft() except it also clears the + * counts before returning. If you call this frequently enough, you will + * not have to worry about the count overflowing. + * + * @return Encoder steps left + */ + int16_t getCountsAndResetLeft() final; + + /** + * This function is just like getCountsRight() except it also clears the + * counts before returning. If you call this frequently enough, you will + * not have to worry about the count overflowing. + * + * @return Encoder steps right + */ + int16_t getCountsAndResetRight() final; + +private: + /** The position sensor of the left motor in the robot simulation. */ + webots::PositionSensor* m_posSensorLeft; + + /** The position sensor of the right motor in the robot simulation. */ + webots::PositionSensor* m_posSensorRight; + + /** Last position value of the left sensor in [m], used as reference. */ + double m_lastResetValueLeft; + + /** Last position value of the right sensor in [m], used as reference. */ + double m_lastResetValueRight; + + /** + * The position sensor provides a distance as double and in [m]. + * The target system provides the distance as int16_t and in [steps]. + * Calling this method will prevent that the difference between the + * reference point and the current position will overflow the target + * data type. + * + * @param[in,out] lastPos Last position in [m] + * @param[in] pos Current position in [m] + */ + void overflowProtection(double& lastPos, double pos); + + /** + * Calculate the absolute number of encoder steps by position change. + * + * @param[in] lastPos Last position in [m] + * @param[in] pos Current position in [m] + * + * @return Absolute number of encoder steps + */ + int16_t calculateSteps(double lastPos, double pos) const; +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* ENCODERS_H */ +/** @} */ diff --git a/lib/HALSim/IMU.cpp b/lib/HALSim/IMU.cpp index 027f4fc6..7f996650 100644 --- a/lib/HALSim/IMU.cpp +++ b/lib/HALSim/IMU.cpp @@ -1,178 +1,178 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief 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() -{ - /* Nothing to do. */ - return true; -} - -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() -{ - /* Nothing to do. */ -} - -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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief 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() +{ + /* Nothing to do. */ + return true; +} + +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() +{ + /* Nothing to do. */ +} + +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 789df831..063cdb42 100644 --- a/lib/HALSim/IMU.h +++ b/lib/HALSim/IMU.h @@ -1,214 +1,214 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief 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] accelerometer The accelerometer - * @param[in] gyro The gyro - * @param[in] compass The compass - */ - IMU(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_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() final; - - /** - * Enables all of the inertial sensors with a default configuration. - */ - void enableDefault() final - { - } - - /** - * Configures the sensors with settings optimized for turn sensing. - */ - void configureForTurnSensing() final - { - } - - /** - * Takes a reading from the accelerometer and makes the measurements available in a. - */ - void readAccelerometer() final; - - /** - * Takes a reading from the gyro and makes the measurements available in g. - */ - void readGyro() final; - - /** - * Takes a reading from the magnetometer and makes the measurements available in m. - */ - void readMagnetometer() final; - - /** - * Indicates whether the accelerometer has new measurement data ready. - * - * @return True if there is new accelerometer data available; false otherwise. - */ - bool accelerometerDataReady() final - { - return true; - } - - /** - * Indicates whether the gyro has new measurement data ready. - * - * @return True if there is new gyro data available; false otherwise. - */ - bool gyroDataReady() final - { - return true; - } - - /** - * Indicates whether the magnetometer has new measurement data ready. - * - * @return True if there is new magnetometer data available; false otherwise. - */ - bool magnetometerDataReady() final - { - 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) final; - - /** - * 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) final; - - /** - * 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) final; - - /** - * Calibrate the IMU. - */ - void calibrate() final; - -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. */ - - 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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief 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] accelerometer The accelerometer + * @param[in] gyro The gyro + * @param[in] compass The compass + */ + IMU(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_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() final; + + /** + * Enables all of the inertial sensors with a default configuration. + */ + void enableDefault() final + { + } + + /** + * Configures the sensors with settings optimized for turn sensing. + */ + void configureForTurnSensing() final + { + } + + /** + * Takes a reading from the accelerometer and makes the measurements available in a. + */ + void readAccelerometer() final; + + /** + * Takes a reading from the gyro and makes the measurements available in g. + */ + void readGyro() final; + + /** + * Takes a reading from the magnetometer and makes the measurements available in m. + */ + void readMagnetometer() final; + + /** + * Indicates whether the accelerometer has new measurement data ready. + * + * @return True if there is new accelerometer data available; false otherwise. + */ + bool accelerometerDataReady() final + { + return true; + } + + /** + * Indicates whether the gyro has new measurement data ready. + * + * @return True if there is new gyro data available; false otherwise. + */ + bool gyroDataReady() final + { + return true; + } + + /** + * Indicates whether the magnetometer has new measurement data ready. + * + * @return True if there is new magnetometer data available; false otherwise. + */ + bool magnetometerDataReady() final + { + 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) final; + + /** + * 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) final; + + /** + * 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) final; + + /** + * Calibrate the IMU. + */ + void calibrate() final; + +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. */ + + 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/HALSim/Keyboard.cpp b/lib/HALSim/Keyboard.cpp index c968ad47..8b9cd123 100644 --- a/lib/HALSim/Keyboard.cpp +++ b/lib/HALSim/Keyboard.cpp @@ -1,124 +1,124 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Keyboard realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "Keyboard.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -bool Keyboard::isButtonPressed(char lowerCaseChar, char upperCaseChar) const -{ - bool buttonPressed = false; - - /* Checks if button is existing in the new values, but not the old ones. - * If so, it's newly pressed and true is returned. - */ - if ((false == arrayContains(m_oldKeys, sizeof(m_oldKeys), upperCaseChar, lowerCaseChar)) && - (true == arrayContains(m_newKeys, sizeof(m_newKeys), upperCaseChar, lowerCaseChar))) - { - buttonPressed = true; - } - - return buttonPressed; -} - -bool Keyboard::isButtonReleased(char lowerCaseChar, char upperCaseChar) const -{ - bool buttonReleased = false; - - /* Checks if button is existing in the new values, but not the old ones. - * If so, it's newly released and true is returned. - */ - if ((false == arrayContains(m_newKeys, sizeof(m_newKeys), upperCaseChar, lowerCaseChar)) && - (true == arrayContains(m_oldKeys, sizeof(m_oldKeys), upperCaseChar, lowerCaseChar))) - { - buttonReleased = true; - } - - return buttonReleased; -} - -bool Keyboard::arrayContains(const uint16_t array[], uint16_t arraySize, char elemLowerCase, char elemUppercase) const -{ - bool elementFound = false; - - for (uint8_t arrayIndex = 0; arrayIndex < (arraySize / sizeof(*array)); ++arrayIndex) - { - if ((array[arrayIndex] == static_cast(elemLowerCase)) || - (array[arrayIndex] == static_cast(elemUppercase))) - { - elementFound = true; - ; - } - } - - return elementFound; -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Keyboard realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "Keyboard.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +bool Keyboard::isButtonPressed(char lowerCaseChar, char upperCaseChar) const +{ + bool buttonPressed = false; + + /* Checks if button is existing in the new values, but not the old ones. + * If so, it's newly pressed and true is returned. + */ + if ((false == arrayContains(m_oldKeys, sizeof(m_oldKeys), upperCaseChar, lowerCaseChar)) && + (true == arrayContains(m_newKeys, sizeof(m_newKeys), upperCaseChar, lowerCaseChar))) + { + buttonPressed = true; + } + + return buttonPressed; +} + +bool Keyboard::isButtonReleased(char lowerCaseChar, char upperCaseChar) const +{ + bool buttonReleased = false; + + /* Checks if button is existing in the new values, but not the old ones. + * If so, it's newly released and true is returned. + */ + if ((false == arrayContains(m_newKeys, sizeof(m_newKeys), upperCaseChar, lowerCaseChar)) && + (true == arrayContains(m_oldKeys, sizeof(m_oldKeys), upperCaseChar, lowerCaseChar))) + { + buttonReleased = true; + } + + return buttonReleased; +} + +bool Keyboard::arrayContains(const uint16_t array[], uint16_t arraySize, char elemLowerCase, char elemUppercase) const +{ + bool elementFound = false; + + for (uint8_t arrayIndex = 0; arrayIndex < (arraySize / sizeof(*array)); ++arrayIndex) + { + if ((array[arrayIndex] == static_cast(elemLowerCase)) || + (array[arrayIndex] == static_cast(elemUppercase))) + { + elementFound = true; + ; + } + } + + return elementFound; +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALSim/Keyboard.h b/lib/HALSim/Keyboard.h index e6929c16..93db7b3a 100644 --- a/lib/HALSim/Keyboard.h +++ b/lib/HALSim/Keyboard.h @@ -1,287 +1,287 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Keyboard realization - * @author Andreas Merkle - * - * @addtogroup HALSim - * - * @{ - */ - -#ifndef KEYBOARD_H -#define KEYBOARD_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include - -#include "SimTime.h" - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class implements a custom keyboard for better functionality. */ -class Keyboard -{ -public: - /** - * Constructs the encoders adapter and initialize it. - */ - Keyboard(SimTime& simTime, webots::Keyboard* keyboard) : - m_oldKeys(), - m_newKeys(), - m_simTime(simTime), - m_keyboard(keyboard) - { - } - - /** - * Destroys the encoders adapter. - */ - ~Keyboard() - { - } - - /** - * Gets the current buttons pressed on the keyboard. Needs to be called - * cyclically to work correct. - */ - void getPressedButtons() - { - /* Copying the new values into the old values array. */ - for (uint8_t arrayIndex = 0; arrayIndex < (sizeof(m_oldKeys) / sizeof(*m_oldKeys)); ++arrayIndex) - { - m_oldKeys[arrayIndex] = m_newKeys[arrayIndex]; - } - - /* Getting the new values. Currently the limit of the simulation is seven - * keypresses that can be detected simultaniously. - */ - for (uint8_t keyIndex = 0; keyIndex < (sizeof(m_oldKeys) / sizeof(*m_oldKeys)); ++keyIndex) - { - m_newKeys[keyIndex] = m_keyboard->getKey(); - } - } - - /** - * Checks weather the button A was pressed since the last update. - * - * @return Return true if button A was pressed since the last update - */ - bool buttonAPressed() - { - return isButtonPressed(KEY_CODE_A_LOWER_CASE, KEY_CODE_A_UPPER_CASE); - } - - /** - * Checks weather the button A was released since the last update. - * - * @return Return true if button A was released since the last update - */ - bool buttonAReleased() - { - return isButtonReleased(KEY_CODE_A_LOWER_CASE, KEY_CODE_A_UPPER_CASE); - } - - /** - * Checks weather the button B was pressed since the last update. - * - * @return Return true if button B was pressed since the last update - */ - bool buttonBPressed() - { - return isButtonPressed(KEY_CODE_B_LOWER_CASE, KEY_CODE_B_UPPER_CASE); - } - - /** - * Checks weather the button B was released since the last update. - * - * @return Return true if button B was released since the last update - */ - bool buttonBReleased() - { - return isButtonReleased(KEY_CODE_B_LOWER_CASE, KEY_CODE_B_UPPER_CASE); - } - - /** - * Checks weather the button C was pressed since the last update. - * - * @return Return true if button C was pressed since the last update - */ - bool buttonCPressed() - { - return isButtonPressed(KEY_CODE_C_LOWER_CASE, KEY_CODE_C_UPPER_CASE); - } - - /** - * Checks weather the button C was released since the last update. - * - * @return Return true if button C was released since the last update - */ - bool buttonCReleased() - { - return isButtonReleased(KEY_CODE_C_LOWER_CASE, KEY_CODE_C_UPPER_CASE); - } - - /** - * Waits until Button A gets released. - * Needs to call the robots step() method and getPressedButtons() - * to update the keypresses correctly and avoid getting stuck - * in the while loop. - */ - void waitForReleaseA() - { - while (!buttonAReleased()) - { - if (false == m_simTime.step()) - { - break; - } - getPressedButtons(); - } - } - - /** - * Waits until Button B gets released. - * Needs to call the robots step() method and getPressedButtons() - * to update the keypresses correctly and avoid getting stuck - * in the while loop. - */ - void waitForReleaseB() - { - while (!buttonBReleased()) - { - if (false == m_simTime.step()) - { - break; - } - getPressedButtons(); - } - } - - /** - * Waits until Button C gets released. - * Needs to call the robots step() method and getPressedButtons() - * to update the keypresses correctly and avoid getting stuck - * in the while loop. - */ - void waitForReleaseC() - { - while (!buttonCReleased()) - { - if (false == m_simTime.step()) - { - break; - } - getPressedButtons(); - } - } - -private: - /** The key code of the lower case a character, which simulates the button. */ - static const char KEY_CODE_A_LOWER_CASE = 'a'; - - /** The key code of the upper case A character, which simulates the button. */ - static const char KEY_CODE_A_UPPER_CASE = 'A'; - - /** The key code of the lower case b character, which simulates the button. */ - static const char KEY_CODE_B_LOWER_CASE = 'b'; - - /** The key code of the upper case B character, which simulates the button. */ - static const char KEY_CODE_B_UPPER_CASE = 'B'; - - /** The key code of the lower case c character, which simulates the button. */ - static const char KEY_CODE_C_LOWER_CASE = 'c'; - - /** The key code of the upper case C character, which simulates the button. */ - static const char KEY_CODE_C_UPPER_CASE = 'C'; - - /** The maximum number of keys pressed simultaniously, that the simulation can process. */ - static const uint8_t MAX_KEY_NUMBER = 7; - - /** The keys presses during the last update. */ - uint16_t m_oldKeys[MAX_KEY_NUMBER]; - - /** The keys pressed during this update. */ - uint16_t m_newKeys[MAX_KEY_NUMBER]; - - webots::Keyboard* m_keyboard; /**< Robot keyboard */ - - SimTime& m_simTime; /**< Simulation time */ - - /** - * Is the button pressed? - * - * @param[in] lowerCaseChar Lower case character ASCII value - * @param[in] upperCaseChar Upper case character ASCII value - * - * @return If pressed, it will return true otherwise false. - */ - bool isButtonPressed(char lowerCaseChar, char upperCaseChar) const; - - /** - * Is the button released? - * - * @param[in] lowerCaseChar Lower case character ASCII value - * @param[in] upperCaseChar Upper case character ASCII value - * - * @return If released, it will return true otherwise false. - */ - bool isButtonReleased(char lowerCaseChar, char upperCaseChar) const; - - /** - * Checks whether the given array contains a element. - * - * @param[in] array The array where to search for the element. - * @param[in] arraySize Number of array elements. - * @param[in] elemLowerCase Lower case character - * @param[in] elemUppercase Upper case character - * - * @return If found, it will return true otherwise false. - */ - bool arrayContains(const uint16_t array[], uint16_t arraySize, char elemLowerCase, char elemUppercase) const; -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* KEYBOARD_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Keyboard realization + * @author Andreas Merkle + * + * @addtogroup HALSim + * + * @{ + */ + +#ifndef KEYBOARD_H +#define KEYBOARD_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include + +#include "SimTime.h" + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** This class implements a custom keyboard for better functionality. */ +class Keyboard +{ +public: + /** + * Constructs the encoders adapter and initialize it. + */ + Keyboard(SimTime& simTime, webots::Keyboard* keyboard) : + m_oldKeys(), + m_newKeys(), + m_simTime(simTime), + m_keyboard(keyboard) + { + } + + /** + * Destroys the encoders adapter. + */ + ~Keyboard() + { + } + + /** + * Gets the current buttons pressed on the keyboard. Needs to be called + * cyclically to work correct. + */ + void getPressedButtons() + { + /* Copying the new values into the old values array. */ + for (uint8_t arrayIndex = 0; arrayIndex < (sizeof(m_oldKeys) / sizeof(*m_oldKeys)); ++arrayIndex) + { + m_oldKeys[arrayIndex] = m_newKeys[arrayIndex]; + } + + /* Getting the new values. Currently the limit of the simulation is seven + * keypresses that can be detected simultaniously. + */ + for (uint8_t keyIndex = 0; keyIndex < (sizeof(m_oldKeys) / sizeof(*m_oldKeys)); ++keyIndex) + { + m_newKeys[keyIndex] = m_keyboard->getKey(); + } + } + + /** + * Checks weather the button A was pressed since the last update. + * + * @return Return true if button A was pressed since the last update + */ + bool buttonAPressed() + { + return isButtonPressed(KEY_CODE_A_LOWER_CASE, KEY_CODE_A_UPPER_CASE); + } + + /** + * Checks weather the button A was released since the last update. + * + * @return Return true if button A was released since the last update + */ + bool buttonAReleased() + { + return isButtonReleased(KEY_CODE_A_LOWER_CASE, KEY_CODE_A_UPPER_CASE); + } + + /** + * Checks weather the button B was pressed since the last update. + * + * @return Return true if button B was pressed since the last update + */ + bool buttonBPressed() + { + return isButtonPressed(KEY_CODE_B_LOWER_CASE, KEY_CODE_B_UPPER_CASE); + } + + /** + * Checks weather the button B was released since the last update. + * + * @return Return true if button B was released since the last update + */ + bool buttonBReleased() + { + return isButtonReleased(KEY_CODE_B_LOWER_CASE, KEY_CODE_B_UPPER_CASE); + } + + /** + * Checks weather the button C was pressed since the last update. + * + * @return Return true if button C was pressed since the last update + */ + bool buttonCPressed() + { + return isButtonPressed(KEY_CODE_C_LOWER_CASE, KEY_CODE_C_UPPER_CASE); + } + + /** + * Checks weather the button C was released since the last update. + * + * @return Return true if button C was released since the last update + */ + bool buttonCReleased() + { + return isButtonReleased(KEY_CODE_C_LOWER_CASE, KEY_CODE_C_UPPER_CASE); + } + + /** + * Waits until Button A gets released. + * Needs to call the robots step() method and getPressedButtons() + * to update the keypresses correctly and avoid getting stuck + * in the while loop. + */ + void waitForReleaseA() + { + while (!buttonAReleased()) + { + if (false == m_simTime.step()) + { + break; + } + getPressedButtons(); + } + } + + /** + * Waits until Button B gets released. + * Needs to call the robots step() method and getPressedButtons() + * to update the keypresses correctly and avoid getting stuck + * in the while loop. + */ + void waitForReleaseB() + { + while (!buttonBReleased()) + { + if (false == m_simTime.step()) + { + break; + } + getPressedButtons(); + } + } + + /** + * Waits until Button C gets released. + * Needs to call the robots step() method and getPressedButtons() + * to update the keypresses correctly and avoid getting stuck + * in the while loop. + */ + void waitForReleaseC() + { + while (!buttonCReleased()) + { + if (false == m_simTime.step()) + { + break; + } + getPressedButtons(); + } + } + +private: + /** The key code of the lower case a character, which simulates the button. */ + static const char KEY_CODE_A_LOWER_CASE = 'a'; + + /** The key code of the upper case A character, which simulates the button. */ + static const char KEY_CODE_A_UPPER_CASE = 'A'; + + /** The key code of the lower case b character, which simulates the button. */ + static const char KEY_CODE_B_LOWER_CASE = 'b'; + + /** The key code of the upper case B character, which simulates the button. */ + static const char KEY_CODE_B_UPPER_CASE = 'B'; + + /** The key code of the lower case c character, which simulates the button. */ + static const char KEY_CODE_C_LOWER_CASE = 'c'; + + /** The key code of the upper case C character, which simulates the button. */ + static const char KEY_CODE_C_UPPER_CASE = 'C'; + + /** The maximum number of keys pressed simultaniously, that the simulation can process. */ + static const uint8_t MAX_KEY_NUMBER = 7; + + /** The keys presses during the last update. */ + uint16_t m_oldKeys[MAX_KEY_NUMBER]; + + /** The keys pressed during this update. */ + uint16_t m_newKeys[MAX_KEY_NUMBER]; + + webots::Keyboard* m_keyboard; /**< Robot keyboard */ + + SimTime& m_simTime; /**< Simulation time */ + + /** + * Is the button pressed? + * + * @param[in] lowerCaseChar Lower case character ASCII value + * @param[in] upperCaseChar Upper case character ASCII value + * + * @return If pressed, it will return true otherwise false. + */ + bool isButtonPressed(char lowerCaseChar, char upperCaseChar) const; + + /** + * Is the button released? + * + * @param[in] lowerCaseChar Lower case character ASCII value + * @param[in] upperCaseChar Upper case character ASCII value + * + * @return If released, it will return true otherwise false. + */ + bool isButtonReleased(char lowerCaseChar, char upperCaseChar) const; + + /** + * Checks whether the given array contains a element. + * + * @param[in] array The array where to search for the element. + * @param[in] arraySize Number of array elements. + * @param[in] elemLowerCase Lower case character + * @param[in] elemUppercase Upper case character + * + * @return If found, it will return true otherwise false. + */ + bool arrayContains(const uint16_t array[], uint16_t arraySize, char elemLowerCase, char elemUppercase) const; +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* KEYBOARD_H */ +/** @} */ diff --git a/lib/HALSim/LedGreen.cpp b/lib/HALSim/LedGreen.cpp index e87baaa8..d373d57d 100644 --- a/lib/HALSim/LedGreen.cpp +++ b/lib/HALSim/LedGreen.cpp @@ -1,90 +1,90 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Green LED realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "LedGreen.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void LedGreen::enable(bool enableIt) -{ - if (nullptr != m_led) - { - if (false == enableIt) - { - m_led->set(COLOR_OFF); - } - else - { - m_led->set(COLOR_ON); - } - } -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Green LED realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "LedGreen.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void LedGreen::enable(bool enableIt) +{ + if (nullptr != m_led) + { + if (false == enableIt) + { + m_led->set(COLOR_OFF); + } + else + { + m_led->set(COLOR_ON); + } + } +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALSim/LedGreen.h b/lib/HALSim/LedGreen.h index 63739ebb..1c24fd85 100644 --- a/lib/HALSim/LedGreen.h +++ b/lib/HALSim/LedGreen.h @@ -1,105 +1,105 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Green LED realization - * @author Andreas Merkle - * - * @addtogroup HALSim - * - * @{ - */ - -#ifndef LEDGREEN_H -#define LEDGREEN_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ILed.h" - -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class provides access to the Zumo target green LED. */ -class LedGreen : public ILed -{ -public: - /** - * Constructs the green LED adapter. - * - * @param[in] led The green LED of the simulated robot. - */ - LedGreen(webots::LED* led) : - ILed(), - m_led(led) - { - } - - /** - * Destroys the green LED adapter. - */ - ~LedGreen() - { - } - - /** - * Enables/Disables the LED. - * - * @param[in] enableIt Enable LED with true, disable it with false. - */ - void enable(bool enableIt) final; - -private: - /** The intensity of the monochromatic LED in the off state. */ - static const int COLOR_OFF = 0; - - /** The intensity of the monochromatic LED in the on state */ - static const int COLOR_ON = 255; - - webots::LED* m_led; /**< The LED on the simulated robot. */ - - /* Default constructor not allowed. */ - LedGreen(); -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* LEDGREEN_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Green LED realization + * @author Andreas Merkle + * + * @addtogroup HALSim + * + * @{ + */ + +#ifndef LEDGREEN_H +#define LEDGREEN_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ILed.h" + +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** This class provides access to the Zumo target green LED. */ +class LedGreen : public ILed +{ +public: + /** + * Constructs the green LED adapter. + * + * @param[in] led The green LED of the simulated robot. + */ + LedGreen(webots::LED* led) : + ILed(), + m_led(led) + { + } + + /** + * Destroys the green LED adapter. + */ + ~LedGreen() + { + } + + /** + * Enables/Disables the LED. + * + * @param[in] enableIt Enable LED with true, disable it with false. + */ + void enable(bool enableIt) final; + +private: + /** The intensity of the monochromatic LED in the off state. */ + static const int COLOR_OFF = 0; + + /** The intensity of the monochromatic LED in the on state */ + static const int COLOR_ON = 255; + + webots::LED* m_led; /**< The LED on the simulated robot. */ + + /* Default constructor not allowed. */ + LedGreen(); +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* LEDGREEN_H */ +/** @} */ diff --git a/lib/HALSim/LedRed.cpp b/lib/HALSim/LedRed.cpp index b306c686..a5047a61 100644 --- a/lib/HALSim/LedRed.cpp +++ b/lib/HALSim/LedRed.cpp @@ -1,90 +1,90 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Red LED realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "LedRed.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void LedRed::enable(bool enableIt) -{ - if (nullptr != m_led) - { - if (false == enableIt) - { - m_led->set(COLOR_OFF); - } - else - { - m_led->set(COLOR_ON); - } - } -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Red LED realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "LedRed.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void LedRed::enable(bool enableIt) +{ + if (nullptr != m_led) + { + if (false == enableIt) + { + m_led->set(COLOR_OFF); + } + else + { + m_led->set(COLOR_ON); + } + } +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALSim/LedRed.h b/lib/HALSim/LedRed.h index e60c0c9b..6e831502 100644 --- a/lib/HALSim/LedRed.h +++ b/lib/HALSim/LedRed.h @@ -1,105 +1,105 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Red LED realization - * @author Andreas Merkle - * - * @addtogroup HALSim - * - * @{ - */ - -#ifndef LEDRED_H -#define LEDRED_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ILed.h" - -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class provides access to the Zumo target red LED. */ -class LedRed : public ILed -{ -public: - /** - * Constructs the red LED adapter. - * - * @param[in] led The red LED of the simulated robot. - */ - LedRed(webots::LED* led) : - ILed(), - m_led(led) - { - } - - /** - * Destroys the red LED adapter. - */ - ~LedRed() - { - } - - /** - * Enables/Disables the LED. - * - * @param[in] enableIt Enable LED with true, disable it with false. - */ - void enable(bool enableIt) final; - -private: - /** The intensity of the monochromatic LED in the off state. */ - static const int COLOR_OFF = 0; - - /** The intensity of the monochromatic LED in the on state. */ - static const int COLOR_ON = 255; - - webots::LED* m_led; /**< The LED on the simulated robot. */ - - /* Default constructor not allowed. */ - LedRed(); -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* LEDRED_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Red LED realization + * @author Andreas Merkle + * + * @addtogroup HALSim + * + * @{ + */ + +#ifndef LEDRED_H +#define LEDRED_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ILed.h" + +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** This class provides access to the Zumo target red LED. */ +class LedRed : public ILed +{ +public: + /** + * Constructs the red LED adapter. + * + * @param[in] led The red LED of the simulated robot. + */ + LedRed(webots::LED* led) : + ILed(), + m_led(led) + { + } + + /** + * Destroys the red LED adapter. + */ + ~LedRed() + { + } + + /** + * Enables/Disables the LED. + * + * @param[in] enableIt Enable LED with true, disable it with false. + */ + void enable(bool enableIt) final; + +private: + /** The intensity of the monochromatic LED in the off state. */ + static const int COLOR_OFF = 0; + + /** The intensity of the monochromatic LED in the on state. */ + static const int COLOR_ON = 255; + + webots::LED* m_led; /**< The LED on the simulated robot. */ + + /* Default constructor not allowed. */ + LedRed(); +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* LEDRED_H */ +/** @} */ diff --git a/lib/HALSim/LedYellow.cpp b/lib/HALSim/LedYellow.cpp index dbe09b4b..a480a2b8 100644 --- a/lib/HALSim/LedYellow.cpp +++ b/lib/HALSim/LedYellow.cpp @@ -1,90 +1,90 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Yellow LED realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "LedYellow.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void LedYellow::enable(bool enableIt) -{ - if (nullptr != m_led) - { - if (false == enableIt) - { - m_led->set(COLOR_OFF); - } - else - { - m_led->set(COLOR_ON); - } - } -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Yellow LED realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "LedYellow.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void LedYellow::enable(bool enableIt) +{ + if (nullptr != m_led) + { + if (false == enableIt) + { + m_led->set(COLOR_OFF); + } + else + { + m_led->set(COLOR_ON); + } + } +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALSim/LedYellow.h b/lib/HALSim/LedYellow.h index a46bffdf..72b83232 100644 --- a/lib/HALSim/LedYellow.h +++ b/lib/HALSim/LedYellow.h @@ -1,105 +1,105 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Yellow LED realization - * @author Andreas Merkle - * - * @addtogroup HALSim - * - * @{ - */ - -#ifndef LEDYELLOW_H -#define LEDYELLOW_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ILed.h" - -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class provides access to the Zumo target yellow LED. */ -class LedYellow : public ILed -{ -public: - /** - * Constructs the yellow LED adapter. - * - * @param[in] led The yellow LED of the simulated robot. - */ - LedYellow(webots::LED* led) : - ILed(), - m_led(led) - { - } - - /** - * Destroys the yellow LED adapter. - */ - ~LedYellow() - { - } - - /** - * Enables/Disables the LED. - * - * @param[in] enableIt Enable LED with true, disable it with false. - */ - void enable(bool enableIt) final; - -private: - /** The intensity of the monochromatic LED in the off state. */ - static const int COLOR_OFF = 0; - - /** The intensity of the monochromatic LED in the on state. */ - static const int COLOR_ON = 255; - - webots::LED* m_led; /**< The LED on the simulated robot. */ - - /* Default constructor not allowed. */ - LedYellow(); -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* LEDYELLOW_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Yellow LED realization + * @author Andreas Merkle + * + * @addtogroup HALSim + * + * @{ + */ + +#ifndef LEDYELLOW_H +#define LEDYELLOW_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ILed.h" + +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** This class provides access to the Zumo target yellow LED. */ +class LedYellow : public ILed +{ +public: + /** + * Constructs the yellow LED adapter. + * + * @param[in] led The yellow LED of the simulated robot. + */ + LedYellow(webots::LED* led) : + ILed(), + m_led(led) + { + } + + /** + * Destroys the yellow LED adapter. + */ + ~LedYellow() + { + } + + /** + * Enables/Disables the LED. + * + * @param[in] enableIt Enable LED with true, disable it with false. + */ + void enable(bool enableIt) final; + +private: + /** The intensity of the monochromatic LED in the off state. */ + static const int COLOR_OFF = 0; + + /** The intensity of the monochromatic LED in the on state. */ + static const int COLOR_ON = 255; + + webots::LED* m_led; /**< The LED on the simulated robot. */ + + /* Default constructor not allowed. */ + LedYellow(); +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* LEDYELLOW_H */ +/** @} */ diff --git a/lib/HALSim/LineSensors.cpp b/lib/HALSim/LineSensors.cpp index d53a46e6..64e6dfbf 100644 --- a/lib/HALSim/LineSensors.cpp +++ b/lib/HALSim/LineSensors.cpp @@ -1,220 +1,220 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Line sensors array realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "LineSensors.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void LineSensors::init() -{ - for (uint8_t sensorIndex = 0; sensorIndex < MAX_SENSORS; ++sensorIndex) - { - m_sensorValuesU16[sensorIndex] = 0; - m_sensorMaxValues[sensorIndex] = 0; - m_sensorMinValues[sensorIndex] = SENSOR_MAX_VALUE; - } -} - -void LineSensors::calibrate() -{ - (void)getSensorValues(); - - for (uint8_t sensorIndex = 0; sensorIndex < MAX_SENSORS; ++sensorIndex) - { - if (m_sensorValuesU16[sensorIndex] < m_sensorMinValues[sensorIndex]) - { - m_sensorMinValues[sensorIndex] = m_sensorValuesU16[sensorIndex]; - } - - if (m_sensorValuesU16[sensorIndex] > m_sensorMaxValues[sensorIndex]) - { - m_sensorMaxValues[sensorIndex] = m_sensorValuesU16[sensorIndex]; - } - } - - m_sensorCalibStarted = true; -} - -int16_t LineSensors::readLine() -{ - uint32_t estimatedPos = 0; - uint32_t numerator = 0; - uint32_t denominator = 0; - const uint32_t WEIGHT = 1000; - bool isOnLine = false; - - (void)getSensorValues(); - - for (uint32_t idx = 0; idx < MAX_SENSORS; ++idx) - { - uint32_t sensorValue = m_sensorValuesU16[idx]; - - numerator += idx * WEIGHT * sensorValue; - denominator += sensorValue; - - /* Keep track of whether we see the line at all. */ - if (SENSOR_OFF_LINE_THRESHOLD < sensorValue) - { - isOnLine = true; - } - } - - if (false == isOnLine) - { - /* If it last read to the left of center, return 0. */ - if (m_lastPosValue < (((MAX_SENSORS - 1) * SENSOR_MAX_VALUE) / 2)) - { - estimatedPos = 0; - } - /* If it last read to the right of center, return the max. value. */ - else - { - estimatedPos = (MAX_SENSORS - 1) * SENSOR_MAX_VALUE; - } - } - else - { - /* Check to avoid division by zero. */ - if (0 != denominator) - { - estimatedPos = numerator / denominator; - } - - m_lastPosValue = estimatedPos; - } - - return static_cast(estimatedPos); -} - -const uint16_t* LineSensors::getSensorValues() -{ - for (uint8_t sensorIndex = 0; sensorIndex < MAX_SENSORS; ++sensorIndex) - { - if (nullptr != m_lightSensors[sensorIndex]) - { - m_sensorValuesU16[sensorIndex] = static_cast(m_lightSensors[sensorIndex]->getValue()); - } - } - - return m_sensorValuesU16; -} - -bool LineSensors::isCalibrationSuccessful() -{ - bool isSuccessful = false; - - m_calibErrorInfo = CALIB_ERROR_NOT_CALIBRATED; - - if (true == m_sensorCalibStarted) - { - uint8_t index = 0; - - isSuccessful = true; - while ((MAX_SENSORS > index) && (true == isSuccessful)) - { - uint16_t distance = 0; - - /* Check whether the max. value is really greater than the min. value. - * It can happen that someone try to calibrate over a blank surface. - */ - if (m_sensorMaxValues[index] > m_sensorMinValues[index]) - { - distance = m_sensorMaxValues[index] - m_sensorMinValues[index]; - } - - /* The assumption here is, that the distance (max. value - min. value) must be - * higher than a quarter of the max. measured duration. - */ - if ((SENSOR_MAX_VALUE / 4) > distance) - { - m_calibErrorInfo = index; - isSuccessful = false; - } - - ++index; - } - } - - return isSuccessful; -} - -void LineSensors::resetCalibration() -{ - for (uint8_t sensorIndex = 0; sensorIndex < MAX_SENSORS; ++sensorIndex) - { - m_sensorValuesU16[sensorIndex] = 0; - m_sensorMaxValues[sensorIndex] = 0; - m_sensorMinValues[sensorIndex] = SENSOR_MAX_VALUE; - } - - m_sensorCalibStarted = false; -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Line sensors array realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "LineSensors.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void LineSensors::init() +{ + for (uint8_t sensorIndex = 0; sensorIndex < MAX_SENSORS; ++sensorIndex) + { + m_sensorValuesU16[sensorIndex] = 0; + m_sensorMaxValues[sensorIndex] = 0; + m_sensorMinValues[sensorIndex] = SENSOR_MAX_VALUE; + } +} + +void LineSensors::calibrate() +{ + (void)getSensorValues(); + + for (uint8_t sensorIndex = 0; sensorIndex < MAX_SENSORS; ++sensorIndex) + { + if (m_sensorValuesU16[sensorIndex] < m_sensorMinValues[sensorIndex]) + { + m_sensorMinValues[sensorIndex] = m_sensorValuesU16[sensorIndex]; + } + + if (m_sensorValuesU16[sensorIndex] > m_sensorMaxValues[sensorIndex]) + { + m_sensorMaxValues[sensorIndex] = m_sensorValuesU16[sensorIndex]; + } + } + + m_sensorCalibStarted = true; +} + +int16_t LineSensors::readLine() +{ + uint32_t estimatedPos = 0; + uint32_t numerator = 0; + uint32_t denominator = 0; + const uint32_t WEIGHT = 1000; + bool isOnLine = false; + + (void)getSensorValues(); + + for (uint32_t idx = 0; idx < MAX_SENSORS; ++idx) + { + uint32_t sensorValue = m_sensorValuesU16[idx]; + + numerator += idx * WEIGHT * sensorValue; + denominator += sensorValue; + + /* Keep track of whether we see the line at all. */ + if (SENSOR_OFF_LINE_THRESHOLD < sensorValue) + { + isOnLine = true; + } + } + + if (false == isOnLine) + { + /* If it last read to the left of center, return 0. */ + if (m_lastPosValue < (((MAX_SENSORS - 1) * SENSOR_MAX_VALUE) / 2)) + { + estimatedPos = 0; + } + /* If it last read to the right of center, return the max. value. */ + else + { + estimatedPos = (MAX_SENSORS - 1) * SENSOR_MAX_VALUE; + } + } + else + { + /* Check to avoid division by zero. */ + if (0 != denominator) + { + estimatedPos = numerator / denominator; + } + + m_lastPosValue = estimatedPos; + } + + return static_cast(estimatedPos); +} + +const uint16_t* LineSensors::getSensorValues() +{ + for (uint8_t sensorIndex = 0; sensorIndex < MAX_SENSORS; ++sensorIndex) + { + if (nullptr != m_lightSensors[sensorIndex]) + { + m_sensorValuesU16[sensorIndex] = static_cast(m_lightSensors[sensorIndex]->getValue()); + } + } + + return m_sensorValuesU16; +} + +bool LineSensors::isCalibrationSuccessful() +{ + bool isSuccessful = false; + + m_calibErrorInfo = CALIB_ERROR_NOT_CALIBRATED; + + if (true == m_sensorCalibStarted) + { + uint8_t index = 0; + + isSuccessful = true; + while ((MAX_SENSORS > index) && (true == isSuccessful)) + { + uint16_t distance = 0; + + /* Check whether the max. value is really greater than the min. value. + * It can happen that someone try to calibrate over a blank surface. + */ + if (m_sensorMaxValues[index] > m_sensorMinValues[index]) + { + distance = m_sensorMaxValues[index] - m_sensorMinValues[index]; + } + + /* The assumption here is, that the distance (max. value - min. value) must be + * higher than a quarter of the max. measured duration. + */ + if ((SENSOR_MAX_VALUE / 4) > distance) + { + m_calibErrorInfo = index; + isSuccessful = false; + } + + ++index; + } + } + + return isSuccessful; +} + +void LineSensors::resetCalibration() +{ + for (uint8_t sensorIndex = 0; sensorIndex < MAX_SENSORS; ++sensorIndex) + { + m_sensorValuesU16[sensorIndex] = 0; + m_sensorMaxValues[sensorIndex] = 0; + m_sensorMinValues[sensorIndex] = SENSOR_MAX_VALUE; + } + + m_sensorCalibStarted = false; +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALSim/LineSensors.h b/lib/HALSim/LineSensors.h index d222e142..ef29264f 100644 --- a/lib/HALSim/LineSensors.h +++ b/lib/HALSim/LineSensors.h @@ -1,225 +1,225 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Line sensors array realization - * @author Andreas Merkle - * - * @addtogroup HALSim - * - * @{ - */ - -#ifndef LINESENSORS_H -#define LINESENSORS_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ILineSensors.h" -#include "SimTime.h" - -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class provides access to the Zumo target line sensors. */ -class LineSensors : public ILineSensors -{ -public: - /** - * Constructs the line sensors adapter. - * - * @param[in] emitter0 The most left infrared emitter 0 - * @param[in] emitter1 The infrared emitter 1 - * @param[in] emitter2 The infrared emitter 2 - * @param[in] emitter3 The infrared emitter 3 - * @param[in] emitter4 The most right infrared emitter 4 - * @param[in] lightSensor0 The most left light sensor 0 - * @param[in] lightSensor1 The light sensor 1 - * @param[in] lightSensor2 The light sensor 2 - * @param[in] lightSensor3 The light sensor 3 - * @param[in] lightSensor4 The most right light sensor 4 - */ - LineSensors(webots::Emitter* emitter0, webots::Emitter* emitter1, webots::Emitter* emitter2, - webots::Emitter* emitter3, webots::Emitter* emitter4, webots::DistanceSensor* lightSensor0, - webots::DistanceSensor* lightSensor1, webots::DistanceSensor* lightSensor2, - webots::DistanceSensor* lightSensor3, webots::DistanceSensor* lightSensor4) : - ILineSensors(), - m_sensorValuesU16(), - m_emitters{emitter0, emitter1, emitter2, emitter3, emitter4}, - m_lightSensors{lightSensor0, lightSensor1, lightSensor2, lightSensor3, lightSensor4}, - m_sensorCalibSuccessfull(false), - m_sensorCalibStarted(false), - m_calibErrorInfo(CALIB_ERROR_NOT_CALIBRATED), - m_sensorMinValues(), - m_sensorMaxValues(), - m_lastPosValue(SENSOR_MAX_VALUE * (MAX_SENSORS / 2)) - { - } - - /** - * Destroys the line sensors adapter. - */ - ~LineSensors() - { - } - - /** - * Initializes the line sensors. - */ - void init() final; - - /** - * Reads the sensors for calibration. Call this method several times during - * turning the sensors over the line to determine the minimum and maximum - * values. - * - * The calibration factors are stored internally. - */ - void calibrate() final; - - /** - * Determines the deviation and returns an estimated position of the robot - * with respect to a line. The estimate is made using a weighted average of - * the sensor indices multiplied by 1000, so that a return value of 0 - * indicates that the line is directly below sensor 0, a return value of - * 1000 indicates that the line is directly below sensor 1, 2000 - * indicates that it's below sensor 2000, etc. Intermediate values - * indicate that the line is between two sensors. The formula is: - * - * 0*value0 + 1000*value1 + 2000*value2 + ... - * -------------------------------------------- - * value0 + value1 + value2 + ... - * - * This function assumes a dark line (high values) surrounded by white - * (low values). - * - * @return Estimated position with respect to track. - */ - int16_t readLine() final; - - /** - * Get last line sensor values. - * - * @return Line sensor values - */ - const uint16_t* getSensorValues() final; - - /** - * Checks whether the calibration was successful or not. - * It assumes that the environment brightness compensation is active. - * - * @return If successful, it will return true otherwise false. - */ - bool isCalibrationSuccessful() final; - - /** - * It will return the index of the sensor, which caused to fail the calibration. - * If calibration was successful, it will return 0xFF. - * If calibration was not not done yet, it will return 0xFE. - * - * @return Sensor index, starting with 0. Note the other cases in description. - */ - uint8_t getCalibErrorInfo() const final - { - return m_calibErrorInfo; - } - - /** - * Get number of used line sensors. - * - * @return Number of used line sensors - */ - uint8_t getNumLineSensors() const final - { - return MAX_SENSORS; - } - - /** - * Get max. value of a single line sensor in digits. - * The sensor value is indirect proportional to the reflectance. - * - * @return Max. line sensor value - */ - int16_t getSensorValueMax() const final - { - return SENSOR_MAX_VALUE; - } - - /** - * Resets the maximum and minimum values measured by each sensor. - */ - void resetCalibration() final; - -private: - /** - * Number of used line sensors. This depends on the Zumo hardware configuration. - */ - static const uint8_t MAX_SENSORS = 5; - - /** - * Max. value of a single line sensor in digits. - * It depends on the Zumo32U4LineSensors implementation. - */ - static const int16_t SENSOR_MAX_VALUE = 1000; - - /** - * If above the threshold, it will be assumed that the sensor see's the line. - */ - static const uint16_t SENSOR_OFF_LINE_THRESHOLD = 200; - - uint16_t m_sensorValuesU16[MAX_SENSORS]; /**< The last value of each sensor as unsigned 16-bit values. */ - webots::Emitter* m_emitters[MAX_SENSORS]; /**< The infrared emitters (0: most left) */ - webots::DistanceSensor* m_lightSensors[MAX_SENSORS]; /**< The light sensors (0: most left) */ - bool m_sensorCalibSuccessfull; /**< Indicates weather the calibration was successfull or not. */ - bool m_sensorCalibStarted; /**< Indicates weather the calibration has started or not. */ - uint8_t m_calibErrorInfo; /**< Indicates which sensor failed the calibration, if the calibration failed. */ - uint16_t m_sensorMinValues[MAX_SENSORS]; /**< Stores the minimal calibration values for the sensors. */ - uint16_t m_sensorMaxValues[MAX_SENSORS]; /**< Stores the minimal calibration values for the sensors. */ - int16_t m_lastPosValue; /**< Last valid line sensor position value. */ - - /* Default constructor not allowed. */ - LineSensors(); -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* LINESENSORS_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Line sensors array realization + * @author Andreas Merkle + * + * @addtogroup HALSim + * + * @{ + */ + +#ifndef LINESENSORS_H +#define LINESENSORS_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ILineSensors.h" +#include "SimTime.h" + +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** This class provides access to the Zumo target line sensors. */ +class LineSensors : public ILineSensors +{ +public: + /** + * Constructs the line sensors adapter. + * + * @param[in] emitter0 The most left infrared emitter 0 + * @param[in] emitter1 The infrared emitter 1 + * @param[in] emitter2 The infrared emitter 2 + * @param[in] emitter3 The infrared emitter 3 + * @param[in] emitter4 The most right infrared emitter 4 + * @param[in] lightSensor0 The most left light sensor 0 + * @param[in] lightSensor1 The light sensor 1 + * @param[in] lightSensor2 The light sensor 2 + * @param[in] lightSensor3 The light sensor 3 + * @param[in] lightSensor4 The most right light sensor 4 + */ + LineSensors(webots::Emitter* emitter0, webots::Emitter* emitter1, webots::Emitter* emitter2, + webots::Emitter* emitter3, webots::Emitter* emitter4, webots::DistanceSensor* lightSensor0, + webots::DistanceSensor* lightSensor1, webots::DistanceSensor* lightSensor2, + webots::DistanceSensor* lightSensor3, webots::DistanceSensor* lightSensor4) : + ILineSensors(), + m_sensorValuesU16(), + m_emitters{emitter0, emitter1, emitter2, emitter3, emitter4}, + m_lightSensors{lightSensor0, lightSensor1, lightSensor2, lightSensor3, lightSensor4}, + m_sensorCalibSuccessfull(false), + m_sensorCalibStarted(false), + m_calibErrorInfo(CALIB_ERROR_NOT_CALIBRATED), + m_sensorMinValues(), + m_sensorMaxValues(), + m_lastPosValue(SENSOR_MAX_VALUE * (MAX_SENSORS / 2)) + { + } + + /** + * Destroys the line sensors adapter. + */ + ~LineSensors() + { + } + + /** + * Initializes the line sensors. + */ + void init() final; + + /** + * Reads the sensors for calibration. Call this method several times during + * turning the sensors over the line to determine the minimum and maximum + * values. + * + * The calibration factors are stored internally. + */ + void calibrate() final; + + /** + * Determines the deviation and returns an estimated position of the robot + * with respect to a line. The estimate is made using a weighted average of + * the sensor indices multiplied by 1000, so that a return value of 0 + * indicates that the line is directly below sensor 0, a return value of + * 1000 indicates that the line is directly below sensor 1, 2000 + * indicates that it's below sensor 2000, etc. Intermediate values + * indicate that the line is between two sensors. The formula is: + * + * 0*value0 + 1000*value1 + 2000*value2 + ... + * -------------------------------------------- + * value0 + value1 + value2 + ... + * + * This function assumes a dark line (high values) surrounded by white + * (low values). + * + * @return Estimated position with respect to track. + */ + int16_t readLine() final; + + /** + * Get last line sensor values. + * + * @return Line sensor values + */ + const uint16_t* getSensorValues() final; + + /** + * Checks whether the calibration was successful or not. + * It assumes that the environment brightness compensation is active. + * + * @return If successful, it will return true otherwise false. + */ + bool isCalibrationSuccessful() final; + + /** + * It will return the index of the sensor, which caused to fail the calibration. + * If calibration was successful, it will return 0xFF. + * If calibration was not not done yet, it will return 0xFE. + * + * @return Sensor index, starting with 0. Note the other cases in description. + */ + uint8_t getCalibErrorInfo() const final + { + return m_calibErrorInfo; + } + + /** + * Get number of used line sensors. + * + * @return Number of used line sensors + */ + uint8_t getNumLineSensors() const final + { + return MAX_SENSORS; + } + + /** + * Get max. value of a single line sensor in digits. + * The sensor value is indirect proportional to the reflectance. + * + * @return Max. line sensor value + */ + int16_t getSensorValueMax() const final + { + return SENSOR_MAX_VALUE; + } + + /** + * Resets the maximum and minimum values measured by each sensor. + */ + void resetCalibration() final; + +private: + /** + * Number of used line sensors. This depends on the Zumo hardware configuration. + */ + static const uint8_t MAX_SENSORS = 5; + + /** + * Max. value of a single line sensor in digits. + * It depends on the Zumo32U4LineSensors implementation. + */ + static const int16_t SENSOR_MAX_VALUE = 1000; + + /** + * If above the threshold, it will be assumed that the sensor see's the line. + */ + static const uint16_t SENSOR_OFF_LINE_THRESHOLD = 200; + + uint16_t m_sensorValuesU16[MAX_SENSORS]; /**< The last value of each sensor as unsigned 16-bit values. */ + webots::Emitter* m_emitters[MAX_SENSORS]; /**< The infrared emitters (0: most left) */ + webots::DistanceSensor* m_lightSensors[MAX_SENSORS]; /**< The light sensors (0: most left) */ + bool m_sensorCalibSuccessfull; /**< Indicates weather the calibration was successfull or not. */ + bool m_sensorCalibStarted; /**< Indicates weather the calibration has started or not. */ + uint8_t m_calibErrorInfo; /**< Indicates which sensor failed the calibration, if the calibration failed. */ + uint16_t m_sensorMinValues[MAX_SENSORS]; /**< Stores the minimal calibration values for the sensors. */ + uint16_t m_sensorMaxValues[MAX_SENSORS]; /**< Stores the minimal calibration values for the sensors. */ + int16_t m_lastPosValue; /**< Last valid line sensor position value. */ + + /* Default constructor not allowed. */ + LineSensors(); +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* LINESENSORS_H */ +/** @} */ diff --git a/lib/HALSim/Motors.cpp b/lib/HALSim/Motors.cpp index 8eceb067..c82d3a42 100644 --- a/lib/HALSim/Motors.cpp +++ b/lib/HALSim/Motors.cpp @@ -1,141 +1,141 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Motrs realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "Motors.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void Motors::init() -{ - initMotor(m_leftMotor); - initMotor(m_rightMotor); -} - -void Motors::setSpeeds(int16_t leftSpeed, int16_t rightSpeed) -{ - const double _MAX_SPEED = static_cast(MAX_SPEED); - - if (nullptr != m_leftMotor) - { - const double MAX_SIM_VELOCITY = m_leftMotor->getMaxVelocity(); - - if (leftSpeed > MAX_SPEED) - { - m_leftMotor->setVelocity(MAX_SIM_VELOCITY); - } - else if (leftSpeed < (-MAX_SPEED)) - { - m_leftMotor->setVelocity(-MAX_SIM_VELOCITY); - } - else - { - double _leftSpeed = static_cast(leftSpeed); - - m_leftMotor->setVelocity((MAX_SIM_VELOCITY * _leftSpeed) / _MAX_SPEED); - } - } - - if (nullptr != m_rightMotor) - { - const double MAX_SIM_VELOCITY = m_rightMotor->getMaxVelocity(); - - if (rightSpeed > MAX_SPEED) - { - m_rightMotor->setVelocity(MAX_SIM_VELOCITY); - } - else if (rightSpeed < (-MAX_SPEED)) - { - m_rightMotor->setVelocity(-MAX_SIM_VELOCITY); - } - else - { - double _rightSpeed = static_cast(rightSpeed); - - m_rightMotor->setVelocity((MAX_SIM_VELOCITY * _rightSpeed) / _MAX_SPEED); - } - } - - m_leftSpeed = leftSpeed; - m_rightSpeed = rightSpeed; -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -void Motors::initMotor(webots::Motor* motor) -{ - if (nullptr != motor) - { - /* Endless linear motion */ - motor->setPosition(std::numeric_limits::infinity()); - - /* Stop motor, note velocity in m/s */ - motor->setVelocity(0.0F); - } -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Motrs realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "Motors.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void Motors::init() +{ + initMotor(m_leftMotor); + initMotor(m_rightMotor); +} + +void Motors::setSpeeds(int16_t leftSpeed, int16_t rightSpeed) +{ + const double _MAX_SPEED = static_cast(MAX_SPEED); + + if (nullptr != m_leftMotor) + { + const double MAX_SIM_VELOCITY = m_leftMotor->getMaxVelocity(); + + if (leftSpeed > MAX_SPEED) + { + m_leftMotor->setVelocity(MAX_SIM_VELOCITY); + } + else if (leftSpeed < (-MAX_SPEED)) + { + m_leftMotor->setVelocity(-MAX_SIM_VELOCITY); + } + else + { + double _leftSpeed = static_cast(leftSpeed); + + m_leftMotor->setVelocity((MAX_SIM_VELOCITY * _leftSpeed) / _MAX_SPEED); + } + } + + if (nullptr != m_rightMotor) + { + const double MAX_SIM_VELOCITY = m_rightMotor->getMaxVelocity(); + + if (rightSpeed > MAX_SPEED) + { + m_rightMotor->setVelocity(MAX_SIM_VELOCITY); + } + else if (rightSpeed < (-MAX_SPEED)) + { + m_rightMotor->setVelocity(-MAX_SIM_VELOCITY); + } + else + { + double _rightSpeed = static_cast(rightSpeed); + + m_rightMotor->setVelocity((MAX_SIM_VELOCITY * _rightSpeed) / _MAX_SPEED); + } + } + + m_leftSpeed = leftSpeed; + m_rightSpeed = rightSpeed; +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +void Motors::initMotor(webots::Motor* motor) +{ + if (nullptr != motor) + { + /* Endless linear motion */ + motor->setPosition(std::numeric_limits::infinity()); + + /* Stop motor, note velocity in m/s */ + motor->setVelocity(0.0F); + } +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALSim/Motors.h b/lib/HALSim/Motors.h index 11226183..b8d27872 100644 --- a/lib/HALSim/Motors.h +++ b/lib/HALSim/Motors.h @@ -1,160 +1,160 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Motors realization - * @author Andreas Merkle - * - * @addtogroup HALSim - * - * @{ - */ - -#ifndef MOTORS_H -#define MOTORS_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "IMotors.h" - -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class provides access to the Zumo target motors. */ -class Motors : public IMotors -{ -public: - /** - * Constructs the motors adapter. - * - * @param[in] leftMotor The left motor. - * @param[in] rightMotor The right motor. - */ - Motors(webots::Motor* leftMotor, webots::Motor* rightMotor) : - IMotors(), - m_leftMotor(leftMotor), - m_rightMotor(rightMotor), - m_leftSpeed(0), - m_rightSpeed(0) - { - } - - /** - * Destroys the motors adapter. - */ - ~Motors() - { - } - - /** - * Initializes the motors. - */ - void init() final; - - /** - * Sets the speeds for both motors. - * - * @param[in] leftSpeed A number from -400 to 400 representing the speed and - * direction of the right motor. Values of -400 or less result in full speed - * reverse, and values of 400 or more result in full speed forward. - * @param[in] rightSpeed A number from -400 to 400 representing the speed and - * direction of the right motor. Values of -400 or less result in full speed - * reverse, and values of 400 or more result in full speed forward. - */ - void setSpeeds(int16_t leftSpeed, int16_t rightSpeed) final; - - /** - * Get maximum speed of the motors in digits. - * - * @return Max. speed in digits - */ - int16_t getMaxSpeed() const final - { - return MAX_SPEED; - } - - /** - * Get the current speed of the left motor. - * - * @return The left motor speed in digits. - */ - int16_t getLeftSpeed() final - { - return m_leftSpeed; - } - - /** - * Get the current speed of the right motor. - * - * @return The right motor speed in digits. - */ - int16_t getRightSpeed() final - { - return m_rightSpeed; - } - -private: - /** - * The maximum speed of a single motor in PWM digits. - */ - static const int16_t MAX_SPEED = 400; - - webots::Motor* m_leftMotor; /**< Left motor of the simulated robot. */ - webots::Motor* m_rightMotor; /**< Right motor of the simulated robot. */ - int16_t m_leftSpeed; /**< Left motor speed in PWM digits, used by the application. */ - int16_t m_rightSpeed; /**< Right motor speed in PWM digits, used by the application. */ - - /* Default constructor not allowed. */ - Motors(); - - /** - * Initialize the simulated motor. - * Shall be done once. - * - * @param[in] motor Moto which to initialize - */ - void initMotor(webots::Motor* motor); -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* MOTORS_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Motors realization + * @author Andreas Merkle + * + * @addtogroup HALSim + * + * @{ + */ + +#ifndef MOTORS_H +#define MOTORS_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "IMotors.h" + +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** This class provides access to the Zumo target motors. */ +class Motors : public IMotors +{ +public: + /** + * Constructs the motors adapter. + * + * @param[in] leftMotor The left motor. + * @param[in] rightMotor The right motor. + */ + Motors(webots::Motor* leftMotor, webots::Motor* rightMotor) : + IMotors(), + m_leftMotor(leftMotor), + m_rightMotor(rightMotor), + m_leftSpeed(0), + m_rightSpeed(0) + { + } + + /** + * Destroys the motors adapter. + */ + ~Motors() + { + } + + /** + * Initializes the motors. + */ + void init() final; + + /** + * Sets the speeds for both motors. + * + * @param[in] leftSpeed A number from -400 to 400 representing the speed and + * direction of the right motor. Values of -400 or less result in full speed + * reverse, and values of 400 or more result in full speed forward. + * @param[in] rightSpeed A number from -400 to 400 representing the speed and + * direction of the right motor. Values of -400 or less result in full speed + * reverse, and values of 400 or more result in full speed forward. + */ + void setSpeeds(int16_t leftSpeed, int16_t rightSpeed) final; + + /** + * Get maximum speed of the motors in digits. + * + * @return Max. speed in digits + */ + int16_t getMaxSpeed() const final + { + return MAX_SPEED; + } + + /** + * Get the current speed of the left motor. + * + * @return The left motor speed in digits. + */ + int16_t getLeftSpeed() final + { + return m_leftSpeed; + } + + /** + * Get the current speed of the right motor. + * + * @return The right motor speed in digits. + */ + int16_t getRightSpeed() final + { + return m_rightSpeed; + } + +private: + /** + * The maximum speed of a single motor in PWM digits. + */ + static const int16_t MAX_SPEED = 400; + + webots::Motor* m_leftMotor; /**< Left motor of the simulated robot. */ + webots::Motor* m_rightMotor; /**< Right motor of the simulated robot. */ + int16_t m_leftSpeed; /**< Left motor speed in PWM digits, used by the application. */ + int16_t m_rightSpeed; /**< Right motor speed in PWM digits, used by the application. */ + + /* Default constructor not allowed. */ + Motors(); + + /** + * Initialize the simulated motor. + * Shall be done once. + * + * @param[in] motor Moto which to initialize + */ + void initMotor(webots::Motor* motor); +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* MOTORS_H */ +/** @} */ diff --git a/lib/HALSim/ProximitySensors.cpp b/lib/HALSim/ProximitySensors.cpp index d55682af..574ec692 100644 --- a/lib/HALSim/ProximitySensors.cpp +++ b/lib/HALSim/ProximitySensors.cpp @@ -1,83 +1,83 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Proximity sensors realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ProximitySensors.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void ProximitySensors::initFrontSensor() -{ - for (uint8_t sensorIndex = 0; sensorIndex < MAX_SENSORS; ++sensorIndex) - { - m_sensorValuesU8[sensorIndex] = 0; - } -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Proximity sensors realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ProximitySensors.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void ProximitySensors::initFrontSensor() +{ + for (uint8_t sensorIndex = 0; sensorIndex < MAX_SENSORS; ++sensorIndex) + { + m_sensorValuesU8[sensorIndex] = 0; + } +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALSim/ProximitySensors.h b/lib/HALSim/ProximitySensors.h index 3c390851..d01be7a0 100644 --- a/lib/HALSim/ProximitySensors.h +++ b/lib/HALSim/ProximitySensors.h @@ -1,188 +1,188 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Proximity sensors realization - * @author Andreas Merkle - * - * @addtogroup HALInterfaces - * - * @{ - */ -#ifndef PROXIMITYSENSORS_H -#define PROXIMITYSENSORS_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "IProximitySensors.h" -#include "SimTime.h" - -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The proximity sensors realization for the simulation. */ -class ProximitySensors : public IProximitySensors -{ -public: - /** - * Constructs the interface. - * - * @param[in] proxSensor0 Front left proximity sensor - * @param[in] proxSensor1 Front right proximity sensor - */ - ProximitySensors(webots::DistanceSensor* proxSensor0, webots::DistanceSensor* proxSensor1) : - IProximitySensors(), - m_proximitySensors{proxSensor0, proxSensor1}, - m_sensorValuesU8{0U} - { - } - - /** - * Destroys the interface. - */ - virtual ~ProximitySensors() - { - } - - /** - * Initialize only the front proximity sensor. - */ - void initFrontSensor() final; - - /** - * Returns the number of sensors. - * - * @return Number of sensors - */ - uint8_t getNumSensors() const final - { - /* Front sensors are counted as one */ - return 1U; - } - - /** - * Emits IR pulses and gets readings from the sensors. - */ - void read() final - { - for (uint8_t sensorIndex = 0U; sensorIndex < MAX_SENSORS; ++sensorIndex) - { - if (nullptr != m_proximitySensors[sensorIndex]) - { - m_sensorValuesU8[sensorIndex] = static_cast(m_proximitySensors[sensorIndex]->getValue()); - } - } - } - - /** - * Returns the number of brightness levels for the left LEDs that - * activated the front proximity sensor. - * - * @return Number of brightness levels in digits. - */ - uint8_t countsFrontWithLeftLeds() const final - { - return m_sensorValuesU8[SENSOR_INDEX::LEFT]; - } - - /** - * Returns the number of brightness levels for the right LEDs that - * activated the front proximity sensor. - * - * @return Number of brightness levels in digits. - */ - uint8_t countsFrontWithRightLeds() const final - { - return m_sensorValuesU8[SENSOR_INDEX::RIGHT]; - } - - /** - * Returns the number of brightness levels. - * - * @return Number of brightness levels. - */ - uint8_t getNumBrightnessLevels() const - { - return SENSOR_MAX_VALUE; - } - -protected: -private: - /** - * Max. value of a single proximity sensor in digits. - * It depends on the Zumo32U4ProximitySensors implementation, - * specifically the number of brightness levels. - */ - static const uint8_t SENSOR_MAX_VALUE = 6; - - /** - * Number of sensors that make up the front of the robot in the simulation. - * Using more than one sensor allows for diferenciation between left and right side. - */ - static const uint8_t MAX_SENSORS = 2; - - /** - * Enumeration of Sensors. - */ - enum SENSOR_INDEX : uint8_t - { - LEFT = 0U, - RIGHT - }; - - /** - * The frontal proximity sensors - */ - webots::DistanceSensor* m_proximitySensors[MAX_SENSORS]; - - /** - * The last value read of the sensors as unsigned 8-bit values. - * Value is in counts/digits representing number of brightness levels received by the sensor. - */ - uint8_t m_sensorValuesU8[MAX_SENSORS]; - - /* Default constructor not allowed. */ - ProximitySensors(); -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* PROXIMITYSENSORS_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Proximity sensors realization + * @author Andreas Merkle + * + * @addtogroup HALInterfaces + * + * @{ + */ +#ifndef PROXIMITYSENSORS_H +#define PROXIMITYSENSORS_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "IProximitySensors.h" +#include "SimTime.h" + +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The proximity sensors realization for the simulation. */ +class ProximitySensors : public IProximitySensors +{ +public: + /** + * Constructs the interface. + * + * @param[in] proxSensor0 Front left proximity sensor + * @param[in] proxSensor1 Front right proximity sensor + */ + ProximitySensors(webots::DistanceSensor* proxSensor0, webots::DistanceSensor* proxSensor1) : + IProximitySensors(), + m_proximitySensors{proxSensor0, proxSensor1}, + m_sensorValuesU8{0U} + { + } + + /** + * Destroys the interface. + */ + virtual ~ProximitySensors() + { + } + + /** + * Initialize only the front proximity sensor. + */ + void initFrontSensor() final; + + /** + * Returns the number of sensors. + * + * @return Number of sensors + */ + uint8_t getNumSensors() const final + { + /* Front sensors are counted as one */ + return 1U; + } + + /** + * Emits IR pulses and gets readings from the sensors. + */ + void read() final + { + for (uint8_t sensorIndex = 0U; sensorIndex < MAX_SENSORS; ++sensorIndex) + { + if (nullptr != m_proximitySensors[sensorIndex]) + { + m_sensorValuesU8[sensorIndex] = static_cast(m_proximitySensors[sensorIndex]->getValue()); + } + } + } + + /** + * Returns the number of brightness levels for the left LEDs that + * activated the front proximity sensor. + * + * @return Number of brightness levels in digits. + */ + uint8_t countsFrontWithLeftLeds() const final + { + return m_sensorValuesU8[SENSOR_INDEX::LEFT]; + } + + /** + * Returns the number of brightness levels for the right LEDs that + * activated the front proximity sensor. + * + * @return Number of brightness levels in digits. + */ + uint8_t countsFrontWithRightLeds() const final + { + return m_sensorValuesU8[SENSOR_INDEX::RIGHT]; + } + + /** + * Returns the number of brightness levels. + * + * @return Number of brightness levels. + */ + uint8_t getNumBrightnessLevels() const + { + return SENSOR_MAX_VALUE; + } + +protected: +private: + /** + * Max. value of a single proximity sensor in digits. + * It depends on the Zumo32U4ProximitySensors implementation, + * specifically the number of brightness levels. + */ + static const uint8_t SENSOR_MAX_VALUE = 6; + + /** + * Number of sensors that make up the front of the robot in the simulation. + * Using more than one sensor allows for diferenciation between left and right side. + */ + static const uint8_t MAX_SENSORS = 2; + + /** + * Enumeration of Sensors. + */ + enum SENSOR_INDEX : uint8_t + { + LEFT = 0U, + RIGHT + }; + + /** + * The frontal proximity sensors + */ + webots::DistanceSensor* m_proximitySensors[MAX_SENSORS]; + + /** + * The last value read of the sensors as unsigned 8-bit values. + * Value is in counts/digits representing number of brightness levels received by the sensor. + */ + uint8_t m_sensorValuesU8[MAX_SENSORS]; + + /* Default constructor not allowed. */ + ProximitySensors(); +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* PROXIMITYSENSORS_H */ +/** @} */ diff --git a/lib/HALSim/RobotConstants.h b/lib/HALSim/RobotConstants.h index a709b2de..90ffb4c9 100644 --- a/lib/HALSim/RobotConstants.h +++ b/lib/HALSim/RobotConstants.h @@ -1,102 +1,102 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Robot specific constants - * @author Andreas Merkle - * - * @addtogroup HALInterfaces - * - * @{ - */ - -#ifndef ROBOTCONSTANTS_H -#define ROBOTCONSTANTS_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** - * Abstracts the physical robot constants. - */ -namespace RobotConstants -{ - /** - * Gear ratio multiplied with 1000. - */ - static const uint32_t GEAR_RATIO = 75810; - - /** - * Encoder resolution in counts per revolution of the motor shaft. - */ - static const uint32_t ENCODER_RESOLUTION = 12; - - /** - * Calibrated wheel diameter in mm. - * This means the real wheel diameter was adapted after calibration drive. - */ - static const uint32_t WHEEL_DIAMETER = 36; - - /** - * Wheel circumference in um. - */ - static const uint32_t WHEEL_CIRCUMFERENCE = - static_cast(static_cast(WHEEL_DIAMETER) * PI * 1000.0f); - - /** - * Wheel base in mm. - * Distance between the left wheel center to the right wheel center. - */ - static const uint32_t WHEEL_BASE = 85; - - /** - * Number of encoder steps per m. - */ - static const uint32_t ENCODER_STEPS_PER_M = (ENCODER_RESOLUTION * GEAR_RATIO * 1000U) / WHEEL_CIRCUMFERENCE; - -}; /* namespace RobotConstants */ - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* ROBOTCONSTANTS_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Robot specific constants + * @author Andreas Merkle + * + * @addtogroup HALInterfaces + * + * @{ + */ + +#ifndef ROBOTCONSTANTS_H +#define ROBOTCONSTANTS_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** + * Abstracts the physical robot constants. + */ +namespace RobotConstants +{ + /** + * Gear ratio multiplied with 1000. + */ + static const uint32_t GEAR_RATIO = 75810; + + /** + * Encoder resolution in counts per revolution of the motor shaft. + */ + static const uint32_t ENCODER_RESOLUTION = 12; + + /** + * Calibrated wheel diameter in mm. + * This means the real wheel diameter was adapted after calibration drive. + */ + static const uint32_t WHEEL_DIAMETER = 36; + + /** + * Wheel circumference in um. + */ + static const uint32_t WHEEL_CIRCUMFERENCE = + static_cast(static_cast(WHEEL_DIAMETER) * PI * 1000.0f); + + /** + * Wheel base in mm. + * Distance between the left wheel center to the right wheel center. + */ + static const uint32_t WHEEL_BASE = 85; + + /** + * Number of encoder steps per m. + */ + static const uint32_t ENCODER_STEPS_PER_M = (ENCODER_RESOLUTION * GEAR_RATIO * 1000U) / WHEEL_CIRCUMFERENCE; + +}; /* namespace RobotConstants */ + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* ROBOTCONSTANTS_H */ +/** @} */ diff --git a/lib/HALSim/RobotDeviceNames.h b/lib/HALSim/RobotDeviceNames.h index a6619367..0cd843c3 100644 --- a/lib/HALSim/RobotDeviceNames.h +++ b/lib/HALSim/RobotDeviceNames.h @@ -1,146 +1,146 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Robot device names in Webots - * @author Andreas Merkle - * - * @addtogroup HALInterfaces - * - * @{ - */ - -#ifndef ROBOT_DEVICE_NAMES_H -#define ROBOT_DEVICE_NAMES_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** - * Robot device names in Webots. - */ -namespace RobotDeviceNames -{ - /** Name of the speaker in the robot simulation. */ - static const char* SPEAKER_NAME = "speaker"; - - /** Name of the display in the robot simulation. */ - static const char* DISPLAY_NAME = "robot_display"; - - /** Name of the left motor in the robot simulation. */ - static const char* LEFT_MOTOR_NAME = "motor_left"; - - /** Name of the right motor in the robot simulation. */ - static const char* RIGHT_MOTOR_NAME = "motor_right"; - - /** Name of the infrared emitter 0 in the robot simulation. */ - static const char* EMITTER_0_NAME = "emitter_l"; - - /** Name of the infrared emitter 1 in the robot simulation. */ - static const char* EMITTER_1_NAME = "emitter_lm"; - - /** Name of the infrared emitter 2 in the robot simulation. */ - static const char* EMITTER_2_NAME = "emitter_m"; - - /** Name of the infrared emitter 3 in the robot simulation. */ - static const char* EMITTER_3_NAME = "emitter_rm"; - - /** Name of the infrared emitter 4 in the robot simulation. */ - static const char* EMITTER_4_NAME = "emitter_r"; - - /** Name of the position sensor of the left motor in the robot simulation. */ - static const char* POS_SENSOR_LEFT_NAME = "position_sensor_left"; - - /** Name of the position sensor of the right motor in the robot simulation. */ - static const char* POS_SENSOR_RIGHT_NAME = "position_sensor_right"; - - /** Name of the light sensor 0 in the robot simulation. */ - static const char* LIGHT_SENSOR_0_NAME = "lightsensor_l"; - - /** Name of the light sensor 1 in the robot simulation. */ - static const char* LIGHT_SENSOR_1_NAME = "lightsensor_lm"; - - /** Name of the light sensor 2 in the robot simulation. */ - static const char* LIGHT_SENSOR_2_NAME = "lightsensor_m"; - - /** Name of the light sensor 3 in the robot simulation. */ - static const char* LIGHT_SENSOR_3_NAME = "lightsensor_rm"; - - /** Name of the light sensor 4 in the robot simulation. */ - static const char* LIGHT_SENSOR_4_NAME = "lightsensor_r"; - - /** Name of the red LED in the robot simulation. */ - static const char* LED_RED_NAME = "led_red"; - - /** Name of the yellow LED in the robot simulation. */ - static const char* LED_YELLOW_NAME = "led_yellow"; - - /** Name of the green LED in the robot simulation. */ - static const char* LED_GREEN_NAME = "led_green"; - - /** Name of the front left proximity sensor in the robot simulation. */ - static const char* PROXIMITY_SENSOR_FRONT_LEFT_NAME = "proxim_sensor_fl"; - - /** Name of the front right proximity sensor in the robot simulation. */ - static const char* PROXIMITY_SENSOR_FRONT_RIGHT_NAME = "proxim_sensor_fr"; - - /** Name of the accelerometer in the robot simulation. */ - static const char* ACCELEROMETER_NAME = "accelerometer"; - - /** Name of the gyro in the robot simulation. */ - static const char* GYRO_NAME = "gyro"; - - /** Name of the magnetometer in the robot simulation. */ - static const char* MAGNETOMETER_NAME = "magnetometer"; - - /** Name of the serial emitter in the RadonUlzer simulation. */ - static const char* EMITTER_NAME_SERIAL = "serialComTx"; - - /** Name of the serial receiver in the RadonUlzer simulation. */ - static const char* RECEIVER_NAME_SERIAL = "serialComRx"; - -}; /* namespace RobotDeviceNames */ - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* ROBOT_DEVICE_NAMES_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Robot device names in Webots + * @author Andreas Merkle + * + * @addtogroup HALInterfaces + * + * @{ + */ + +#ifndef ROBOT_DEVICE_NAMES_H +#define ROBOT_DEVICE_NAMES_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** + * Robot device names in Webots. + */ +namespace RobotDeviceNames +{ + /** Name of the speaker in the robot simulation. */ + static const char* SPEAKER_NAME = "speaker"; + + /** Name of the display in the robot simulation. */ + static const char* DISPLAY_NAME = "robot_display"; + + /** Name of the left motor in the robot simulation. */ + static const char* LEFT_MOTOR_NAME = "motor_left"; + + /** Name of the right motor in the robot simulation. */ + static const char* RIGHT_MOTOR_NAME = "motor_right"; + + /** Name of the infrared emitter 0 in the robot simulation. */ + static const char* EMITTER_0_NAME = "emitter_l"; + + /** Name of the infrared emitter 1 in the robot simulation. */ + static const char* EMITTER_1_NAME = "emitter_lm"; + + /** Name of the infrared emitter 2 in the robot simulation. */ + static const char* EMITTER_2_NAME = "emitter_m"; + + /** Name of the infrared emitter 3 in the robot simulation. */ + static const char* EMITTER_3_NAME = "emitter_rm"; + + /** Name of the infrared emitter 4 in the robot simulation. */ + static const char* EMITTER_4_NAME = "emitter_r"; + + /** Name of the position sensor of the left motor in the robot simulation. */ + static const char* POS_SENSOR_LEFT_NAME = "position_sensor_left"; + + /** Name of the position sensor of the right motor in the robot simulation. */ + static const char* POS_SENSOR_RIGHT_NAME = "position_sensor_right"; + + /** Name of the light sensor 0 in the robot simulation. */ + static const char* LIGHT_SENSOR_0_NAME = "lightsensor_l"; + + /** Name of the light sensor 1 in the robot simulation. */ + static const char* LIGHT_SENSOR_1_NAME = "lightsensor_lm"; + + /** Name of the light sensor 2 in the robot simulation. */ + static const char* LIGHT_SENSOR_2_NAME = "lightsensor_m"; + + /** Name of the light sensor 3 in the robot simulation. */ + static const char* LIGHT_SENSOR_3_NAME = "lightsensor_rm"; + + /** Name of the light sensor 4 in the robot simulation. */ + static const char* LIGHT_SENSOR_4_NAME = "lightsensor_r"; + + /** Name of the red LED in the robot simulation. */ + static const char* LED_RED_NAME = "led_red"; + + /** Name of the yellow LED in the robot simulation. */ + static const char* LED_YELLOW_NAME = "led_yellow"; + + /** Name of the green LED in the robot simulation. */ + static const char* LED_GREEN_NAME = "led_green"; + + /** Name of the front left proximity sensor in the robot simulation. */ + static const char* PROXIMITY_SENSOR_FRONT_LEFT_NAME = "proxim_sensor_fl"; + + /** Name of the front right proximity sensor in the robot simulation. */ + static const char* PROXIMITY_SENSOR_FRONT_RIGHT_NAME = "proxim_sensor_fr"; + + /** Name of the accelerometer in the robot simulation. */ + static const char* ACCELEROMETER_NAME = "accelerometer"; + + /** Name of the gyro in the robot simulation. */ + static const char* GYRO_NAME = "gyro"; + + /** Name of the magnetometer in the robot simulation. */ + static const char* MAGNETOMETER_NAME = "magnetometer"; + + /** Name of the serial emitter in the RadonUlzer simulation. */ + static const char* EMITTER_NAME_SERIAL = "serialComTx"; + + /** Name of the serial receiver in the RadonUlzer simulation. */ + static const char* RECEIVER_NAME_SERIAL = "serialComRx"; + +}; /* namespace RobotDeviceNames */ + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* ROBOT_DEVICE_NAMES_H */ +/** @} */ diff --git a/lib/HALSim/Sender.cpp b/lib/HALSim/Sender.cpp index 641d3df7..4c14efc9 100644 --- a/lib/HALSim/Sender.cpp +++ b/lib/HALSim/Sender.cpp @@ -1,100 +1,100 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Sender realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "Sender.h" -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void Sender::setChannel(int32_t channel) -{ - if (nullptr != m_emitter) - { - m_emitter->setChannel(channel); - } -} - -void Sender::send(const void* data, size_t size) const -{ - if (nullptr != m_emitter) - { - m_emitter->send(data, size); - } -} - -void Sender::send(const char* str) const -{ - if (nullptr != m_emitter) - { - m_emitter->send(str, strlen(str)); - } -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Sender realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "Sender.h" +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void Sender::setChannel(int32_t channel) +{ + if (nullptr != m_emitter) + { + m_emitter->setChannel(channel); + } +} + +void Sender::send(const void* data, size_t size) const +{ + if (nullptr != m_emitter) + { + m_emitter->send(data, size); + } +} + +void Sender::send(const char* str) const +{ + if (nullptr != m_emitter) + { + m_emitter->send(str, strlen(str)); + } +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALSim/Sender.h b/lib/HALSim/Sender.h index 3e0fa5a1..8fb3ca70 100644 --- a/lib/HALSim/Sender.h +++ b/lib/HALSim/Sender.h @@ -1,115 +1,115 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Sender realization - * @author Andreas Merkle - * - * @addtogroup HALSim - * - * @{ - */ - -#ifndef SENDER_H -#define SENDER_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ISender.h" - -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** - * This class provides a sender to communicate with other robots or the supervisor - * in the simulation. - */ -class Sender : public ISender -{ -public: - /** - * Constructs the sender adapter. - * - * @param[in] emitter The emitter of the simulated robot. - */ - Sender(webots::Emitter* emitter) : ISender(), m_emitter(emitter) - { - } - - /** - * Destroys the sender adapter. - */ - ~Sender() - { - } - - /** - * Set channel which to send data to. - * - * @param[in] channel The channel which to use. - */ - void setChannel(int32_t channel) final; - - /** - * Sends data to the configured channel. - * - * @param[in] data Data buffer - * @param[in] size Data buffer size in bytes. - */ - void send(const void* data, size_t size) const final; - - /** - * Sends string to the configured channel. - * - * @param[in] str String which to send. - */ - void send(const char* str) const final; - -private: - webots::Emitter* m_emitter; /**< The emitter on the simulated robot. */ - - /* Default constructor not allowed. */ - Sender(); -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* SENDER_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Sender realization + * @author Andreas Merkle + * + * @addtogroup HALSim + * + * @{ + */ + +#ifndef SENDER_H +#define SENDER_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ISender.h" + +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** + * This class provides a sender to communicate with other robots or the supervisor + * in the simulation. + */ +class Sender : public ISender +{ +public: + /** + * Constructs the sender adapter. + * + * @param[in] emitter The emitter of the simulated robot. + */ + Sender(webots::Emitter* emitter) : ISender(), m_emitter(emitter) + { + } + + /** + * Destroys the sender adapter. + */ + ~Sender() + { + } + + /** + * Set channel which to send data to. + * + * @param[in] channel The channel which to use. + */ + void setChannel(int32_t channel) final; + + /** + * Sends data to the configured channel. + * + * @param[in] data Data buffer + * @param[in] size Data buffer size in bytes. + */ + void send(const void* data, size_t size) const final; + + /** + * Sends string to the configured channel. + * + * @param[in] str String which to send. + */ + void send(const char* str) const final; + +private: + webots::Emitter* m_emitter; /**< The emitter on the simulated robot. */ + + /* Default constructor not allowed. */ + Sender(); +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* SENDER_H */ +/** @} */ diff --git a/lib/HALSim/Settings.cpp b/lib/HALSim/Settings.cpp index 9a48aef5..024ba8f2 100644 --- a/lib/HALSim/Settings.cpp +++ b/lib/HALSim/Settings.cpp @@ -1,206 +1,206 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Settings realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "Settings.h" -#include -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -static long getFileSize(FILE* fd); - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -const char* Settings::SETTINGS_FILE_NAME = "settings.json"; -const uint8_t Settings::DATA_VERSION = 1U; - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void Settings::init() -{ - bool isSuccessful = loadSettings(); - - if (false == isSuccessful) - { - /* Write settings with default values. */ - saveSettings(); - } -} - -int16_t Settings::getMaxSpeed() const -{ - return m_maxSpeed; -} - -void Settings::setMaxSpeed(int16_t maxSpeed) -{ - m_maxSpeed = maxSpeed; - saveSettings(); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -bool Settings::loadSettings() -{ - bool isSuccessful = false; - FILE* fd = fopen(SETTINGS_FILE_NAME, "rb"); - - if (nullptr != fd) - { - long fileSize = getFileSize(fd); - char buffer[fileSize]; - - if (fileSize == fread(buffer, sizeof(char), fileSize, fd)) - { - const size_t JSON_DOC_SIZE = 4096U; - DynamicJsonDocument jsonDoc(JSON_DOC_SIZE); - DeserializationError status = deserializeJson(jsonDoc, buffer); - - if (DeserializationError::Ok == status) - { - JsonVariantConst jsonVersion = jsonDoc["version"]; - - if (false == jsonVersion.isNull()) - { - uint8_t version = jsonVersion.as(); - - if (DATA_VERSION == version) - { - JsonVariantConst jsonMaxSpeed = jsonDoc["maxSpeed"]; - - if (false == jsonMaxSpeed.isNull()) - { - m_maxSpeed = jsonMaxSpeed.as(); - - isSuccessful = true; - } - } - } - } - } - - (void)fclose(fd); - } - - return isSuccessful; -} - -void Settings::saveSettings() -{ - const size_t JSON_DOC_SIZE = 4096U; - DynamicJsonDocument jsonDoc(JSON_DOC_SIZE); - size_t jsonBufferSize; - - /* Don't forget to bump the DATA_VERSION every time you modify - * the settings! - */ - - jsonDoc["version"] = DATA_VERSION; - jsonDoc["maxSpeed"] = m_maxSpeed; - - jsonBufferSize = measureJsonPretty(jsonDoc); /* Size without string termination. */ - - { - char jsonBuffer[jsonBufferSize + 1U]; - - if (jsonBufferSize == serializeJsonPretty(jsonDoc, jsonBuffer, jsonBufferSize)) - { - FILE* fd = fopen(SETTINGS_FILE_NAME, "wb"); - - if (nullptr != fd) - { - (void)fwrite(jsonBuffer, jsonBufferSize, 1, fd); - - (void)fclose(fd); - } - } - } -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ - -/** - * Get file size in byte. - * The file must be opened already. - * - * @param[in] fd File descriptor. - * - * @return File size in byte - */ -static long getFileSize(FILE* fd) -{ - long current = ftell(fd); /* Remember current position. */ - long begin; - long end; - - (void)fseek(fd, 0, SEEK_SET); - begin = ftell(fd); - (void)fseek(fd, 0, SEEK_END); - end = ftell(fd); - - /* Restore position. */ - (void)fseek(fd, current, SEEK_SET); - - return end - begin; -} +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Settings realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "Settings.h" +#include +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +static long getFileSize(FILE* fd); + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +const char* Settings::SETTINGS_FILE_NAME = "settings.json"; +const uint8_t Settings::DATA_VERSION = 1U; + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void Settings::init() +{ + bool isSuccessful = loadSettings(); + + if (false == isSuccessful) + { + /* Write settings with default values. */ + saveSettings(); + } +} + +int16_t Settings::getMaxSpeed() const +{ + return m_maxSpeed; +} + +void Settings::setMaxSpeed(int16_t maxSpeed) +{ + m_maxSpeed = maxSpeed; + saveSettings(); +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +bool Settings::loadSettings() +{ + bool isSuccessful = false; + FILE* fd = fopen(SETTINGS_FILE_NAME, "rb"); + + if (nullptr != fd) + { + long fileSize = getFileSize(fd); + char buffer[fileSize]; + + if (fileSize == fread(buffer, sizeof(char), fileSize, fd)) + { + const size_t JSON_DOC_SIZE = 4096U; + DynamicJsonDocument jsonDoc(JSON_DOC_SIZE); + DeserializationError status = deserializeJson(jsonDoc, buffer); + + if (DeserializationError::Ok == status) + { + JsonVariantConst jsonVersion = jsonDoc["version"]; + + if (false == jsonVersion.isNull()) + { + uint8_t version = jsonVersion.as(); + + if (DATA_VERSION == version) + { + JsonVariantConst jsonMaxSpeed = jsonDoc["maxSpeed"]; + + if (false == jsonMaxSpeed.isNull()) + { + m_maxSpeed = jsonMaxSpeed.as(); + + isSuccessful = true; + } + } + } + } + } + + (void)fclose(fd); + } + + return isSuccessful; +} + +void Settings::saveSettings() +{ + const size_t JSON_DOC_SIZE = 4096U; + DynamicJsonDocument jsonDoc(JSON_DOC_SIZE); + size_t jsonBufferSize; + + /* Don't forget to bump the DATA_VERSION every time you modify + * the settings! + */ + + jsonDoc["version"] = DATA_VERSION; + jsonDoc["maxSpeed"] = m_maxSpeed; + + jsonBufferSize = measureJsonPretty(jsonDoc); /* Size without string termination. */ + + { + char jsonBuffer[jsonBufferSize + 1U]; + + if (jsonBufferSize == serializeJsonPretty(jsonDoc, jsonBuffer, jsonBufferSize)) + { + FILE* fd = fopen(SETTINGS_FILE_NAME, "wb"); + + if (nullptr != fd) + { + (void)fwrite(jsonBuffer, jsonBufferSize, 1, fd); + + (void)fclose(fd); + } + } + } +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ + +/** + * Get file size in byte. + * The file must be opened already. + * + * @param[in] fd File descriptor. + * + * @return File size in byte + */ +static long getFileSize(FILE* fd) +{ + long current = ftell(fd); /* Remember current position. */ + long begin; + long end; + + (void)fseek(fd, 0, SEEK_SET); + begin = ftell(fd); + (void)fseek(fd, 0, SEEK_END); + end = ftell(fd); + + /* Restore position. */ + (void)fseek(fd, current, SEEK_SET); + + return end - begin; +} diff --git a/lib/HALSim/Settings.h b/lib/HALSim/Settings.h index af4d573a..24613ee1 100644 --- a/lib/HALSim/Settings.h +++ b/lib/HALSim/Settings.h @@ -1,135 +1,135 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Settings realization - * @author Andreas Merkle - * - * @addtogroup HALTarget - * - * @{ - */ - -#ifndef SETTINGS_H -#define SETTINGS_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ISettings.h" - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class handles settings stored in a JSON file. */ -class Settings : public ISettings -{ -public: - /** - * Constructs the settings adapter. - */ - Settings() : ISettings(), m_maxSpeed(DEFAULT_MAX_SPEED) - { - } - - /** - * Destroys the button A adapter. - */ - ~Settings() - { - } - - /** - * Initialize the settings. - * - * If the settings are invalid or not compatible to the settings, the - * default values will be written! - */ - void init() final; - - /** - * Get the max. speed. - * - * @return Max. speed in steps/s. - */ - int16_t getMaxSpeed() const final; - - /** - * Set the max. speed. - * - * @param[in] maxSpeed Max. speed in steps/s. - */ - void setMaxSpeed(int16_t maxSpeed) final; - -private: - /** - * Name of the settings file. - */ - static const char* SETTINGS_FILE_NAME; - - /** - * Data version is used to detect whether the data in the EEPROM is - * compatible with the current settings version. - * - * Increase the version number by 1 for every change! - */ - static const uint8_t DATA_VERSION; - - /** - * Max. speed default values in steps/s. - */ - static const int16_t DEFAULT_MAX_SPEED = 0; - - int16_t m_maxSpeed; /**< Max. speed in steps/s. */ - - /** - * Load settings from file. - * - * @return If successful, it will return true otherwise false. - */ - bool loadSettings(); - - /** - * Store settings to file. - */ - void saveSettings(); -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* SETTINGS_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Settings realization + * @author Andreas Merkle + * + * @addtogroup HALTarget + * + * @{ + */ + +#ifndef SETTINGS_H +#define SETTINGS_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ISettings.h" + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** This class handles settings stored in a JSON file. */ +class Settings : public ISettings +{ +public: + /** + * Constructs the settings adapter. + */ + Settings() : ISettings(), m_maxSpeed(DEFAULT_MAX_SPEED) + { + } + + /** + * Destroys the button A adapter. + */ + ~Settings() + { + } + + /** + * Initialize the settings. + * + * If the settings are invalid or not compatible to the settings, the + * default values will be written! + */ + void init() final; + + /** + * Get the max. speed. + * + * @return Max. speed in steps/s. + */ + int16_t getMaxSpeed() const final; + + /** + * Set the max. speed. + * + * @param[in] maxSpeed Max. speed in steps/s. + */ + void setMaxSpeed(int16_t maxSpeed) final; + +private: + /** + * Name of the settings file. + */ + static const char* SETTINGS_FILE_NAME; + + /** + * Data version is used to detect whether the data in the EEPROM is + * compatible with the current settings version. + * + * Increase the version number by 1 for every change! + */ + static const uint8_t DATA_VERSION; + + /** + * Max. speed default values in steps/s. + */ + static const int16_t DEFAULT_MAX_SPEED = 0; + + int16_t m_maxSpeed; /**< Max. speed in steps/s. */ + + /** + * Load settings from file. + * + * @return If successful, it will return true otherwise false. + */ + bool loadSettings(); + + /** + * Store settings to file. + */ + void saveSettings(); +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* SETTINGS_H */ +/** @} */ diff --git a/lib/HALSim/SimTime.cpp b/lib/HALSim/SimTime.cpp index 659cf957..c9833b52 100644 --- a/lib/HALSim/SimTime.cpp +++ b/lib/HALSim/SimTime.cpp @@ -1,75 +1,75 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Simulation time - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "SimTime.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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Simulation time + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "SimTime.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALSim/SimTime.h b/lib/HALSim/SimTime.h index 1116b331..4bdbbb8a 100644 --- a/lib/HALSim/SimTime.h +++ b/lib/HALSim/SimTime.h @@ -1,129 +1,129 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Simulation time - * @author Andreas Merkle - * - * @addtogroup HALSim - * - * @{ - */ - -#ifndef SIM_TIME_H -#define SIM_TIME_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** - * Simulation time handler - * Its responsibility is to provide elapsed time information since reset and - * to step the simulation time forward. - */ -class SimTime -{ -public: - /** - * Construct simulation time handler. - * - * @param[in] robot The simulation environment. - */ - SimTime(webots::Robot& robot) : - m_robot(robot), - m_timeStep(static_cast(m_robot.getBasicTimeStep())), - m_elapsedTimeSinceReset(0) - { - } - - /** - * Destroy the simulation time handler. - */ - ~SimTime() - { - } - - /** - * Step the simulation one single step forward. - * How long one step is depends on the basic basic time step configured in - * the webots simulation. See there in the world information. - * - * @return If successful stepped, it will return true otherwise false. - */ - bool step() - { - int result = m_robot.step(m_timeStep); - m_elapsedTimeSinceReset += m_timeStep; - - return (-1 != result); - } - - /** - * Get basic time step in [ms]. - * - * @return Basic time step [ms] - */ - int getTimeStep() const - { - return m_timeStep; - } - - /** - * Get the elapsed time since reset in [ms]. - * - * @return Elapsed time since reset [ms] - */ - unsigned long int getElapsedTimeSinceReset() const - { - return m_elapsedTimeSinceReset; - } - -private: - webots::Robot& m_robot; /**< Simulation environment, used to step the simulation forward. */ - int m_timeStep; /**< Time in ms of one simulation step. */ - unsigned long int m_elapsedTimeSinceReset; /**< Elapsed time since reset in [ms] */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* SIM_TIME_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Simulation time + * @author Andreas Merkle + * + * @addtogroup HALSim + * + * @{ + */ + +#ifndef SIM_TIME_H +#define SIM_TIME_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** + * Simulation time handler + * Its responsibility is to provide elapsed time information since reset and + * to step the simulation time forward. + */ +class SimTime +{ +public: + /** + * Construct simulation time handler. + * + * @param[in] robot The simulation environment. + */ + SimTime(webots::Robot& robot) : + m_robot(robot), + m_timeStep(static_cast(m_robot.getBasicTimeStep())), + m_elapsedTimeSinceReset(0) + { + } + + /** + * Destroy the simulation time handler. + */ + ~SimTime() + { + } + + /** + * Step the simulation one single step forward. + * How long one step is depends on the basic basic time step configured in + * the webots simulation. See there in the world information. + * + * @return If successful stepped, it will return true otherwise false. + */ + bool step() + { + int result = m_robot.step(m_timeStep); + m_elapsedTimeSinceReset += m_timeStep; + + return (-1 != result); + } + + /** + * Get basic time step in [ms]. + * + * @return Basic time step [ms] + */ + int getTimeStep() const + { + return m_timeStep; + } + + /** + * Get the elapsed time since reset in [ms]. + * + * @return Elapsed time since reset [ms] + */ + unsigned long int getElapsedTimeSinceReset() const + { + return m_elapsedTimeSinceReset; + } + +private: + webots::Robot& m_robot; /**< Simulation environment, used to step the simulation forward. */ + int m_timeStep; /**< Time in ms of one simulation step. */ + unsigned long int m_elapsedTimeSinceReset; /**< Elapsed time since reset in [ms] */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* SIM_TIME_H */ +/** @} */ diff --git a/lib/HALSim/WebotsSerialDrv.cpp b/lib/HALSim/WebotsSerialDrv.cpp index 24d01752..28f0f812 100644 --- a/lib/HALSim/WebotsSerialDrv.cpp +++ b/lib/HALSim/WebotsSerialDrv.cpp @@ -1,309 +1,309 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Webots serial driver - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "WebotsSerialDrv.h" -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -WebotsSerialDrv::WebotsSerialDrv(webots::Emitter* emitter, webots::Receiver* receiver) : - Stream(), - m_emitter(emitter), - m_receiver(receiver), - m_dataRemains(0U) -{ - /* Nothing to do. */ -} - -WebotsSerialDrv::~WebotsSerialDrv() -{ - /* Nothing to do. */ -} - -void WebotsSerialDrv::setRxChannel(int32_t channelId) -{ - if (nullptr != m_receiver) - { - /* Shall be positive for inter-robot communication. */ - if (0 <= channelId) - { - m_receiver->setChannel(channelId); - } - } -} - -void WebotsSerialDrv::setTxChannel(int32_t channelId) -{ - if (nullptr != m_emitter) - { - /* Shall be positive for inter-robot communication. */ - if (0 <= channelId) - { - m_emitter->setChannel(channelId); - } - } -} - -void WebotsSerialDrv::print(const char str[]) -{ - if (nullptr != str) - { - (void)m_emitter->send(str, strlen(str)); - } -} - -void WebotsSerialDrv::print(uint8_t value) -{ - char buffer[4U]; /* [0; 255] + string termination */ - int32_t length = snprintf(buffer, sizeof(buffer), "%u", value); - - (void)m_emitter->send(buffer, length); -} - -void WebotsSerialDrv::print(uint16_t value) -{ - char buffer[6U]; /* [0; 65535] + string termination */ - int32_t length = snprintf(buffer, sizeof(buffer), "%u", value); - - (void)m_emitter->send(buffer, length); -} - -void WebotsSerialDrv::print(uint32_t value) -{ - char buffer[11U]; /* [0; 4294967295] + string termination */ - int32_t length = snprintf(buffer, sizeof(buffer), "%u", value); - - (void)m_emitter->send(buffer, length); -} - -void WebotsSerialDrv::print(int8_t value) -{ - char buffer[5U]; /* [-128; 127] + string termination */ - int32_t length = snprintf(buffer, sizeof(buffer), "%d", value); - - (void)m_emitter->send(buffer, length); -} - -void WebotsSerialDrv::print(int16_t value) -{ - char buffer[6U]; /* [-32768; 32767] + string termination */ - int32_t length = snprintf(buffer, sizeof(buffer), "%d", value); - - (void)m_emitter->send(buffer, length); -} - -void WebotsSerialDrv::print(int32_t value) -{ - char buffer[12U]; /* [-2147483648; 2147483647] + string termination */ - int32_t length = snprintf(buffer, sizeof(buffer), "%d", value); - - (void)m_emitter->send(buffer, length); -} - -void WebotsSerialDrv::println(const char str[]) -{ - if (nullptr != str) - { - size_t length = strlen(str); - char buffer[length + 1U]; - - length = snprintf(buffer, sizeof(buffer), "%s\n", str); - - (void)m_emitter->send(buffer, length); - } -} - -void WebotsSerialDrv::println(uint8_t value) -{ - char buffer[5U]; /* [0; 255] + LF + string termination */ - int32_t length = snprintf(buffer, sizeof(buffer), "%u\n", value); - - (void)m_emitter->send(buffer, length); -} - -void WebotsSerialDrv::println(uint16_t value) -{ - char buffer[7U]; /* [0; 65535] + LF + string termination */ - int32_t length = snprintf(buffer, sizeof(buffer), "%u\n", value); - - (void)m_emitter->send(buffer, length); -} - -void WebotsSerialDrv::println(uint32_t value) -{ - char buffer[12U]; /* [0; 4294967295] + LF + string termination */ - int32_t length = snprintf(buffer, sizeof(buffer), "%u\n", value); - - (void)m_emitter->send(buffer, length); -} - -void WebotsSerialDrv::println(int8_t value) -{ - char buffer[6U]; /* [-128; 127] + LF + string termination */ - int32_t length = snprintf(buffer, sizeof(buffer), "%d\n", value); - - (void)m_emitter->send(buffer, length); -} - -void WebotsSerialDrv::println(int16_t value) -{ - char buffer[7U]; /* [-32768; 32767] + LF + string termination */ - int32_t length = snprintf(buffer, sizeof(buffer), "%d\n", value); - - (void)m_emitter->send(buffer, length); -} - -void WebotsSerialDrv::println(int32_t value) -{ - char buffer[13U]; /* [-2147483648; 2147483647] + LF + string termination */ - int32_t length = snprintf(buffer, sizeof(buffer), "%d\n", value); - - (void)m_emitter->send(buffer, length); -} - -size_t WebotsSerialDrv::write(const uint8_t* buffer, size_t length) -{ - int32_t result = m_emitter->send(buffer, length); - const int32_t SUCCESS = 1; - - return (SUCCESS == result) ? length : 0; -} - -int WebotsSerialDrv::available() const -{ - int32_t dataSize = 0; - - /* If in the last read not all data was read, return the remaining bytes. */ - if (0U < m_dataRemains) - { - dataSize = m_dataRemains; - } - /* Otherwise return the size of the current head packet. - * Attention, calling the getDataSize() is illegal in case the queue is - * empty! - */ - else if (0 < m_receiver->getQueueLength()) - { - dataSize = m_receiver->getDataSize(); - } - else - { - /* Nothing to do. */ - ; - } - - return dataSize; -} - -size_t WebotsSerialDrv::readBytes(uint8_t* buffer, size_t length) -{ - size_t read = 0U; - - /* Attention, calling the getData() or getDataSize() is illegal in case the - * queue is empty! - */ - if (0 < m_receiver->getQueueLength()) - { - const uint8_t* message = static_cast(m_receiver->getData()); - int32_t messageSize = m_receiver->getDataSize(); - - /* If in the last read not all data was read, return the remaining bytes. */ - if (0U < m_dataRemains) - { - /* Partial read? */ - if (m_dataRemains > length) - { - read = length; - } - /* Read all. */ - else - { - read = m_dataRemains; - } - - (void)memcpy(buffer, &message[messageSize - m_dataRemains], read); - m_dataRemains -= read; - } - else - { - /* Partial read? */ - if (messageSize > length) - { - read = length; - } - /* Read all. */ - else - { - read = messageSize; - } - - (void)memcpy(buffer, message, read); - m_dataRemains = messageSize - read; - } - - /* If the complete head packet is read, it will be thrown away. */ - if (0U == m_dataRemains) - { - m_receiver->nextPacket(); - } - } - - return read; -} - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Webots serial driver + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "WebotsSerialDrv.h" +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +WebotsSerialDrv::WebotsSerialDrv(webots::Emitter* emitter, webots::Receiver* receiver) : + Stream(), + m_emitter(emitter), + m_receiver(receiver), + m_dataRemains(0U) +{ + /* Nothing to do. */ +} + +WebotsSerialDrv::~WebotsSerialDrv() +{ + /* Nothing to do. */ +} + +void WebotsSerialDrv::setRxChannel(int32_t channelId) +{ + if (nullptr != m_receiver) + { + /* Shall be positive for inter-robot communication. */ + if (0 <= channelId) + { + m_receiver->setChannel(channelId); + } + } +} + +void WebotsSerialDrv::setTxChannel(int32_t channelId) +{ + if (nullptr != m_emitter) + { + /* Shall be positive for inter-robot communication. */ + if (0 <= channelId) + { + m_emitter->setChannel(channelId); + } + } +} + +void WebotsSerialDrv::print(const char str[]) +{ + if (nullptr != str) + { + (void)m_emitter->send(str, strlen(str)); + } +} + +void WebotsSerialDrv::print(uint8_t value) +{ + char buffer[4U]; /* [0; 255] + string termination */ + int32_t length = snprintf(buffer, sizeof(buffer), "%u", value); + + (void)m_emitter->send(buffer, length); +} + +void WebotsSerialDrv::print(uint16_t value) +{ + char buffer[6U]; /* [0; 65535] + string termination */ + int32_t length = snprintf(buffer, sizeof(buffer), "%u", value); + + (void)m_emitter->send(buffer, length); +} + +void WebotsSerialDrv::print(uint32_t value) +{ + char buffer[11U]; /* [0; 4294967295] + string termination */ + int32_t length = snprintf(buffer, sizeof(buffer), "%u", value); + + (void)m_emitter->send(buffer, length); +} + +void WebotsSerialDrv::print(int8_t value) +{ + char buffer[5U]; /* [-128; 127] + string termination */ + int32_t length = snprintf(buffer, sizeof(buffer), "%d", value); + + (void)m_emitter->send(buffer, length); +} + +void WebotsSerialDrv::print(int16_t value) +{ + char buffer[6U]; /* [-32768; 32767] + string termination */ + int32_t length = snprintf(buffer, sizeof(buffer), "%d", value); + + (void)m_emitter->send(buffer, length); +} + +void WebotsSerialDrv::print(int32_t value) +{ + char buffer[12U]; /* [-2147483648; 2147483647] + string termination */ + int32_t length = snprintf(buffer, sizeof(buffer), "%d", value); + + (void)m_emitter->send(buffer, length); +} + +void WebotsSerialDrv::println(const char str[]) +{ + if (nullptr != str) + { + size_t length = strlen(str); + char buffer[length + 1U]; + + length = snprintf(buffer, sizeof(buffer), "%s\n", str); + + (void)m_emitter->send(buffer, length); + } +} + +void WebotsSerialDrv::println(uint8_t value) +{ + char buffer[5U]; /* [0; 255] + LF + string termination */ + int32_t length = snprintf(buffer, sizeof(buffer), "%u\n", value); + + (void)m_emitter->send(buffer, length); +} + +void WebotsSerialDrv::println(uint16_t value) +{ + char buffer[7U]; /* [0; 65535] + LF + string termination */ + int32_t length = snprintf(buffer, sizeof(buffer), "%u\n", value); + + (void)m_emitter->send(buffer, length); +} + +void WebotsSerialDrv::println(uint32_t value) +{ + char buffer[12U]; /* [0; 4294967295] + LF + string termination */ + int32_t length = snprintf(buffer, sizeof(buffer), "%u\n", value); + + (void)m_emitter->send(buffer, length); +} + +void WebotsSerialDrv::println(int8_t value) +{ + char buffer[6U]; /* [-128; 127] + LF + string termination */ + int32_t length = snprintf(buffer, sizeof(buffer), "%d\n", value); + + (void)m_emitter->send(buffer, length); +} + +void WebotsSerialDrv::println(int16_t value) +{ + char buffer[7U]; /* [-32768; 32767] + LF + string termination */ + int32_t length = snprintf(buffer, sizeof(buffer), "%d\n", value); + + (void)m_emitter->send(buffer, length); +} + +void WebotsSerialDrv::println(int32_t value) +{ + char buffer[13U]; /* [-2147483648; 2147483647] + LF + string termination */ + int32_t length = snprintf(buffer, sizeof(buffer), "%d\n", value); + + (void)m_emitter->send(buffer, length); +} + +size_t WebotsSerialDrv::write(const uint8_t* buffer, size_t length) +{ + int32_t result = m_emitter->send(buffer, length); + const int32_t SUCCESS = 1; + + return (SUCCESS == result) ? length : 0; +} + +int WebotsSerialDrv::available() const +{ + int32_t dataSize = 0; + + /* If in the last read not all data was read, return the remaining bytes. */ + if (0U < m_dataRemains) + { + dataSize = m_dataRemains; + } + /* Otherwise return the size of the current head packet. + * Attention, calling the getDataSize() is illegal in case the queue is + * empty! + */ + else if (0 < m_receiver->getQueueLength()) + { + dataSize = m_receiver->getDataSize(); + } + else + { + /* Nothing to do. */ + ; + } + + return dataSize; +} + +size_t WebotsSerialDrv::readBytes(uint8_t* buffer, size_t length) +{ + size_t read = 0U; + + /* Attention, calling the getData() or getDataSize() is illegal in case the + * queue is empty! + */ + if (0 < m_receiver->getQueueLength()) + { + const uint8_t* message = static_cast(m_receiver->getData()); + int32_t messageSize = m_receiver->getDataSize(); + + /* If in the last read not all data was read, return the remaining bytes. */ + if (0U < m_dataRemains) + { + /* Partial read? */ + if (m_dataRemains > length) + { + read = length; + } + /* Read all. */ + else + { + read = m_dataRemains; + } + + (void)memcpy(buffer, &message[messageSize - m_dataRemains], read); + m_dataRemains -= read; + } + else + { + /* Partial read? */ + if (messageSize > length) + { + read = length; + } + /* Read all. */ + else + { + read = messageSize; + } + + (void)memcpy(buffer, message, read); + m_dataRemains = messageSize - read; + } + + /* If the complete head packet is read, it will be thrown away. */ + if (0U == m_dataRemains) + { + m_receiver->nextPacket(); + } + } + + return read; +} + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALSim/WebotsSerialDrv.h b/lib/HALSim/WebotsSerialDrv.h index c5d8253c..857700e9 100644 --- a/lib/HALSim/WebotsSerialDrv.h +++ b/lib/HALSim/WebotsSerialDrv.h @@ -1,231 +1,231 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Webots serial driver - * @author Andreas Merkle - * - * @addtogroup HALSim - * - * @{ - */ - -#ifndef WEBOTS_SERIAL_DRV_H -#define WEBOTS_SERIAL_DRV_H - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "Stream.h" -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** - * Webots serial driver. - */ -class WebotsSerialDrv : public Stream -{ -public: - /** - * Construct a Webots serial driver. - * - * @param[in] emitter Webots emitter used to send data over simulated serial. - * @param[in] receiver Webots receiver used to receive data over simulated serial. - */ - WebotsSerialDrv(webots::Emitter* emitter, webots::Receiver* receiver); - - /** - * Destruct the Webots serial driver. - */ - virtual ~WebotsSerialDrv(); - - /** - * Set the serial receive channel id. - * - * @param[in] channelId Channel ID, shall be positive for inter-robot communication. - */ - void setRxChannel(int32_t channelId); - - /** - * Set the serial sender channel id. - * - * @param[in] channelId Channel ID, shall be positive for inter-robot communication. - */ - void setTxChannel(int32_t channelId); - - /** - * Print argument to the output stream. - * - * @param[in] str Argument to print. - */ - void print(const char str[]) final; - - /** - * Print argument to the output stream. - * - * @param[in] value Argument to print. - */ - void print(uint8_t value) final; - - /** - * Print argument to the output stream. - * - * @param[in] value Argument to print. - */ - void print(uint16_t value) final; - - /** - * Print argument to the output stream. - * - * @param[in] value Argument to print. - */ - void print(uint32_t value) final; - - /** - * Print argument to the output stream. - * - * @param[in] value Argument to print. - */ - void print(int8_t value) final; - - /** - * Print argument to the output stream. - * - * @param[in] value Argument to print. - */ - void print(int16_t value) final; - - /** - * Print argument to the output stream. - * - * @param[in] value Argument to print. - */ - void print(int32_t value) final; - - /** - * Print argument to the output stream. - * Appends carriage return at the end of the argument. - * - * @param[in] str Argument to print. - */ - void println(const char str[]) final; - - /** - * Print argument to the output stream. - * Appends carriage return at the end of the argument. - * - * @param[in] value Argument to print. - */ - void println(uint8_t value) final; - - /** - * Print argument to the output stream. - * Appends carriage return at the end of the argument. - * - * @param[in] value Argument to print. - */ - void println(uint16_t value) final; - - /** - * Print argument to the output stream. - * Appends carriage return at the end of the argument. - * - * @param[in] value Argument to print. - */ - void println(uint32_t value) final; - - /** - * Print argument to the output stream. - * Appends carriage return at the end of the argument. - * - * @param[in] value Argument to print. - */ - void println(int8_t value) final; - - /** - * Print argument to the output stream. - * Appends carriage return at the end of the argument. - * - * @param[in] value Argument to print. - */ - void println(int16_t value) final; - - /** - * Print argument to the output stream. - * Appends carriage return at the end of the argument. - * - * @param[in] value Argument to print. - */ - void println(int32_t value) final; - - /** - * Write data buffer to serial. - * - * @param[in] buffer Byte buffer to send. - * @param[in] length Number of bytes to send. - * - * @return Number of bytes written. - */ - size_t write(const uint8_t* buffer, size_t length) final; - - /** - * Check if any data has been received. - * - * @return Number of available bytes. - */ - int available() const final; - - /** - * Read data from serial. - * - * @param[in] buffer Array to read data in. - * @param[in] length Number of bytes to be read. - * - * @return Number of bytes read from stream. - */ - size_t readBytes(uint8_t* buffer, size_t length) final; - -private: - webots::Emitter* m_emitter; /**< The emitter used to send data. */ - webots::Receiver* m_receiver; /**< The receiver used to receive data. */ - size_t m_dataRemains; /**< Number of bytes which are remaining in head packet. */ - - /* Not allowed. */ - WebotsSerialDrv(); - WebotsSerialDrv(const WebotsSerialDrv& srv); /**< Copy construction of an instance. */ - WebotsSerialDrv& operator=(const WebotsSerialDrv& srv); /**< Assignment of an instance. */ -}; - -#endif /* WEBOTS_SERIAL_DRV_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Webots serial driver + * @author Andreas Merkle + * + * @addtogroup HALSim + * + * @{ + */ + +#ifndef WEBOTS_SERIAL_DRV_H +#define WEBOTS_SERIAL_DRV_H + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "Stream.h" +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** + * Webots serial driver. + */ +class WebotsSerialDrv : public Stream +{ +public: + /** + * Construct a Webots serial driver. + * + * @param[in] emitter Webots emitter used to send data over simulated serial. + * @param[in] receiver Webots receiver used to receive data over simulated serial. + */ + WebotsSerialDrv(webots::Emitter* emitter, webots::Receiver* receiver); + + /** + * Destruct the Webots serial driver. + */ + virtual ~WebotsSerialDrv(); + + /** + * Set the serial receive channel id. + * + * @param[in] channelId Channel ID, shall be positive for inter-robot communication. + */ + void setRxChannel(int32_t channelId); + + /** + * Set the serial sender channel id. + * + * @param[in] channelId Channel ID, shall be positive for inter-robot communication. + */ + void setTxChannel(int32_t channelId); + + /** + * Print argument to the output stream. + * + * @param[in] str Argument to print. + */ + void print(const char str[]) final; + + /** + * Print argument to the output stream. + * + * @param[in] value Argument to print. + */ + void print(uint8_t value) final; + + /** + * Print argument to the output stream. + * + * @param[in] value Argument to print. + */ + void print(uint16_t value) final; + + /** + * Print argument to the output stream. + * + * @param[in] value Argument to print. + */ + void print(uint32_t value) final; + + /** + * Print argument to the output stream. + * + * @param[in] value Argument to print. + */ + void print(int8_t value) final; + + /** + * Print argument to the output stream. + * + * @param[in] value Argument to print. + */ + void print(int16_t value) final; + + /** + * Print argument to the output stream. + * + * @param[in] value Argument to print. + */ + void print(int32_t value) final; + + /** + * Print argument to the output stream. + * Appends carriage return at the end of the argument. + * + * @param[in] str Argument to print. + */ + void println(const char str[]) final; + + /** + * Print argument to the output stream. + * Appends carriage return at the end of the argument. + * + * @param[in] value Argument to print. + */ + void println(uint8_t value) final; + + /** + * Print argument to the output stream. + * Appends carriage return at the end of the argument. + * + * @param[in] value Argument to print. + */ + void println(uint16_t value) final; + + /** + * Print argument to the output stream. + * Appends carriage return at the end of the argument. + * + * @param[in] value Argument to print. + */ + void println(uint32_t value) final; + + /** + * Print argument to the output stream. + * Appends carriage return at the end of the argument. + * + * @param[in] value Argument to print. + */ + void println(int8_t value) final; + + /** + * Print argument to the output stream. + * Appends carriage return at the end of the argument. + * + * @param[in] value Argument to print. + */ + void println(int16_t value) final; + + /** + * Print argument to the output stream. + * Appends carriage return at the end of the argument. + * + * @param[in] value Argument to print. + */ + void println(int32_t value) final; + + /** + * Write data buffer to serial. + * + * @param[in] buffer Byte buffer to send. + * @param[in] length Number of bytes to send. + * + * @return Number of bytes written. + */ + size_t write(const uint8_t* buffer, size_t length) final; + + /** + * Check if any data has been received. + * + * @return Number of available bytes. + */ + int available() const final; + + /** + * Read data from serial. + * + * @param[in] buffer Array to read data in. + * @param[in] length Number of bytes to be read. + * + * @return Number of bytes read from stream. + */ + size_t readBytes(uint8_t* buffer, size_t length) final; + +private: + webots::Emitter* m_emitter; /**< The emitter used to send data. */ + webots::Receiver* m_receiver; /**< The receiver used to receive data. */ + size_t m_dataRemains; /**< Number of bytes which are remaining in head packet. */ + + /* Not allowed. */ + WebotsSerialDrv(); + WebotsSerialDrv(const WebotsSerialDrv& srv); /**< Copy construction of an instance. */ + WebotsSerialDrv& operator=(const WebotsSerialDrv& srv); /**< Assignment of an instance. */ +}; + +#endif /* WEBOTS_SERIAL_DRV_H */ +/** @} */ diff --git a/lib/HALSim/library.json b/lib/HALSim/library.json index 57792935..8d919483 100644 --- a/lib/HALSim/library.json +++ b/lib/HALSim/library.json @@ -1,19 +1,19 @@ -{ - "name": "HALSim", - "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": "Webots" - }], - "frameworks": "*", - "platforms": "*" +{ + "name": "HALSim", + "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": "Webots" + }], + "frameworks": "*", + "platforms": "*" } \ No newline at end of file diff --git a/lib/HALTarget/ButtonA.cpp b/lib/HALTarget/ButtonA.cpp index c37847b5..6ece3e08 100644 --- a/lib/HALTarget/ButtonA.cpp +++ b/lib/HALTarget/ButtonA.cpp @@ -1,75 +1,75 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Button A realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ButtonA.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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Button A realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ButtonA.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALTarget/ButtonA.h b/lib/HALTarget/ButtonA.h index 5fb2f169..53caa792 100644 --- a/lib/HALTarget/ButtonA.h +++ b/lib/HALTarget/ButtonA.h @@ -1,102 +1,102 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Button A realization - * @author Andreas Merkle - * - * @addtogroup HALTarget - * - * @{ - */ - -#ifndef BUTTONA_H -#define BUTTONA_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "IButton.h" -#include "Zumo32U4.h" - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class provides access to the Zumo target button A. */ -class ButtonA : public IButton -{ -public: - /** - * Constructs the button A adapter. - */ - ButtonA() : IButton(), m_button() - { - } - - /** - * Destroys the button A adapter. - */ - ~ButtonA() - { - } - - /** - * Is button pressed or not - * - * @return If button is pressed, returns true otherwise false. - */ - bool isPressed() final - { - return m_button.getSingleDebouncedPress(); - } - - /** - * Wait until button is released. - */ - void waitForRelease() final - { - m_button.waitForRelease(); - } - -private: - Zumo32U4ButtonA m_button; /**< Zumo button driver from Pololu */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* BUTTONA_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Button A realization + * @author Andreas Merkle + * + * @addtogroup HALTarget + * + * @{ + */ + +#ifndef BUTTONA_H +#define BUTTONA_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "IButton.h" +#include "Zumo32U4.h" + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** This class provides access to the Zumo target button A. */ +class ButtonA : public IButton +{ +public: + /** + * Constructs the button A adapter. + */ + ButtonA() : IButton(), m_button() + { + } + + /** + * Destroys the button A adapter. + */ + ~ButtonA() + { + } + + /** + * Is button pressed or not + * + * @return If button is pressed, returns true otherwise false. + */ + bool isPressed() final + { + return m_button.getSingleDebouncedPress(); + } + + /** + * Wait until button is released. + */ + void waitForRelease() final + { + m_button.waitForRelease(); + } + +private: + Zumo32U4ButtonA m_button; /**< Zumo button driver from Pololu */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* BUTTONA_H */ +/** @} */ diff --git a/lib/HALTarget/ButtonB.cpp b/lib/HALTarget/ButtonB.cpp index 8d41e381..7ea5381d 100644 --- a/lib/HALTarget/ButtonB.cpp +++ b/lib/HALTarget/ButtonB.cpp @@ -1,75 +1,75 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Button B realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ButtonB.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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Button B realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ButtonB.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALTarget/ButtonB.h b/lib/HALTarget/ButtonB.h index 27c3f327..911a56fa 100644 --- a/lib/HALTarget/ButtonB.h +++ b/lib/HALTarget/ButtonB.h @@ -1,102 +1,102 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Button B realization - * @author Andreas Merkle - * - * @addtogroup HALTarget - * - * @{ - */ - -#ifndef BUTTONB_H -#define BUTTONB_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "IButton.h" -#include "Zumo32U4.h" - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class provides access to the Zumo target button B. */ -class ButtonB : public IButton -{ -public: - /** - * Constructs the button B adapter. - */ - ButtonB() : IButton(), m_button() - { - } - - /** - * Destroys the button B adapter. - */ - ~ButtonB() - { - } - - /** - * Is button pressed or not - * - * @return If button is pressed, returns true otherwise false. - */ - bool isPressed() final - { - return m_button.getSingleDebouncedPress(); - } - - /** - * Wait until button is released. - */ - void waitForRelease() final - { - m_button.waitForRelease(); - } - -private: - Zumo32U4ButtonB m_button; /**< Zumo button driver from Pololu */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* BUTTONB_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Button B realization + * @author Andreas Merkle + * + * @addtogroup HALTarget + * + * @{ + */ + +#ifndef BUTTONB_H +#define BUTTONB_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "IButton.h" +#include "Zumo32U4.h" + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** This class provides access to the Zumo target button B. */ +class ButtonB : public IButton +{ +public: + /** + * Constructs the button B adapter. + */ + ButtonB() : IButton(), m_button() + { + } + + /** + * Destroys the button B adapter. + */ + ~ButtonB() + { + } + + /** + * Is button pressed or not + * + * @return If button is pressed, returns true otherwise false. + */ + bool isPressed() final + { + return m_button.getSingleDebouncedPress(); + } + + /** + * Wait until button is released. + */ + void waitForRelease() final + { + m_button.waitForRelease(); + } + +private: + Zumo32U4ButtonB m_button; /**< Zumo button driver from Pololu */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* BUTTONB_H */ +/** @} */ diff --git a/lib/HALTarget/ButtonC.cpp b/lib/HALTarget/ButtonC.cpp index 5103da49..b3946a38 100644 --- a/lib/HALTarget/ButtonC.cpp +++ b/lib/HALTarget/ButtonC.cpp @@ -1,75 +1,75 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Button C realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ButtonC.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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Button C realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ButtonC.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALTarget/ButtonC.h b/lib/HALTarget/ButtonC.h index fef07ace..d90000ea 100644 --- a/lib/HALTarget/ButtonC.h +++ b/lib/HALTarget/ButtonC.h @@ -1,102 +1,102 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Button C realization - * @author Andreas Merkle - * - * @addtogroup HALTarget - * - * @{ - */ - -#ifndef BUTTONC_H -#define BUTTONC_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "IButton.h" -#include "Zumo32U4.h" - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class provides access to the Zumo target button C. */ -class ButtonC : public IButton -{ -public: - /** - * Constructs the button C adapter. - */ - ButtonC() : IButton(), m_button() - { - } - - /** - * Destroys the button C adapter. - */ - ~ButtonC() - { - } - - /** - * Is button pressed or not - * - * @return If button is pressed, returns true otherwise false. - */ - bool isPressed() final - { - return m_button.getSingleDebouncedPress(); - } - - /** - * Wait until button is released. - */ - void waitForRelease() final - { - m_button.waitForRelease(); - } - -private: - Zumo32U4ButtonC m_button; /**< Zumo button driver from Pololu */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* BUTTONC_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Button C realization + * @author Andreas Merkle + * + * @addtogroup HALTarget + * + * @{ + */ + +#ifndef BUTTONC_H +#define BUTTONC_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "IButton.h" +#include "Zumo32U4.h" + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** This class provides access to the Zumo target button C. */ +class ButtonC : public IButton +{ +public: + /** + * Constructs the button C adapter. + */ + ButtonC() : IButton(), m_button() + { + } + + /** + * Destroys the button C adapter. + */ + ~ButtonC() + { + } + + /** + * Is button pressed or not + * + * @return If button is pressed, returns true otherwise false. + */ + bool isPressed() final + { + return m_button.getSingleDebouncedPress(); + } + + /** + * Wait until button is released. + */ + void waitForRelease() final + { + m_button.waitForRelease(); + } + +private: + Zumo32U4ButtonC m_button; /**< Zumo button driver from Pololu */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* BUTTONC_H */ +/** @} */ diff --git a/lib/HALTarget/Buzzer.cpp b/lib/HALTarget/Buzzer.cpp index 1121af76..e9dcc720 100644 --- a/lib/HALTarget/Buzzer.cpp +++ b/lib/HALTarget/Buzzer.cpp @@ -1,91 +1,91 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Buzzer realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "Buzzer.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void Buzzer::playFrequency(uint16_t freq, uint16_t duration, uint8_t volume) -{ - unsigned int freqTarget = freq; - - /* If the frequency has a fractional value, the most significant bit in the target frequency - * must be set. This means that the lower 15 bit are considered in 0.1 Hz. - */ - if (0 != (freq % 10)) - { - freqTarget |= DIV_BY_10; - } - else - /* The target frequency is considered in Hz, therefore divide by 10. */ - { - freqTarget /= 10; - } - - m_buzzer.playFrequency(freqTarget, duration, volume); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Buzzer realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "Buzzer.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void Buzzer::playFrequency(uint16_t freq, uint16_t duration, uint8_t volume) +{ + unsigned int freqTarget = freq; + + /* If the frequency has a fractional value, the most significant bit in the target frequency + * must be set. This means that the lower 15 bit are considered in 0.1 Hz. + */ + if (0 != (freq % 10)) + { + freqTarget |= DIV_BY_10; + } + else + /* The target frequency is considered in Hz, therefore divide by 10. */ + { + freqTarget /= 10; + } + + m_buzzer.playFrequency(freqTarget, duration, volume); +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ diff --git a/lib/HALTarget/Display.cpp b/lib/HALTarget/Display.cpp index a3628897..6468b6cb 100644 --- a/lib/HALTarget/Display.cpp +++ b/lib/HALTarget/Display.cpp @@ -1,75 +1,75 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Display realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "Display.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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Display realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "Display.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALTarget/Display.h b/lib/HALTarget/Display.h index ef0b8720..f3f85286 100644 --- a/lib/HALTarget/Display.h +++ b/lib/HALTarget/Display.h @@ -1,187 +1,187 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Display realization - * @author Andreas Merkle - * - * @addtogroup HALTarget - * - * @{ - */ - -#ifndef DISPLAY_H -#define DISPLAY_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "IDisplay.h" -#include "Zumo32U4.h" - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class provides access to the Zumo target LCD. */ -class Display : public IDisplay -{ -public: - /** - * Constructs the display adapter. - */ - Display() : IDisplay(), m_lcd() - { - } - - /** - * Destroys the display adapter. - */ - ~Display() - { - } - - /** - * Clear the display and set the cursor to the upper left corner. - */ - void clear() final - { - m_lcd.clear(); - } - - /** - * Set the cursor to the given position. - * - * @param[in] xCoord x-coordinate, 0 is the most left position. - * @param[in] yCoord y-coordinate, 0 is the most upper position. - */ - void gotoXY(uint8_t xCoord, uint8_t yCoord) final - { - m_lcd.gotoXY(xCoord, yCoord); - } - - /** - * Print the string to the display at the current cursor position. - * - * @param[in] str String - * - * @return Printed number of characters - */ - size_t print(const char str[]) final - { - return m_lcd.print(str); - } - - /** - * Print the unsigned 8-bit value to the display at the current cursor position. - * - * @param[in] value Value - * - * @return Printed number of characters - */ - size_t print(uint8_t value) final - { - return m_lcd.print(value); - } - - /** - * Print the unsigned 16-bit value to the display at the current cursor position. - * - * @param[in] value Value - * - * @return Printed number of characters - */ - size_t print(uint16_t value) final - { - return m_lcd.print(value); - } - - /** - * Print the unsigned 32-bit value to the display at the current cursor position. - * - * @param[in] value Value - * - * @return Printed number of characters - */ - size_t print(uint32_t value) final - { - return m_lcd.print(value); - } - - /** - * Print the signed 8-bit value to the display at the current cursor position. - * - * @param[in] value Value - * - * @return Printed number of characters - */ - size_t print(int8_t value) final - { - return m_lcd.print(value); - } - - /** - * Print the signed 16-bit value to the display at the current cursor position. - * - * @param[in] value Value - * - * @return Printed number of characters - */ - size_t print(int16_t value) final - { - return m_lcd.print(value); - } - - /** - * Print the signed 32-bit value to the display at the current cursor position. - * - * @param[in] value Value - * - * @return Printed number of characters - */ - size_t print(int32_t value) final - { - return m_lcd.print(value); - } - -private: - Zumo32U4LCD m_lcd; /**< Zumo LCD driver from Pololu */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* DISPLAY_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Display realization + * @author Andreas Merkle + * + * @addtogroup HALTarget + * + * @{ + */ + +#ifndef DISPLAY_H +#define DISPLAY_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "IDisplay.h" +#include "Zumo32U4.h" + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** This class provides access to the Zumo target LCD. */ +class Display : public IDisplay +{ +public: + /** + * Constructs the display adapter. + */ + Display() : IDisplay(), m_lcd() + { + } + + /** + * Destroys the display adapter. + */ + ~Display() + { + } + + /** + * Clear the display and set the cursor to the upper left corner. + */ + void clear() final + { + m_lcd.clear(); + } + + /** + * Set the cursor to the given position. + * + * @param[in] xCoord x-coordinate, 0 is the most left position. + * @param[in] yCoord y-coordinate, 0 is the most upper position. + */ + void gotoXY(uint8_t xCoord, uint8_t yCoord) final + { + m_lcd.gotoXY(xCoord, yCoord); + } + + /** + * Print the string to the display at the current cursor position. + * + * @param[in] str String + * + * @return Printed number of characters + */ + size_t print(const char str[]) final + { + return m_lcd.print(str); + } + + /** + * Print the unsigned 8-bit value to the display at the current cursor position. + * + * @param[in] value Value + * + * @return Printed number of characters + */ + size_t print(uint8_t value) final + { + return m_lcd.print(value); + } + + /** + * Print the unsigned 16-bit value to the display at the current cursor position. + * + * @param[in] value Value + * + * @return Printed number of characters + */ + size_t print(uint16_t value) final + { + return m_lcd.print(value); + } + + /** + * Print the unsigned 32-bit value to the display at the current cursor position. + * + * @param[in] value Value + * + * @return Printed number of characters + */ + size_t print(uint32_t value) final + { + return m_lcd.print(value); + } + + /** + * Print the signed 8-bit value to the display at the current cursor position. + * + * @param[in] value Value + * + * @return Printed number of characters + */ + size_t print(int8_t value) final + { + return m_lcd.print(value); + } + + /** + * Print the signed 16-bit value to the display at the current cursor position. + * + * @param[in] value Value + * + * @return Printed number of characters + */ + size_t print(int16_t value) final + { + return m_lcd.print(value); + } + + /** + * Print the signed 32-bit value to the display at the current cursor position. + * + * @param[in] value Value + * + * @return Printed number of characters + */ + size_t print(int32_t value) final + { + return m_lcd.print(value); + } + +private: + Zumo32U4LCD m_lcd; /**< Zumo LCD driver from Pololu */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* DISPLAY_H */ +/** @} */ diff --git a/lib/HALTarget/Encoders.cpp b/lib/HALTarget/Encoders.cpp index 9eab875e..ce06f385 100644 --- a/lib/HALTarget/Encoders.cpp +++ b/lib/HALTarget/Encoders.cpp @@ -1,75 +1,75 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Encoders realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "Encoders.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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Encoders realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "Encoders.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALTarget/Encoders.h b/lib/HALTarget/Encoders.h index 6d05c803..9c43b845 100644 --- a/lib/HALTarget/Encoders.h +++ b/lib/HALTarget/Encoders.h @@ -1,151 +1,151 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Encoders realization - * @author Andreas Merkle - * - * @addtogroup HALTarget - * - * @{ - */ - -#ifndef ENCODERS_H -#define ENCODERS_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "IEncoders.h" -#include "Zumo32U4.h" - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class provides access to the Zumo target encoders. */ -class Encoders : public IEncoders -{ -public: - /** - * Constructs the encoders adapter. - */ - Encoders() : IEncoders(), m_encoders() - { - } - - /** - * Destroys the encoders adapter. - */ - ~Encoders() - { - } - - /** - * Initialize or re-initialize the encoders. - */ - void init() final - { - /* Nothing to do.*/ - } - - /** - * Returns the number of counts that have been detected from the left-side - * encoder. These counts start at 0. Positive counts correspond to forward - * movement of the left side of the robot, while negative counts correspond - * to backwards movement. - * - * The count is returned as a signed 16-bit integer. When the count goes - * over 32767, it will overflow down to -32768. When the count goes below - * -32768, it will overflow up to 32767. - * - * @return Encoder steps left - */ - int16_t getCountsLeft() final - { - return m_encoders.getCountsLeft(); - } - - /** - * Returns the number of counts that have been detected from the right-side - * encoder. These counts start at 0. Positive counts correspond to forward - * movement of the left side of the robot, while negative counts correspond - * to backwards movement. - * - * The count is returned as a signed 16-bit integer. When the count goes - * over 32767, it will overflow down to -32768. When the count goes below - * -32768, it will overflow up to 32767. - * - * @return Encoder steps right - */ - int16_t getCountsRight() final - { - return m_encoders.getCountsRight(); - } - - /** - * This function is just like getCountsLeft() except it also clears the - * counts before returning. If you call this frequently enough, you will - * not have to worry about the count overflowing. - * - * @return Encoder steps left - */ - int16_t getCountsAndResetLeft() final - { - return m_encoders.getCountsAndResetLeft(); - } - - /** - * This function is just like getCountsRight() except it also clears the - * counts before returning. If you call this frequently enough, you will - * not have to worry about the count overflowing. - * - * @return Encoder steps right - */ - int16_t getCountsAndResetRight() final - { - return m_encoders.getCountsAndResetRight(); - } - -private: - - Zumo32U4Encoders m_encoders; /**< Zumo encoders driver from Pololu */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* ENCODERS_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Encoders realization + * @author Andreas Merkle + * + * @addtogroup HALTarget + * + * @{ + */ + +#ifndef ENCODERS_H +#define ENCODERS_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "IEncoders.h" +#include "Zumo32U4.h" + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** This class provides access to the Zumo target encoders. */ +class Encoders : public IEncoders +{ +public: + /** + * Constructs the encoders adapter. + */ + Encoders() : IEncoders(), m_encoders() + { + } + + /** + * Destroys the encoders adapter. + */ + ~Encoders() + { + } + + /** + * Initialize or re-initialize the encoders. + */ + void init() final + { + /* Nothing to do.*/ + } + + /** + * Returns the number of counts that have been detected from the left-side + * encoder. These counts start at 0. Positive counts correspond to forward + * movement of the left side of the robot, while negative counts correspond + * to backwards movement. + * + * The count is returned as a signed 16-bit integer. When the count goes + * over 32767, it will overflow down to -32768. When the count goes below + * -32768, it will overflow up to 32767. + * + * @return Encoder steps left + */ + int16_t getCountsLeft() final + { + return m_encoders.getCountsLeft(); + } + + /** + * Returns the number of counts that have been detected from the right-side + * encoder. These counts start at 0. Positive counts correspond to forward + * movement of the left side of the robot, while negative counts correspond + * to backwards movement. + * + * The count is returned as a signed 16-bit integer. When the count goes + * over 32767, it will overflow down to -32768. When the count goes below + * -32768, it will overflow up to 32767. + * + * @return Encoder steps right + */ + int16_t getCountsRight() final + { + return m_encoders.getCountsRight(); + } + + /** + * This function is just like getCountsLeft() except it also clears the + * counts before returning. If you call this frequently enough, you will + * not have to worry about the count overflowing. + * + * @return Encoder steps left + */ + int16_t getCountsAndResetLeft() final + { + return m_encoders.getCountsAndResetLeft(); + } + + /** + * This function is just like getCountsRight() except it also clears the + * counts before returning. If you call this frequently enough, you will + * not have to worry about the count overflowing. + * + * @return Encoder steps right + */ + int16_t getCountsAndResetRight() final + { + return m_encoders.getCountsAndResetRight(); + } + +private: + + Zumo32U4Encoders m_encoders; /**< Zumo encoders driver from Pololu */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* ENCODERS_H */ +/** @} */ diff --git a/lib/HALTarget/IMU.cpp b/lib/HALTarget/IMU.cpp index e2b7af08..83fcb759 100644 --- a/lib/HALTarget/IMU.cpp +++ b/lib/HALTarget/IMU.cpp @@ -1,227 +1,227 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief IMU implementation - * @author Juliane Kerpe - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "IMU.h" -#include -#include -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ -bool IMU::init() -{ - Wire.begin(); - return m_imuDrv.init(); -} - -void IMU::enableDefault() -{ - m_imuDrv.enableDefault(); -} - -void IMU::configureForTurnSensing() -{ - m_imuDrv.configureForTurnSensing(); - /* Set a different Full Scale Factor. The Original Function uses +/- 2000 dps which is far too high for the our - * purposes. The values are defined in the Data Sheets of the gyros L3GD20H and LSM6DS33. - * - * Disabling the non-used axes boosts the performance of the Pololu Zumo. */ - switch (m_imuDrv.getType()) - { - case Zumo32U4IMUType::LSM303D_L3GD20H: - /* Set the Full Scale factor of the L3GD20H to +/- 500 dps (0x10 = 0b00010000) */ - m_imuDrv.writeReg(L3GD20H_ADDR, L3GD20H_REG_CTRL4, 0x10); - - /* Enable only the x-axis of the LSM303D accelerometer. */ - m_imuDrv.writeReg(LSM303D_ADDR, LSM303D_REG_CTRL1, 0x51); - - /* Enable only the z-axis of the L3GD20H gyroscope. */ - m_imuDrv.writeReg(L3GD20H_ADDR, L3GD20H_REG_CTRL1, 0xFC); - break; - case Zumo32U4IMUType::LSM6DS33_LIS3MDL: - /* Set the Full Scale factor of the LSM6DS33 to +/- 500 dps (0x7C = 0b01110100). Also set the Output Data Rate - * to 833 Hz (high performance). */ - m_imuDrv.writeReg(LSM6DS33_ADDR, LSM6DS33_REG_CTRL2_G, 0x74); - - /* Enable only the x-axis of the LSM6DS33 accelerometer. 0x18 is the address of the CTRL9_XL Register. */ - m_imuDrv.writeReg(LSM6DS33_ADDR, 0x18, 0x8); - - /* Enable only the z-axis of the LSM6DS33 gyroscope. 0x19 is the address of the CTRL10_C Register. */ - m_imuDrv.writeReg(LSM6DS33_ADDR, 0x19, 0x20); - break; - default: - return; - } -} - -void IMU::readAccelerometer() -{ - m_imuDrv.readAcc(); - m_accelerometerValues.valueX = m_imuDrv.a.x - m_rawAccelerometerOffsetX; - m_accelerometerValues.valueY = 0; - m_accelerometerValues.valueZ = 0; -} - -void IMU::readGyro() -{ - m_imuDrv.readGyro(); - m_gyroValues.valueX = 0; - m_gyroValues.valueY = 0; - m_gyroValues.valueZ = m_imuDrv.g.z - m_rawGyroOffsetZ; -} - -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; - accelerationValues->valueY = m_accelerometerValues.valueY; - accelerationValues->valueZ = m_accelerometerValues.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() -{ - /* Wait for a short amount of time to make sure, the IMU is done setting up. */ - delay(100U); - - /* Define how many measurements should be made for calibration. */ - const int32_t NUMBER_OF_MEASUREMENTS = 50; - - /* Calibration takes place while the robot doesn't move. Therefore the Acceleration and turn values are near 0 and - * int16_t values are enough. */ - int32_t sumOfRawAccelValuesX = 0; - - int32_t sumOfRawGyroValuesZ = 0; - - int32_t measurementIndex = 0; - while (measurementIndex < NUMBER_OF_MEASUREMENTS) - { - if ((true == m_imuDrv.accDataReady()) && (true == m_imuDrv.gyroDataReady())) - { - m_imuDrv.readAcc(); - m_imuDrv.readGyro(); - sumOfRawGyroValuesZ += m_imuDrv.g.z; - sumOfRawAccelValuesX += m_imuDrv.a.x; - ++measurementIndex; - } - else - { - /* Do nothing and wati until new Sensor Data is available. */ - delay(20U); - } - } - - m_rawAccelerometerOffsetX = static_cast(sumOfRawAccelValuesX / NUMBER_OF_MEASUREMENTS); /* In digits */ - - m_rawGyroOffsetZ = static_cast(sumOfRawGyroValuesZ / NUMBER_OF_MEASUREMENTS); /* In digits */ -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief IMU implementation + * @author Juliane Kerpe + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "IMU.h" +#include +#include +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ +bool IMU::init() +{ + Wire.begin(); + return m_imuDrv.init(); +} + +void IMU::enableDefault() +{ + m_imuDrv.enableDefault(); +} + +void IMU::configureForTurnSensing() +{ + m_imuDrv.configureForTurnSensing(); + /* Set a different Full Scale Factor. The Original Function uses +/- 2000 dps which is far too high for the our + * purposes. The values are defined in the Data Sheets of the gyros L3GD20H and LSM6DS33. + * + * Disabling the non-used axes boosts the performance of the Pololu Zumo. */ + switch (m_imuDrv.getType()) + { + case Zumo32U4IMUType::LSM303D_L3GD20H: + /* Set the Full Scale factor of the L3GD20H to +/- 500 dps (0x10 = 0b00010000) */ + m_imuDrv.writeReg(L3GD20H_ADDR, L3GD20H_REG_CTRL4, 0x10); + + /* Enable only the x-axis of the LSM303D accelerometer. */ + m_imuDrv.writeReg(LSM303D_ADDR, LSM303D_REG_CTRL1, 0x51); + + /* Enable only the z-axis of the L3GD20H gyroscope. */ + m_imuDrv.writeReg(L3GD20H_ADDR, L3GD20H_REG_CTRL1, 0xFC); + break; + case Zumo32U4IMUType::LSM6DS33_LIS3MDL: + /* Set the Full Scale factor of the LSM6DS33 to +/- 500 dps (0x7C = 0b01110100). Also set the Output Data Rate + * to 833 Hz (high performance). */ + m_imuDrv.writeReg(LSM6DS33_ADDR, LSM6DS33_REG_CTRL2_G, 0x74); + + /* Enable only the x-axis of the LSM6DS33 accelerometer. 0x18 is the address of the CTRL9_XL Register. */ + m_imuDrv.writeReg(LSM6DS33_ADDR, 0x18, 0x8); + + /* Enable only the z-axis of the LSM6DS33 gyroscope. 0x19 is the address of the CTRL10_C Register. */ + m_imuDrv.writeReg(LSM6DS33_ADDR, 0x19, 0x20); + break; + default: + return; + } +} + +void IMU::readAccelerometer() +{ + m_imuDrv.readAcc(); + m_accelerometerValues.valueX = m_imuDrv.a.x - m_rawAccelerometerOffsetX; + m_accelerometerValues.valueY = 0; + m_accelerometerValues.valueZ = 0; +} + +void IMU::readGyro() +{ + m_imuDrv.readGyro(); + m_gyroValues.valueX = 0; + m_gyroValues.valueY = 0; + m_gyroValues.valueZ = m_imuDrv.g.z - m_rawGyroOffsetZ; +} + +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; + accelerationValues->valueY = m_accelerometerValues.valueY; + accelerationValues->valueZ = m_accelerometerValues.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() +{ + /* Wait for a short amount of time to make sure, the IMU is done setting up. */ + delay(100U); + + /* Define how many measurements should be made for calibration. */ + const int32_t NUMBER_OF_MEASUREMENTS = 50; + + /* Calibration takes place while the robot doesn't move. Therefore the Acceleration and turn values are near 0 and + * int16_t values are enough. */ + int32_t sumOfRawAccelValuesX = 0; + + int32_t sumOfRawGyroValuesZ = 0; + + int32_t measurementIndex = 0; + while (measurementIndex < NUMBER_OF_MEASUREMENTS) + { + if ((true == m_imuDrv.accDataReady()) && (true == m_imuDrv.gyroDataReady())) + { + m_imuDrv.readAcc(); + m_imuDrv.readGyro(); + sumOfRawGyroValuesZ += m_imuDrv.g.z; + sumOfRawAccelValuesX += m_imuDrv.a.x; + ++measurementIndex; + } + else + { + /* Do nothing and wati until new Sensor Data is available. */ + delay(20U); + } + } + + m_rawAccelerometerOffsetX = static_cast(sumOfRawAccelValuesX / NUMBER_OF_MEASUREMENTS); /* In digits */ + + m_rawGyroOffsetZ = static_cast(sumOfRawGyroValuesZ / NUMBER_OF_MEASUREMENTS); /* In digits */ +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALTarget/IMU.h b/lib/HALTarget/IMU.h index 92cf3b40..285936ea 100644 --- a/lib/HALTarget/IMU.h +++ b/lib/HALTarget/IMU.h @@ -1,200 +1,200 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief 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_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() final; - - /** - * Enables all of the inertial sensors with a default configuration. - */ - void enableDefault() final; - - /** - * Configures the sensors with settings optimized for turn sensing. - * - * Additionally, only the z-axis of the gyroscope and the x-axis of the accelerometer are enabled. - */ - void configureForTurnSensing() final; - - /** - * Takes a reading from the accelerometer and makes the measurements available in a. - * - * Only the Acceleration along the x-axis is read. The measurements along the other axes are disabled and set to 0. - */ - void readAccelerometer() final; - - /** - * Takes a reading from the gyro and makes the measurements available in g. - * - * Only the Turn Rate around the z-axis is read. The measurements along the other axes are disabled and set to 0. - */ - void readGyro() final; - - /** - * Takes a reading from the magnetometer and makes the measurements available in m. - */ - void readMagnetometer() final; - - /** - * Indicates whether the accelerometer has new measurement data ready. - * - * @return True if there is new accelerometer data available; false otherwise. - */ - bool accelerometerDataReady() final; - - /** - * Indicates whether the gyro has new measurement data ready. - * - * @return True if there is new gyro data available; false otherwise. - */ - bool gyroDataReady() final; - - /** - * Indicates whether the magnetometer has new measurement data ready. - * - * @return True if there is new magnetometer data available; false otherwise. - */ - bool magnetometerDataReady() final; - - /** - * Get last raw Accelerometer values as an IMUData struct containing values in x, y and z in digits. - * - * Note: Currently, only the Acceleration along the x-axis is read. The measurements along the other axes are - * disabled and set to 0. - * If you need to access the other measurements, you need to modify the method readAccelerometer(). - * - * @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) final; - - /** - * Get last raw Gyro values as an IMUData struct containing values in x, y and z in digits. - * - * Note: Currently, only the Turn Rate around the z-axis is read. The measurements along the other axes are disabled - * and set to 0. - * If you need to access the other measurements, you need to modify the method readGyro(). - * - * @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) final; - - /** - * 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) final; - - /** - * Calibrate the IMU. - * Only the used axes (acceleration along x and y and turn rate around z) are calibrated. - */ - void calibrate() final; - -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_rawGyroOffsetZ; /**< Mean raw gyro offset in z-direction determined by calibration. */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* IMU_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief 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_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() final; + + /** + * Enables all of the inertial sensors with a default configuration. + */ + void enableDefault() final; + + /** + * Configures the sensors with settings optimized for turn sensing. + * + * Additionally, only the z-axis of the gyroscope and the x-axis of the accelerometer are enabled. + */ + void configureForTurnSensing() final; + + /** + * Takes a reading from the accelerometer and makes the measurements available in a. + * + * Only the Acceleration along the x-axis is read. The measurements along the other axes are disabled and set to 0. + */ + void readAccelerometer() final; + + /** + * Takes a reading from the gyro and makes the measurements available in g. + * + * Only the Turn Rate around the z-axis is read. The measurements along the other axes are disabled and set to 0. + */ + void readGyro() final; + + /** + * Takes a reading from the magnetometer and makes the measurements available in m. + */ + void readMagnetometer() final; + + /** + * Indicates whether the accelerometer has new measurement data ready. + * + * @return True if there is new accelerometer data available; false otherwise. + */ + bool accelerometerDataReady() final; + + /** + * Indicates whether the gyro has new measurement data ready. + * + * @return True if there is new gyro data available; false otherwise. + */ + bool gyroDataReady() final; + + /** + * Indicates whether the magnetometer has new measurement data ready. + * + * @return True if there is new magnetometer data available; false otherwise. + */ + bool magnetometerDataReady() final; + + /** + * Get last raw Accelerometer values as an IMUData struct containing values in x, y and z in digits. + * + * Note: Currently, only the Acceleration along the x-axis is read. The measurements along the other axes are + * disabled and set to 0. + * If you need to access the other measurements, you need to modify the method readAccelerometer(). + * + * @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) final; + + /** + * Get last raw Gyro values as an IMUData struct containing values in x, y and z in digits. + * + * Note: Currently, only the Turn Rate around the z-axis is read. The measurements along the other axes are disabled + * and set to 0. + * If you need to access the other measurements, you need to modify the method readGyro(). + * + * @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) final; + + /** + * 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) final; + + /** + * Calibrate the IMU. + * Only the used axes (acceleration along x and y and turn rate around z) are calibrated. + */ + void calibrate() final; + +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_rawGyroOffsetZ; /**< Mean raw gyro offset in z-direction determined by calibration. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* IMU_H */ +/** @} */ diff --git a/lib/HALTarget/LedGreen.cpp b/lib/HALTarget/LedGreen.cpp index 98af3626..00d00a8d 100644 --- a/lib/HALTarget/LedGreen.cpp +++ b/lib/HALTarget/LedGreen.cpp @@ -1,75 +1,75 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Green LED realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "LedGreen.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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Green LED realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "LedGreen.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALTarget/LedGreen.h b/lib/HALTarget/LedGreen.h index c95df408..9e9fee34 100644 --- a/lib/HALTarget/LedGreen.h +++ b/lib/HALTarget/LedGreen.h @@ -1,93 +1,93 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Green LED realization - * @author Andreas Merkle - * - * @addtogroup HALTarget - * - * @{ - */ - -#ifndef LEDGREEN_H -#define LEDGREEN_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ILed.h" -#include "Zumo32U4.h" - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class provides access to the Zumo target green LED. */ -class LedGreen : public ILed -{ -public: - /** - * Constructs the green LED adapter. - */ - LedGreen() : ILed() - { - } - - /** - * Destroys the green LED adapter. - */ - ~LedGreen() - { - } - - /** - * Enables/Disables the LED. - * - * @param[in] enableIt Enable LED with true, disable it with false. - */ - void enable(bool enableIt) final - { - ledGreen(enableIt); - } - -private: -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* LEDGREEN_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Green LED realization + * @author Andreas Merkle + * + * @addtogroup HALTarget + * + * @{ + */ + +#ifndef LEDGREEN_H +#define LEDGREEN_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ILed.h" +#include "Zumo32U4.h" + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** This class provides access to the Zumo target green LED. */ +class LedGreen : public ILed +{ +public: + /** + * Constructs the green LED adapter. + */ + LedGreen() : ILed() + { + } + + /** + * Destroys the green LED adapter. + */ + ~LedGreen() + { + } + + /** + * Enables/Disables the LED. + * + * @param[in] enableIt Enable LED with true, disable it with false. + */ + void enable(bool enableIt) final + { + ledGreen(enableIt); + } + +private: +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* LEDGREEN_H */ +/** @} */ diff --git a/lib/HALTarget/LedRed.cpp b/lib/HALTarget/LedRed.cpp index 3c3b13da..4ea34bd8 100644 --- a/lib/HALTarget/LedRed.cpp +++ b/lib/HALTarget/LedRed.cpp @@ -1,75 +1,75 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Red LED realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "LedRed.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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Red LED realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "LedRed.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALTarget/LedRed.h b/lib/HALTarget/LedRed.h index 71e9f66b..3debb470 100644 --- a/lib/HALTarget/LedRed.h +++ b/lib/HALTarget/LedRed.h @@ -1,93 +1,93 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Red LED realization - * @author Andreas Merkle - * - * @addtogroup HALTarget - * - * @{ - */ - -#ifndef LEDRED_H -#define LEDRED_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ILed.h" -#include "Zumo32U4.h" - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class provides access to the Zumo target red LED. */ -class LedRed : public ILed -{ -public: - /** - * Constructs the red LED adapter. - */ - LedRed() : ILed() - { - } - - /** - * Destroys the red LED adapter. - */ - ~LedRed() - { - } - - /** - * Enables/Disables the LED. - * - * @param[in] enableIt Enable LED with true, disable it with false. - */ - void enable(bool enableIt) final - { - ledRed(enableIt); - } - -private: -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* LEDRED_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Red LED realization + * @author Andreas Merkle + * + * @addtogroup HALTarget + * + * @{ + */ + +#ifndef LEDRED_H +#define LEDRED_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ILed.h" +#include "Zumo32U4.h" + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** This class provides access to the Zumo target red LED. */ +class LedRed : public ILed +{ +public: + /** + * Constructs the red LED adapter. + */ + LedRed() : ILed() + { + } + + /** + * Destroys the red LED adapter. + */ + ~LedRed() + { + } + + /** + * Enables/Disables the LED. + * + * @param[in] enableIt Enable LED with true, disable it with false. + */ + void enable(bool enableIt) final + { + ledRed(enableIt); + } + +private: +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* LEDRED_H */ +/** @} */ diff --git a/lib/HALTarget/LedYellow.cpp b/lib/HALTarget/LedYellow.cpp index e7c7ced2..daac91da 100644 --- a/lib/HALTarget/LedYellow.cpp +++ b/lib/HALTarget/LedYellow.cpp @@ -1,75 +1,75 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Yellow LED realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "LedYellow.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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Yellow LED realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "LedYellow.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALTarget/LedYellow.h b/lib/HALTarget/LedYellow.h index 3bd0cb84..d144f1c2 100644 --- a/lib/HALTarget/LedYellow.h +++ b/lib/HALTarget/LedYellow.h @@ -1,93 +1,93 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Yellow LED realization - * @author Andreas Merkle - * - * @addtogroup HALTarget - * - * @{ - */ - -#ifndef LEDYELLOW_H -#define LEDYELLOW_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ILed.h" -#include "Zumo32U4.h" - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class provides access to the Zumo target yellow LED. */ -class LedYellow : public ILed -{ -public: - /** - * Constructs the yellow LED adapter. - */ - LedYellow() : ILed() - { - } - - /** - * Destroys the yellow LED adapter. - */ - ~LedYellow() - { - } - - /** - * Enables/Disables the LED. - * - * @param[in] enableIt Enable LED with true, disable it with false. - */ - void enable(bool enableIt) final - { - ledYellow(enableIt); - } - -private: -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* LEDYELLOW_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Yellow LED realization + * @author Andreas Merkle + * + * @addtogroup HALTarget + * + * @{ + */ + +#ifndef LEDYELLOW_H +#define LEDYELLOW_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ILed.h" +#include "Zumo32U4.h" + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** This class provides access to the Zumo target yellow LED. */ +class LedYellow : public ILed +{ +public: + /** + * Constructs the yellow LED adapter. + */ + LedYellow() : ILed() + { + } + + /** + * Destroys the yellow LED adapter. + */ + ~LedYellow() + { + } + + /** + * Enables/Disables the LED. + * + * @param[in] enableIt Enable LED with true, disable it with false. + */ + void enable(bool enableIt) final + { + ledYellow(enableIt); + } + +private: +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* LEDYELLOW_H */ +/** @} */ diff --git a/lib/HALTarget/LineSensors.cpp b/lib/HALTarget/LineSensors.cpp index 5b1ed1fd..1d5f213c 100644 --- a/lib/HALTarget/LineSensors.cpp +++ b/lib/HALTarget/LineSensors.cpp @@ -1,118 +1,118 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Line sensors array realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "LineSensors.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -bool LineSensors::isCalibrationSuccessful() -{ - bool isSuccessful = false; - - m_calibErrorInfo = CALIB_ERROR_NOT_CALIBRATED; - - if ((nullptr != m_lineSensors.calibratedMinimumOn) && - (nullptr != m_lineSensors.calibratedMaximumOn) && - (nullptr != m_lineSensors.calibratedMinimumOff) && - (nullptr != m_lineSensors.calibratedMaximumOff)) - { - uint8_t index = 0; - - isSuccessful = true; - while((MAX_SENSORS > index) && (true == isSuccessful)) - { - uint16_t distance = 0; - - /* Check whether the max. value is really greater than the min. value. - * It can happen that someone try to calibrate over a blank surface. - */ - if (m_lineSensors.calibratedMaximumOn[index] > m_lineSensors.calibratedMinimumOn[index]) - { - distance = m_lineSensors.calibratedMaximumOn[index] - m_lineSensors.calibratedMinimumOn[index]; - } - - /* The assumption here is, that the distance (max. value - min. value) must be - * higher than a sixth of the max. measure duration. - * Measured example values from left to right when on the line: 1092 / 436 / 384 / 596 / 996 - */ - if ((MEASURE_DURATION / 6) > distance) - { - m_calibErrorInfo = index; - isSuccessful = false; - } - - ++index; - } - } - - return isSuccessful; -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Line sensors array realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "LineSensors.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +bool LineSensors::isCalibrationSuccessful() +{ + bool isSuccessful = false; + + m_calibErrorInfo = CALIB_ERROR_NOT_CALIBRATED; + + if ((nullptr != m_lineSensors.calibratedMinimumOn) && + (nullptr != m_lineSensors.calibratedMaximumOn) && + (nullptr != m_lineSensors.calibratedMinimumOff) && + (nullptr != m_lineSensors.calibratedMaximumOff)) + { + uint8_t index = 0; + + isSuccessful = true; + while((MAX_SENSORS > index) && (true == isSuccessful)) + { + uint16_t distance = 0; + + /* Check whether the max. value is really greater than the min. value. + * It can happen that someone try to calibrate over a blank surface. + */ + if (m_lineSensors.calibratedMaximumOn[index] > m_lineSensors.calibratedMinimumOn[index]) + { + distance = m_lineSensors.calibratedMaximumOn[index] - m_lineSensors.calibratedMinimumOn[index]; + } + + /* The assumption here is, that the distance (max. value - min. value) must be + * higher than a sixth of the max. measure duration. + * Measured example values from left to right when on the line: 1092 / 436 / 384 / 596 / 996 + */ + if ((MEASURE_DURATION / 6) > distance) + { + m_calibErrorInfo = index; + isSuccessful = false; + } + + ++index; + } + } + + return isSuccessful; +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALTarget/LineSensors.h b/lib/HALTarget/LineSensors.h index 0610148d..f1afc759 100644 --- a/lib/HALTarget/LineSensors.h +++ b/lib/HALTarget/LineSensors.h @@ -1,226 +1,226 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Line sensors array realization - * @author Andreas Merkle - * - * @addtogroup HALTarget - * - * @{ - */ - -#ifndef LINESENSORS_H -#define LINESENSORS_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ILineSensors.h" -#include "Zumo32U4.h" - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class provides access to the Zumo target line sensors. */ -class LineSensors : public ILineSensors -{ -public: - /** - * Constructs the line sensors adapter. - */ - LineSensors() : ILineSensors(), m_sensorValues(), m_sensorValuesU16(), m_calibErrorInfo(CALIB_ERROR_NOT_CALIBRATED) - { - } - - /** - * Destroys the line sensors adapter. - */ - ~LineSensors() - { - } - - /** - * Initializes the line sensors. - */ - void init() final - { - m_lineSensors.initFiveSensors(); - } - - /** - * Reads the sensors for calibration. Call this method several times during - * turning the sensors over the line to determine the minimum and maximum - * values. - * - * The calibration factors are stored internally. - */ - void calibrate() final - { - /* The environment brightness compensation is enabled and must be considered - * during calibration as well. - * See LineSensors::readLine() - */ - m_lineSensors.calibrate(QTR_EMITTERS_ON_AND_OFF); - } - - /** - * Determines the deviation and returns an estimated position of the robot - * with respect to a line. The estimate is made using a weighted average of - * the sensor indices multiplied by 1000, so that a return value of 0 - * indicates that the line is directly below sensor 0, a return value of - * 1000 indicates that the line is directly below sensor 1, 2000 - * indicates that it's below sensor 2000, etc. Intermediate values - * indicate that the line is between two sensors. The formula is: - * - * 0*value0 + 1000*value1 + 2000*value2 + ... - * -------------------------------------------- - * value0 + value1 + value2 + ... - * - * This function assumes a dark line (high values) surrounded by white - * (low values). - * - * @return Estimated position with respect to track. - */ - int16_t readLine() final - { - /* The environment brightness compensation is enabled by using QTR_EMITTERS_ON_AND_OFF. - * This is done by measuring the sensor on values with enabled IR emitters. After that - * the IR emitters are disabled and the sensor off values are measured. - * The compensation is quite simple: compenstated value = on value - off value - */ - return m_lineSensors.readLine(m_sensorValues, QTR_EMITTERS_ON_AND_OFF); - } - - /** - * Get last line sensor values. - * - * @return Line sensor values - */ - const uint16_t* getSensorValues() final - { - uint8_t index = 0; - - /* This is only done to be able to provide a platform independed data type. */ - for (index = 0; index < MAX_SENSORS; ++index) - { - m_sensorValuesU16[index] = static_cast(m_sensorValues[index]); - } - - return m_sensorValuesU16; - } - - /** - * Checks whether the calibration was successful or not. - * It assumes that the environment brightness compensation is active. - * - * @return If successful, it will return true otherwise false. - */ - bool isCalibrationSuccessful() final; - - /** - * It will return the index of the sensor, which caused to fail the calibration. - * If calibration was successful, it will return 0xFF. - * If calibration was not not done yet, it will return 0xFE. - * - * @return Sensor index, starting with 0. Note the other cases in description. - */ - uint8_t getCalibErrorInfo() const final - { - return m_calibErrorInfo; - } - - /** - * Get number of used line sensors. - * - * @return Number of used line sensors - */ - uint8_t getNumLineSensors() const final - { - return MAX_SENSORS; - } - - /** - * Get max. value of a single line sensor in digits. - * The sensor value is indirect proportional to the reflectance. - * - * @return Max. line sensor value - */ - int16_t getSensorValueMax() const final - { - return SENSOR_MAX_VALUE; - } - - /** - * Resets the maximum and minimum values measured by each sensor. - */ - void resetCalibration() final - { - m_lineSensors.resetCalibration(); - } - -private: - /** - * Number of used line sensors. This depends on the Zumo hardware configuration. - */ - static const uint8_t MAX_SENSORS = 5; - - /** - * Max. value of a single line sensor in digits (calibration already considered). - * It depends on the Zumo32U4LineSensors implementation. - * See Zumo32U4\QTRSensors.cpp @ readCalibrated() - */ - static const int16_t SENSOR_MAX_VALUE = 1000; - - /** - * Max. value for a single line sensor in us. In other words, it is the max. measure duration. - * Note, the measured time is indirect proportional to the measured reflectance. - * That means a black line is detected in case of the measured value is around this max. value. - * For the value see Zumo32U4\Zumo32U4LineSensors.h @ initFiveSensors() - */ - static const uint16_t MEASURE_DURATION = 2000; - - Zumo32U4LineSensors m_lineSensors; /**< Zumo line sensors driver from Pololu */ - unsigned int m_sensorValues[MAX_SENSORS]; /**< The last value of each sensor. */ - uint16_t m_sensorValuesU16[MAX_SENSORS]; /**< The last value of each sensor as unsigned 16-bit values. */ - uint8_t m_calibErrorInfo; /**< Calibration error information. */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* LINESENSORS_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Line sensors array realization + * @author Andreas Merkle + * + * @addtogroup HALTarget + * + * @{ + */ + +#ifndef LINESENSORS_H +#define LINESENSORS_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ILineSensors.h" +#include "Zumo32U4.h" + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** This class provides access to the Zumo target line sensors. */ +class LineSensors : public ILineSensors +{ +public: + /** + * Constructs the line sensors adapter. + */ + LineSensors() : ILineSensors(), m_sensorValues(), m_sensorValuesU16(), m_calibErrorInfo(CALIB_ERROR_NOT_CALIBRATED) + { + } + + /** + * Destroys the line sensors adapter. + */ + ~LineSensors() + { + } + + /** + * Initializes the line sensors. + */ + void init() final + { + m_lineSensors.initFiveSensors(); + } + + /** + * Reads the sensors for calibration. Call this method several times during + * turning the sensors over the line to determine the minimum and maximum + * values. + * + * The calibration factors are stored internally. + */ + void calibrate() final + { + /* The environment brightness compensation is enabled and must be considered + * during calibration as well. + * See LineSensors::readLine() + */ + m_lineSensors.calibrate(QTR_EMITTERS_ON_AND_OFF); + } + + /** + * Determines the deviation and returns an estimated position of the robot + * with respect to a line. The estimate is made using a weighted average of + * the sensor indices multiplied by 1000, so that a return value of 0 + * indicates that the line is directly below sensor 0, a return value of + * 1000 indicates that the line is directly below sensor 1, 2000 + * indicates that it's below sensor 2000, etc. Intermediate values + * indicate that the line is between two sensors. The formula is: + * + * 0*value0 + 1000*value1 + 2000*value2 + ... + * -------------------------------------------- + * value0 + value1 + value2 + ... + * + * This function assumes a dark line (high values) surrounded by white + * (low values). + * + * @return Estimated position with respect to track. + */ + int16_t readLine() final + { + /* The environment brightness compensation is enabled by using QTR_EMITTERS_ON_AND_OFF. + * This is done by measuring the sensor on values with enabled IR emitters. After that + * the IR emitters are disabled and the sensor off values are measured. + * The compensation is quite simple: compenstated value = on value - off value + */ + return m_lineSensors.readLine(m_sensorValues, QTR_EMITTERS_ON_AND_OFF); + } + + /** + * Get last line sensor values. + * + * @return Line sensor values + */ + const uint16_t* getSensorValues() final + { + uint8_t index = 0; + + /* This is only done to be able to provide a platform independed data type. */ + for (index = 0; index < MAX_SENSORS; ++index) + { + m_sensorValuesU16[index] = static_cast(m_sensorValues[index]); + } + + return m_sensorValuesU16; + } + + /** + * Checks whether the calibration was successful or not. + * It assumes that the environment brightness compensation is active. + * + * @return If successful, it will return true otherwise false. + */ + bool isCalibrationSuccessful() final; + + /** + * It will return the index of the sensor, which caused to fail the calibration. + * If calibration was successful, it will return 0xFF. + * If calibration was not not done yet, it will return 0xFE. + * + * @return Sensor index, starting with 0. Note the other cases in description. + */ + uint8_t getCalibErrorInfo() const final + { + return m_calibErrorInfo; + } + + /** + * Get number of used line sensors. + * + * @return Number of used line sensors + */ + uint8_t getNumLineSensors() const final + { + return MAX_SENSORS; + } + + /** + * Get max. value of a single line sensor in digits. + * The sensor value is indirect proportional to the reflectance. + * + * @return Max. line sensor value + */ + int16_t getSensorValueMax() const final + { + return SENSOR_MAX_VALUE; + } + + /** + * Resets the maximum and minimum values measured by each sensor. + */ + void resetCalibration() final + { + m_lineSensors.resetCalibration(); + } + +private: + /** + * Number of used line sensors. This depends on the Zumo hardware configuration. + */ + static const uint8_t MAX_SENSORS = 5; + + /** + * Max. value of a single line sensor in digits (calibration already considered). + * It depends on the Zumo32U4LineSensors implementation. + * See Zumo32U4\QTRSensors.cpp @ readCalibrated() + */ + static const int16_t SENSOR_MAX_VALUE = 1000; + + /** + * Max. value for a single line sensor in us. In other words, it is the max. measure duration. + * Note, the measured time is indirect proportional to the measured reflectance. + * That means a black line is detected in case of the measured value is around this max. value. + * For the value see Zumo32U4\Zumo32U4LineSensors.h @ initFiveSensors() + */ + static const uint16_t MEASURE_DURATION = 2000; + + Zumo32U4LineSensors m_lineSensors; /**< Zumo line sensors driver from Pololu */ + unsigned int m_sensorValues[MAX_SENSORS]; /**< The last value of each sensor. */ + uint16_t m_sensorValuesU16[MAX_SENSORS]; /**< The last value of each sensor as unsigned 16-bit values. */ + uint8_t m_calibErrorInfo; /**< Calibration error information. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* LINESENSORS_H */ +/** @} */ diff --git a/lib/HALTarget/Motors.cpp b/lib/HALTarget/Motors.cpp index 12d26064..8ff04e38 100644 --- a/lib/HALTarget/Motors.cpp +++ b/lib/HALTarget/Motors.cpp @@ -1,75 +1,75 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Motors realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "LineSensors.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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Motors realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "LineSensors.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALTarget/Motors.h b/lib/HALTarget/Motors.h index dec7a267..952ef71c 100644 --- a/lib/HALTarget/Motors.h +++ b/lib/HALTarget/Motors.h @@ -1,147 +1,147 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Motors realization - * @author Andreas Merkle - * - * @addtogroup HALTarget - * - * @{ - */ - -#ifndef MOTORS_H -#define MOTORS_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "IMotors.h" -#include "Zumo32U4.h" - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class provides access to the Zumo target motors. */ -class Motors : public IMotors -{ -public: - /** - * Constructs the motors adapter. - */ - Motors() : IMotors(), m_motors(), m_leftSpeed(0), m_rightSpeed(0) - { - } - - /** - * Destroys the motors adapter. - */ - ~Motors() - { - } - - /** - * Initializes the motors. - */ - void init() final - { - /* Nothing to do. */ - } - - /** - * Sets the speeds for both motors. - * - * @param[in] leftSpeed A number from -400 to 400 representing the speed and - * direction of the right motor. Values of -400 or less result in full speed - * reverse, and values of 400 or more result in full speed forward. - * @param[in] rightSpeed A number from -400 to 400 representing the speed and - * direction of the right motor. Values of -400 or less result in full speed - * reverse, and values of 400 or more result in full speed forward. - */ - void setSpeeds(int16_t leftSpeed, int16_t rightSpeed) final - { - m_motors.setSpeeds(leftSpeed, rightSpeed); - - m_leftSpeed = leftSpeed; - m_rightSpeed = rightSpeed; - } - - /** - * Get maximum speed of the motors in digits. - * - * @return Max. speed in digits - */ - int16_t getMaxSpeed() const final - { - return MAX_SPEED; - } - - /** - * Get the current speed of the left motor. - * - * @return The left motor speed in digits. - */ - int16_t getLeftSpeed() final - { - return m_leftSpeed; - } - - /** - * Get the current speed of the right motor. - * - * @return The right motor speed in digits. - */ - int16_t getRightSpeed() final - { - return m_rightSpeed; - } - -private: - /** - * The maximum speed of a single motor in PWM digits. - */ - static const int16_t MAX_SPEED = 400; - - Zumo32U4Motors m_motors; /**< Zumo motors driver from Pololu */ - int16_t m_leftSpeed; /**< Left motor speed in PWM digits, used by the application. */ - int16_t m_rightSpeed; /**< Right motor speed in PWM digits, used by the application. */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* MOTORS_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Motors realization + * @author Andreas Merkle + * + * @addtogroup HALTarget + * + * @{ + */ + +#ifndef MOTORS_H +#define MOTORS_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "IMotors.h" +#include "Zumo32U4.h" + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** This class provides access to the Zumo target motors. */ +class Motors : public IMotors +{ +public: + /** + * Constructs the motors adapter. + */ + Motors() : IMotors(), m_motors(), m_leftSpeed(0), m_rightSpeed(0) + { + } + + /** + * Destroys the motors adapter. + */ + ~Motors() + { + } + + /** + * Initializes the motors. + */ + void init() final + { + /* Nothing to do. */ + } + + /** + * Sets the speeds for both motors. + * + * @param[in] leftSpeed A number from -400 to 400 representing the speed and + * direction of the right motor. Values of -400 or less result in full speed + * reverse, and values of 400 or more result in full speed forward. + * @param[in] rightSpeed A number from -400 to 400 representing the speed and + * direction of the right motor. Values of -400 or less result in full speed + * reverse, and values of 400 or more result in full speed forward. + */ + void setSpeeds(int16_t leftSpeed, int16_t rightSpeed) final + { + m_motors.setSpeeds(leftSpeed, rightSpeed); + + m_leftSpeed = leftSpeed; + m_rightSpeed = rightSpeed; + } + + /** + * Get maximum speed of the motors in digits. + * + * @return Max. speed in digits + */ + int16_t getMaxSpeed() const final + { + return MAX_SPEED; + } + + /** + * Get the current speed of the left motor. + * + * @return The left motor speed in digits. + */ + int16_t getLeftSpeed() final + { + return m_leftSpeed; + } + + /** + * Get the current speed of the right motor. + * + * @return The right motor speed in digits. + */ + int16_t getRightSpeed() final + { + return m_rightSpeed; + } + +private: + /** + * The maximum speed of a single motor in PWM digits. + */ + static const int16_t MAX_SPEED = 400; + + Zumo32U4Motors m_motors; /**< Zumo motors driver from Pololu */ + int16_t m_leftSpeed; /**< Left motor speed in PWM digits, used by the application. */ + int16_t m_rightSpeed; /**< Right motor speed in PWM digits, used by the application. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* MOTORS_H */ +/** @} */ diff --git a/lib/HALTarget/ProximitySensors.cpp b/lib/HALTarget/ProximitySensors.cpp index f4a7c1a2..2f39262c 100644 --- a/lib/HALTarget/ProximitySensors.cpp +++ b/lib/HALTarget/ProximitySensors.cpp @@ -1,75 +1,75 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Proximity sensors realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ProximitySensors.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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Proximity sensors realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ProximitySensors.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALTarget/ProximitySensors.h b/lib/HALTarget/ProximitySensors.h index 37548936..806ce7d5 100644 --- a/lib/HALTarget/ProximitySensors.h +++ b/lib/HALTarget/ProximitySensors.h @@ -1,143 +1,143 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Proximity sensors realization - * @author Andreas Merkle - * - * @addtogroup HALInterfaces - * - * @{ - */ -#ifndef PROXIMITYSENSORS_H -#define PROXIMITYSENSORS_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The proximity sensors realization for the target. */ -class ProximitySensors : public IProximitySensors -{ -public: - /** - * Constructs the interface. - */ - ProximitySensors() : IProximitySensors(), m_proximitySensors() - { - } - - /** - * Destroys the interface. - */ - virtual ~ProximitySensors() - { - } - - /** - * Initialize only the front proximity sensor. - */ - void initFrontSensor() final - { - m_proximitySensors.initFrontSensor(); - } - - /** - * Returns the number of sensors. - * - * @return Number of sensors - */ - uint8_t getNumSensors() const final - { - return m_proximitySensors.getNumSensors(); - } - - /** - * Emits IR pulses and gets readings from the sensors. - */ - void read() final - { - m_proximitySensors.read(); - } - - /** - * Returns the number of brightness levels for the left LEDs that - * activated the front proximity sensor. - * - * @return Number of brightness levels - */ - uint8_t countsFrontWithLeftLeds() const final - { - return m_proximitySensors.countsFrontWithLeftLeds(); - } - - /** - * Returns the number of brightness levels for the right LEDs that - * activated the front proximity sensor. - * - * @return Number of brightness levels - */ - uint8_t countsFrontWithRightLeds() const final - { - return m_proximitySensors.countsFrontWithRightLeds(); - } - - /** - * Returns the number of brightness levels. - * - * @return Number of brightness levels. - */ - uint8_t getNumBrightnessLevels() const final - { - return m_proximitySensors.getNumBrightnessLevels(); - } - -protected: -private: - Zumo32U4ProximitySensors m_proximitySensors; /**< Zumo32U4 proximity sensors */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* PROXIMITYSENSORS_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Proximity sensors realization + * @author Andreas Merkle + * + * @addtogroup HALInterfaces + * + * @{ + */ +#ifndef PROXIMITYSENSORS_H +#define PROXIMITYSENSORS_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The proximity sensors realization for the target. */ +class ProximitySensors : public IProximitySensors +{ +public: + /** + * Constructs the interface. + */ + ProximitySensors() : IProximitySensors(), m_proximitySensors() + { + } + + /** + * Destroys the interface. + */ + virtual ~ProximitySensors() + { + } + + /** + * Initialize only the front proximity sensor. + */ + void initFrontSensor() final + { + m_proximitySensors.initFrontSensor(); + } + + /** + * Returns the number of sensors. + * + * @return Number of sensors + */ + uint8_t getNumSensors() const final + { + return m_proximitySensors.getNumSensors(); + } + + /** + * Emits IR pulses and gets readings from the sensors. + */ + void read() final + { + m_proximitySensors.read(); + } + + /** + * Returns the number of brightness levels for the left LEDs that + * activated the front proximity sensor. + * + * @return Number of brightness levels + */ + uint8_t countsFrontWithLeftLeds() const final + { + return m_proximitySensors.countsFrontWithLeftLeds(); + } + + /** + * Returns the number of brightness levels for the right LEDs that + * activated the front proximity sensor. + * + * @return Number of brightness levels + */ + uint8_t countsFrontWithRightLeds() const final + { + return m_proximitySensors.countsFrontWithRightLeds(); + } + + /** + * Returns the number of brightness levels. + * + * @return Number of brightness levels. + */ + uint8_t getNumBrightnessLevels() const final + { + return m_proximitySensors.getNumBrightnessLevels(); + } + +protected: +private: + Zumo32U4ProximitySensors m_proximitySensors; /**< Zumo32U4 proximity sensors */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* PROXIMITYSENSORS_H */ +/** @} */ diff --git a/lib/HALTarget/RobotConstants.h b/lib/HALTarget/RobotConstants.h index a709b2de..90ffb4c9 100644 --- a/lib/HALTarget/RobotConstants.h +++ b/lib/HALTarget/RobotConstants.h @@ -1,102 +1,102 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Robot specific constants - * @author Andreas Merkle - * - * @addtogroup HALInterfaces - * - * @{ - */ - -#ifndef ROBOTCONSTANTS_H -#define ROBOTCONSTANTS_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** - * Abstracts the physical robot constants. - */ -namespace RobotConstants -{ - /** - * Gear ratio multiplied with 1000. - */ - static const uint32_t GEAR_RATIO = 75810; - - /** - * Encoder resolution in counts per revolution of the motor shaft. - */ - static const uint32_t ENCODER_RESOLUTION = 12; - - /** - * Calibrated wheel diameter in mm. - * This means the real wheel diameter was adapted after calibration drive. - */ - static const uint32_t WHEEL_DIAMETER = 36; - - /** - * Wheel circumference in um. - */ - static const uint32_t WHEEL_CIRCUMFERENCE = - static_cast(static_cast(WHEEL_DIAMETER) * PI * 1000.0f); - - /** - * Wheel base in mm. - * Distance between the left wheel center to the right wheel center. - */ - static const uint32_t WHEEL_BASE = 85; - - /** - * Number of encoder steps per m. - */ - static const uint32_t ENCODER_STEPS_PER_M = (ENCODER_RESOLUTION * GEAR_RATIO * 1000U) / WHEEL_CIRCUMFERENCE; - -}; /* namespace RobotConstants */ - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* ROBOTCONSTANTS_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Robot specific constants + * @author Andreas Merkle + * + * @addtogroup HALInterfaces + * + * @{ + */ + +#ifndef ROBOTCONSTANTS_H +#define ROBOTCONSTANTS_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** + * Abstracts the physical robot constants. + */ +namespace RobotConstants +{ + /** + * Gear ratio multiplied with 1000. + */ + static const uint32_t GEAR_RATIO = 75810; + + /** + * Encoder resolution in counts per revolution of the motor shaft. + */ + static const uint32_t ENCODER_RESOLUTION = 12; + + /** + * Calibrated wheel diameter in mm. + * This means the real wheel diameter was adapted after calibration drive. + */ + static const uint32_t WHEEL_DIAMETER = 36; + + /** + * Wheel circumference in um. + */ + static const uint32_t WHEEL_CIRCUMFERENCE = + static_cast(static_cast(WHEEL_DIAMETER) * PI * 1000.0f); + + /** + * Wheel base in mm. + * Distance between the left wheel center to the right wheel center. + */ + static const uint32_t WHEEL_BASE = 85; + + /** + * Number of encoder steps per m. + */ + static const uint32_t ENCODER_STEPS_PER_M = (ENCODER_RESOLUTION * GEAR_RATIO * 1000U) / WHEEL_CIRCUMFERENCE; + +}; /* namespace RobotConstants */ + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* ROBOTCONSTANTS_H */ +/** @} */ diff --git a/lib/HALTarget/Sender.cpp b/lib/HALTarget/Sender.cpp index 5f403011..c49c338e 100644 --- a/lib/HALTarget/Sender.cpp +++ b/lib/HALTarget/Sender.cpp @@ -1,75 +1,75 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Sender realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "Sender.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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Sender realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "Sender.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALTarget/Sender.h b/lib/HALTarget/Sender.h index 7e3c9028..732ef697 100644 --- a/lib/HALTarget/Sender.h +++ b/lib/HALTarget/Sender.h @@ -1,112 +1,112 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Sender realization - * @author Andreas Merkle - * - * @addtogroup HALTarget - * - * @{ - */ - -#ifndef SENDER_H -#define SENDER_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ISender.h" - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class simulates a sender, because its only relevant for the simulation. */ -class Sender : public ISender -{ -public: - /** - * Constructs the sender adapter. - */ - Sender() : ISender() - { - } - - /** - * Destroys the sender adapter. - */ - ~Sender() - { - } - - /** - * Set channel which to send data to. - * - * @param[in] channel The channel which to use. - */ - void setChannel(int32_t channel) final - { - (void)channel; - } - - /** - * Sends data to the configured channel. - * - * @param[in] data Data buffer - * @param[in] size Data buffer size in bytes. - */ - void send(const void* data, size_t size) const final - { - (void)data; - (void)size; - } - - /** - * Sends string to the configured channel. - * - * @param[in] str String which to send. - */ - void send(const char* str) const final - { - (void)str; - } -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* SENDER_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Sender realization + * @author Andreas Merkle + * + * @addtogroup HALTarget + * + * @{ + */ + +#ifndef SENDER_H +#define SENDER_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ISender.h" + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** This class simulates a sender, because its only relevant for the simulation. */ +class Sender : public ISender +{ +public: + /** + * Constructs the sender adapter. + */ + Sender() : ISender() + { + } + + /** + * Destroys the sender adapter. + */ + ~Sender() + { + } + + /** + * Set channel which to send data to. + * + * @param[in] channel The channel which to use. + */ + void setChannel(int32_t channel) final + { + (void)channel; + } + + /** + * Sends data to the configured channel. + * + * @param[in] data Data buffer + * @param[in] size Data buffer size in bytes. + */ + void send(const void* data, size_t size) const final + { + (void)data; + (void)size; + } + + /** + * Sends string to the configured channel. + * + * @param[in] str String which to send. + */ + void send(const char* str) const final + { + (void)str; + } +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* SENDER_H */ +/** @} */ diff --git a/lib/HALTarget/Settings.cpp b/lib/HALTarget/Settings.cpp index 12625447..b4b6cf09 100644 --- a/lib/HALTarget/Settings.cpp +++ b/lib/HALTarget/Settings.cpp @@ -1,170 +1,170 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Settings realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "Settings.h" -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/** - * The magic pattern which is used to determine whether the EEPROM - * is initialized or not. - */ -static const uint32_t MAGIC_PATTERN = 0xC0FFEE; - -/** - * Data version is used to detect whether the data in the EEPROM is - * compatible with the current settings version. - * - * Increase the version number by 1 for every change! - */ -static const uint8_t DATA_VERSION = 1U; - -/** - * Max. speed default values in steps/s. - */ -static const int16_t DEFAULT_MAX_SPEED = 0; - -/* ---------- Attention! ---------- - * Keep the following order of variables in the EEPROM. - * Add new values at the tail. - * Increase the data version (DATA_VERSION) for every change! - */ - -/** - * Magic pattern in EEPROM. - */ -static uint32_t EEMEM gMagicPattern = MAGIC_PATTERN; - -/** - * Data version in EEPROM. - */ -static uint8_t EEMEM gDataVersion = DATA_VERSION; - -/** - * Max. speed in EEPROM. - */ -static int16_t EEMEM gMaxSpeed = DEFAULT_MAX_SPEED; - -/* ---------- Tail of EEPROM data. ---------- */ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void Settings::init() -{ - uint32_t magicPattern = getMagicPattern(); - uint8_t dataVersion = getDataVersion(); - - if ((MAGIC_PATTERN != magicPattern) || (DATA_VERSION != dataVersion)) - { - /* Write default values. */ - setMaxSpeed(DEFAULT_MAX_SPEED); - - /* Mark data in EEPROM as valid. */ - setMagicPattern(MAGIC_PATTERN); - setDataVersion(DATA_VERSION); - } -} - -int16_t Settings::getMaxSpeed() const -{ - void* vAddr = &gMaxSpeed; /* Avoid reinterpret_cast<>() */ - uint16_t* addr = static_cast(vAddr); - - return static_cast(eeprom_read_word(addr)); -} - -void Settings::setMaxSpeed(int16_t maxSpeed) -{ - void* vAddr = &gMaxSpeed; /* Avoid reinterpret_cast<>() */ - uint16_t* addr = static_cast(vAddr); - - eeprom_update_word(addr, static_cast(maxSpeed)); -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -uint32_t Settings::getMagicPattern() const -{ - return eeprom_read_dword(&gMagicPattern); -} - -void Settings::setMagicPattern(uint32_t value) const -{ - eeprom_update_dword(&gMagicPattern, value); -} - -uint8_t Settings::getDataVersion() const -{ - return eeprom_read_byte(&gDataVersion); -} - -void Settings::setDataVersion(uint8_t value) const -{ - eeprom_update_byte(&gDataVersion, value); -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Settings realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "Settings.h" +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/** + * The magic pattern which is used to determine whether the EEPROM + * is initialized or not. + */ +static const uint32_t MAGIC_PATTERN = 0xC0FFEE; + +/** + * Data version is used to detect whether the data in the EEPROM is + * compatible with the current settings version. + * + * Increase the version number by 1 for every change! + */ +static const uint8_t DATA_VERSION = 1U; + +/** + * Max. speed default values in steps/s. + */ +static const int16_t DEFAULT_MAX_SPEED = 0; + +/* ---------- Attention! ---------- + * Keep the following order of variables in the EEPROM. + * Add new values at the tail. + * Increase the data version (DATA_VERSION) for every change! + */ + +/** + * Magic pattern in EEPROM. + */ +static uint32_t EEMEM gMagicPattern = MAGIC_PATTERN; + +/** + * Data version in EEPROM. + */ +static uint8_t EEMEM gDataVersion = DATA_VERSION; + +/** + * Max. speed in EEPROM. + */ +static int16_t EEMEM gMaxSpeed = DEFAULT_MAX_SPEED; + +/* ---------- Tail of EEPROM data. ---------- */ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void Settings::init() +{ + uint32_t magicPattern = getMagicPattern(); + uint8_t dataVersion = getDataVersion(); + + if ((MAGIC_PATTERN != magicPattern) || (DATA_VERSION != dataVersion)) + { + /* Write default values. */ + setMaxSpeed(DEFAULT_MAX_SPEED); + + /* Mark data in EEPROM as valid. */ + setMagicPattern(MAGIC_PATTERN); + setDataVersion(DATA_VERSION); + } +} + +int16_t Settings::getMaxSpeed() const +{ + void* vAddr = &gMaxSpeed; /* Avoid reinterpret_cast<>() */ + uint16_t* addr = static_cast(vAddr); + + return static_cast(eeprom_read_word(addr)); +} + +void Settings::setMaxSpeed(int16_t maxSpeed) +{ + void* vAddr = &gMaxSpeed; /* Avoid reinterpret_cast<>() */ + uint16_t* addr = static_cast(vAddr); + + eeprom_update_word(addr, static_cast(maxSpeed)); +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +uint32_t Settings::getMagicPattern() const +{ + return eeprom_read_dword(&gMagicPattern); +} + +void Settings::setMagicPattern(uint32_t value) const +{ + eeprom_update_dword(&gMagicPattern, value); +} + +uint8_t Settings::getDataVersion() const +{ + return eeprom_read_byte(&gDataVersion); +} + +void Settings::setDataVersion(uint8_t value) const +{ + eeprom_update_byte(&gDataVersion, value); +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALTarget/Settings.h b/lib/HALTarget/Settings.h index 8d95f1ca..40deb79e 100644 --- a/lib/HALTarget/Settings.h +++ b/lib/HALTarget/Settings.h @@ -1,132 +1,132 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Settings realization - * @author Andreas Merkle - * - * @addtogroup HALTarget - * - * @{ - */ - -#ifndef SETTINGS_H -#define SETTINGS_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ISettings.h" - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class handles settings stored in the EEPROM. */ -class Settings : public ISettings -{ -public: - /** - * Constructs the settings adapter. - */ - Settings() : ISettings() - { - } - - /** - * Destroys the button A adapter. - */ - ~Settings() - { - } - - /** - * Initialize the settings. - * - * If the settings are invalid or not compatible to the settings, the - * default values will be written! - */ - void init() final; - - /** - * Get the max. speed. - * - * @return Max. speed in steps/s. - */ - int16_t getMaxSpeed() const final; - - /** - * Set the max. speed. - * - * @param[in] maxSpeed Max. speed in steps/s. - */ - void setMaxSpeed(int16_t maxSpeed) final; - -private: - - /** - * Get magic pattern. - * - * @return Magic pattern - */ - uint32_t getMagicPattern() const; - - /** - * Set magic pattern. - * - * @param[in] value Magic pattern - */ - void setMagicPattern(uint32_t value) const; - - /** - * Get data version. - * - * @return Data version - */ - uint8_t getDataVersion() const; - - /** - * Set data version. - * - * @param[in] value Data version - */ - void setDataVersion(uint8_t value) const; -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* SETTINGS_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Settings realization + * @author Andreas Merkle + * + * @addtogroup HALTarget + * + * @{ + */ + +#ifndef SETTINGS_H +#define SETTINGS_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ISettings.h" + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** This class handles settings stored in the EEPROM. */ +class Settings : public ISettings +{ +public: + /** + * Constructs the settings adapter. + */ + Settings() : ISettings() + { + } + + /** + * Destroys the button A adapter. + */ + ~Settings() + { + } + + /** + * Initialize the settings. + * + * If the settings are invalid or not compatible to the settings, the + * default values will be written! + */ + void init() final; + + /** + * Get the max. speed. + * + * @return Max. speed in steps/s. + */ + int16_t getMaxSpeed() const final; + + /** + * Set the max. speed. + * + * @param[in] maxSpeed Max. speed in steps/s. + */ + void setMaxSpeed(int16_t maxSpeed) final; + +private: + + /** + * Get magic pattern. + * + * @return Magic pattern + */ + uint32_t getMagicPattern() const; + + /** + * Set magic pattern. + * + * @param[in] value Magic pattern + */ + void setMagicPattern(uint32_t value) const; + + /** + * Get data version. + * + * @return Data version + */ + uint8_t getDataVersion() const; + + /** + * Set data version. + * + * @param[in] value Data version + */ + void setDataVersion(uint8_t value) const; +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* SETTINGS_H */ +/** @} */ diff --git a/lib/HALTarget/library.json b/lib/HALTarget/library.json index c7767af1..68a3d530 100644 --- a/lib/HALTarget/library.json +++ b/lib/HALTarget/library.json @@ -1,21 +1,21 @@ -{ - "name": "HALTarget", - "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" - }, { - "owner": "pololu", - "name": "Zumo32U4", - "version": "^2.0.1" - }], - "frameworks": "arduino", - "platforms": "*" +{ + "name": "HALTarget", + "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" + }, { + "owner": "pololu", + "name": "Zumo32U4", + "version": "^2.0.1" + }], + "frameworks": "arduino", + "platforms": "*" } \ No newline at end of file diff --git a/lib/HALTest/ButtonA.cpp b/lib/HALTest/ButtonA.cpp index c37847b5..6ece3e08 100644 --- a/lib/HALTest/ButtonA.cpp +++ b/lib/HALTest/ButtonA.cpp @@ -1,75 +1,75 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Button A realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ButtonA.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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Button A realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ButtonA.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/ButtonA.h b/lib/HALTest/ButtonA.h index 1d752b9b..2725a20d 100644 --- a/lib/HALTest/ButtonA.h +++ b/lib/HALTest/ButtonA.h @@ -1,100 +1,100 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Button A realization - * @author Andreas Merkle - * - * @addtogroup HALTarget - * - * @{ - */ - -#ifndef BUTTONA_H -#define BUTTONA_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "IButton.h" - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class provides access to the Zumo target button A. */ -class ButtonA : public IButton -{ -public: - /** - * Constructs the button A adapter. - */ - ButtonA() : IButton() - { - } - - /** - * Destroys the button A adapter. - */ - ~ButtonA() - { - } - - /** - * Is button pressed or not - * - * @return If button is pressed, returns true otherwise false. - */ - bool isPressed() final - { - return false; - } - - /** - * Wait until button is released. - */ - void waitForRelease() final - { - } - -private: - -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* BUTTONA_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Button A realization + * @author Andreas Merkle + * + * @addtogroup HALTarget + * + * @{ + */ + +#ifndef BUTTONA_H +#define BUTTONA_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "IButton.h" + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** This class provides access to the Zumo target button A. */ +class ButtonA : public IButton +{ +public: + /** + * Constructs the button A adapter. + */ + ButtonA() : IButton() + { + } + + /** + * Destroys the button A adapter. + */ + ~ButtonA() + { + } + + /** + * Is button pressed or not + * + * @return If button is pressed, returns true otherwise false. + */ + bool isPressed() final + { + return false; + } + + /** + * Wait until button is released. + */ + void waitForRelease() final + { + } + +private: + +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* BUTTONA_H */ +/** @} */ diff --git a/lib/HALTest/ButtonB.cpp b/lib/HALTest/ButtonB.cpp index 8d41e381..7ea5381d 100644 --- a/lib/HALTest/ButtonB.cpp +++ b/lib/HALTest/ButtonB.cpp @@ -1,75 +1,75 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Button B realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ButtonB.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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Button B realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ButtonB.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/ButtonB.h b/lib/HALTest/ButtonB.h index 63f7e792..f072f366 100644 --- a/lib/HALTest/ButtonB.h +++ b/lib/HALTest/ButtonB.h @@ -1,100 +1,100 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Button B realization - * @author Andreas Merkle - * - * @addtogroup HALTarget - * - * @{ - */ - -#ifndef BUTTONB_H -#define BUTTONB_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "IButton.h" - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class provides access to the Zumo target button B. */ -class ButtonB : public IButton -{ -public: - /** - * Constructs the button B adapter. - */ - ButtonB() : IButton() - { - } - - /** - * Destroys the button B adapter. - */ - ~ButtonB() - { - } - - /** - * Is button pressed or not - * - * @return If button is pressed, returns true otherwise false. - */ - bool isPressed() final - { - return false; - } - - /** - * Wait until button is released. - */ - void waitForRelease() final - { - } - -private: - -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* BUTTONB_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Button B realization + * @author Andreas Merkle + * + * @addtogroup HALTarget + * + * @{ + */ + +#ifndef BUTTONB_H +#define BUTTONB_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "IButton.h" + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** This class provides access to the Zumo target button B. */ +class ButtonB : public IButton +{ +public: + /** + * Constructs the button B adapter. + */ + ButtonB() : IButton() + { + } + + /** + * Destroys the button B adapter. + */ + ~ButtonB() + { + } + + /** + * Is button pressed or not + * + * @return If button is pressed, returns true otherwise false. + */ + bool isPressed() final + { + return false; + } + + /** + * Wait until button is released. + */ + void waitForRelease() final + { + } + +private: + +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* BUTTONB_H */ +/** @} */ diff --git a/lib/HALTest/ButtonC.cpp b/lib/HALTest/ButtonC.cpp index 5103da49..b3946a38 100644 --- a/lib/HALTest/ButtonC.cpp +++ b/lib/HALTest/ButtonC.cpp @@ -1,75 +1,75 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Button C realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ButtonC.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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Button C realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ButtonC.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/ButtonC.h b/lib/HALTest/ButtonC.h index 900331ab..ed0e97c8 100644 --- a/lib/HALTest/ButtonC.h +++ b/lib/HALTest/ButtonC.h @@ -1,100 +1,100 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Button C realization - * @author Andreas Merkle - * - * @addtogroup HALTarget - * - * @{ - */ - -#ifndef BUTTONC_H -#define BUTTONC_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "IButton.h" - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class provides access to the Zumo target button C. */ -class ButtonC : public IButton -{ -public: - /** - * Constructs the button C adapter. - */ - ButtonC() : IButton() - { - } - - /** - * Destroys the button C adapter. - */ - ~ButtonC() - { - } - - /** - * Is button pressed or not - * - * @return If button is pressed, returns true otherwise false. - */ - bool isPressed() final - { - return false; - } - - /** - * Wait until button is released. - */ - void waitForRelease() final - { - } - -private: - -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* BUTTONC_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Button C realization + * @author Andreas Merkle + * + * @addtogroup HALTarget + * + * @{ + */ + +#ifndef BUTTONC_H +#define BUTTONC_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "IButton.h" + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** This class provides access to the Zumo target button C. */ +class ButtonC : public IButton +{ +public: + /** + * Constructs the button C adapter. + */ + ButtonC() : IButton() + { + } + + /** + * Destroys the button C adapter. + */ + ~ButtonC() + { + } + + /** + * Is button pressed or not + * + * @return If button is pressed, returns true otherwise false. + */ + bool isPressed() final + { + return false; + } + + /** + * Wait until button is released. + */ + void waitForRelease() final + { + } + +private: + +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* BUTTONC_H */ +/** @} */ diff --git a/lib/HALTest/Display.cpp b/lib/HALTest/Display.cpp index a3628897..6468b6cb 100644 --- a/lib/HALTest/Display.cpp +++ b/lib/HALTest/Display.cpp @@ -1,75 +1,75 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Display realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "Display.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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Display realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "Display.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/Display.h b/lib/HALTest/Display.h index 1850a70c..a61e4a8f 100644 --- a/lib/HALTest/Display.h +++ b/lib/HALTest/Display.h @@ -1,183 +1,183 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Display realization - * @author Andreas Merkle - * - * @addtogroup HALTarget - * - * @{ - */ - -#ifndef DISPLAY_H -#define DISPLAY_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "IDisplay.h" - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class provides access to the Zumo target LCD. */ -class Display : public IDisplay -{ -public: - /** - * Constructs the display adapter. - */ - Display() : IDisplay() - { - } - - /** - * Destroys the display adapter. - */ - ~Display() - { - } - - /** - * Clear the display and set the cursor to the upper left corner. - */ - void clear() final - { - } - - /** - * Set the cursor to the given position. - * - * @param[in] xCoord x-coordinate, 0 is the most left position. - * @param[in] yCoord y-coordinate, 0 is the most upper position. - */ - void gotoXY(uint8_t xCoord, uint8_t yCoord) final - { - } - - /** - * Print the string to the display at the current cursor position. - * - * @param[in] str String - * - * @return Printed number of characters - */ - size_t print(const char str[]) final - { - return 0; - } - - /** - * Print the unsigned 8-bit value to the display at the current cursor position. - * - * @param[in] value Value - * - * @return Printed number of characters - */ - size_t print(uint8_t value) final - { - return 0; - } - - /** - * Print the unsigned 16-bit value to the display at the current cursor position. - * - * @param[in] value Value - * - * @return Printed number of characters - */ - size_t print(uint16_t value) final - { - return 0; - } - - /** - * Print the unsigned 32-bit value to the display at the current cursor position. - * - * @param[in] value Value - * - * @return Printed number of characters - */ - size_t print(uint32_t value) final - { - return 0; - } - - /** - * Print the signed 8-bit value to the display at the current cursor position. - * - * @param[in] value Value - * - * @return Printed number of characters - */ - size_t print(int8_t value) final - { - return 0; - } - - /** - * Print the signed 16-bit value to the display at the current cursor position. - * - * @param[in] value Value - * - * @return Printed number of characters - */ - size_t print(int16_t value) final - { - return 0; - } - - /** - * Print the signed 32-bit value to the display at the current cursor position. - * - * @param[in] value Value - * - * @return Printed number of characters - */ - size_t print(int32_t value) final - { - return 0; - } - -private: -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* DISPLAY_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Display realization + * @author Andreas Merkle + * + * @addtogroup HALTarget + * + * @{ + */ + +#ifndef DISPLAY_H +#define DISPLAY_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "IDisplay.h" + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** This class provides access to the Zumo target LCD. */ +class Display : public IDisplay +{ +public: + /** + * Constructs the display adapter. + */ + Display() : IDisplay() + { + } + + /** + * Destroys the display adapter. + */ + ~Display() + { + } + + /** + * Clear the display and set the cursor to the upper left corner. + */ + void clear() final + { + } + + /** + * Set the cursor to the given position. + * + * @param[in] xCoord x-coordinate, 0 is the most left position. + * @param[in] yCoord y-coordinate, 0 is the most upper position. + */ + void gotoXY(uint8_t xCoord, uint8_t yCoord) final + { + } + + /** + * Print the string to the display at the current cursor position. + * + * @param[in] str String + * + * @return Printed number of characters + */ + size_t print(const char str[]) final + { + return 0; + } + + /** + * Print the unsigned 8-bit value to the display at the current cursor position. + * + * @param[in] value Value + * + * @return Printed number of characters + */ + size_t print(uint8_t value) final + { + return 0; + } + + /** + * Print the unsigned 16-bit value to the display at the current cursor position. + * + * @param[in] value Value + * + * @return Printed number of characters + */ + size_t print(uint16_t value) final + { + return 0; + } + + /** + * Print the unsigned 32-bit value to the display at the current cursor position. + * + * @param[in] value Value + * + * @return Printed number of characters + */ + size_t print(uint32_t value) final + { + return 0; + } + + /** + * Print the signed 8-bit value to the display at the current cursor position. + * + * @param[in] value Value + * + * @return Printed number of characters + */ + size_t print(int8_t value) final + { + return 0; + } + + /** + * Print the signed 16-bit value to the display at the current cursor position. + * + * @param[in] value Value + * + * @return Printed number of characters + */ + size_t print(int16_t value) final + { + return 0; + } + + /** + * Print the signed 32-bit value to the display at the current cursor position. + * + * @param[in] value Value + * + * @return Printed number of characters + */ + size_t print(int32_t value) final + { + return 0; + } + +private: +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* DISPLAY_H */ +/** @} */ diff --git a/lib/HALTest/Encoders.cpp b/lib/HALTest/Encoders.cpp index 9eab875e..ce06f385 100644 --- a/lib/HALTest/Encoders.cpp +++ b/lib/HALTest/Encoders.cpp @@ -1,75 +1,75 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Encoders realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "Encoders.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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Encoders realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "Encoders.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/Encoders.h b/lib/HALTest/Encoders.h index 7eedc2f2..1c0a1129 100644 --- a/lib/HALTest/Encoders.h +++ b/lib/HALTest/Encoders.h @@ -1,183 +1,183 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Encoders realization - * @author Andreas Merkle - * - * @addtogroup HALTarget - * - * @{ - */ - -#ifndef ENCODERS_H -#define ENCODERS_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "IEncoders.h" -#include "IEncodersTest.h" - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class provides access to the Zumo target encoders. */ -class Encoders : public IEncoders, public IEncodersTest -{ -public: - /** - * Constructs the encoders adapter. - */ - Encoders() : IEncoders(), m_stepsLeft(0), m_stepsRight(0) - { - } - - /** - * Destroys the encoders adapter. - */ - ~Encoders() - { - } - - /** - * Initialize or re-initialize the encoders. - * This is used e.g. to re-initialize the encoders after the simulation world - * was reset, but the ext. robot is still active. - */ - void init() final - { - /* Nothing to do. */ - } - - /** - * Returns the number of counts that have been detected from the left-side - * encoder. These counts start at 0. Positive counts correspond to forward - * movement of the left side of the robot, while negative counts correspond - * to backwards movement. - * - * The count is returned as a signed 16-bit integer. When the count goes - * over 32767, it will overflow down to -32768. When the count goes below - * -32768, it will overflow up to 32767. - * - * @return Encoder steps left - */ - int16_t getCountsLeft() final - { - return m_stepsLeft; - } - - /** - * Returns the number of counts that have been detected from the right-side - * encoder. These counts start at 0. Positive counts correspond to forward - * movement of the left side of the robot, while negative counts correspond - * to backwards movement. - * - * The count is returned as a signed 16-bit integer. When the count goes - * over 32767, it will overflow down to -32768. When the count goes below - * -32768, it will overflow up to 32767. - * - * @return Encoder steps right - */ - int16_t getCountsRight() final - { - return m_stepsRight; - } - - /** - * This function is just like getCountsLeft() except it also clears the - * counts before returning. If you call this frequently enough, you will - * not have to worry about the count overflowing. - * - * @return Encoder steps left - */ - int16_t getCountsAndResetLeft() final - { - int16_t stepsLeft = m_stepsLeft; - - m_stepsLeft = 0; - - return stepsLeft; - } - - /** - * This function is just like getCountsRight() except it also clears the - * counts before returning. If you call this frequently enough, you will - * not have to worry about the count overflowing. - * - * @return Encoder steps right - */ - int16_t getCountsAndResetRight() final - { - int16_t stepsRight = m_stepsRight; - - m_stepsRight = 0; - - return stepsRight; - } - - /* ---------- Test Interface ---------- */ - - /** - * Set encoder steps left. - * - * @param[in] steps Encoder steps - */ - void setCountsLeft(int16_t steps) final - { - m_stepsLeft = steps; - } - - /** - * Set encoder steps right. - * - * @param[in] steps Encoder steps - */ - void setCountsRight(int16_t steps) final - { - m_stepsRight = steps; - } - -private: - int16_t m_stepsLeft; /**< Encoder steps left */ - int16_t m_stepsRight; /**< Encoder steps right*/ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* ENCODERS_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Encoders realization + * @author Andreas Merkle + * + * @addtogroup HALTarget + * + * @{ + */ + +#ifndef ENCODERS_H +#define ENCODERS_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "IEncoders.h" +#include "IEncodersTest.h" + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** This class provides access to the Zumo target encoders. */ +class Encoders : public IEncoders, public IEncodersTest +{ +public: + /** + * Constructs the encoders adapter. + */ + Encoders() : IEncoders(), m_stepsLeft(0), m_stepsRight(0) + { + } + + /** + * Destroys the encoders adapter. + */ + ~Encoders() + { + } + + /** + * Initialize or re-initialize the encoders. + * This is used e.g. to re-initialize the encoders after the simulation world + * was reset, but the ext. robot is still active. + */ + void init() final + { + /* Nothing to do. */ + } + + /** + * Returns the number of counts that have been detected from the left-side + * encoder. These counts start at 0. Positive counts correspond to forward + * movement of the left side of the robot, while negative counts correspond + * to backwards movement. + * + * The count is returned as a signed 16-bit integer. When the count goes + * over 32767, it will overflow down to -32768. When the count goes below + * -32768, it will overflow up to 32767. + * + * @return Encoder steps left + */ + int16_t getCountsLeft() final + { + return m_stepsLeft; + } + + /** + * Returns the number of counts that have been detected from the right-side + * encoder. These counts start at 0. Positive counts correspond to forward + * movement of the left side of the robot, while negative counts correspond + * to backwards movement. + * + * The count is returned as a signed 16-bit integer. When the count goes + * over 32767, it will overflow down to -32768. When the count goes below + * -32768, it will overflow up to 32767. + * + * @return Encoder steps right + */ + int16_t getCountsRight() final + { + return m_stepsRight; + } + + /** + * This function is just like getCountsLeft() except it also clears the + * counts before returning. If you call this frequently enough, you will + * not have to worry about the count overflowing. + * + * @return Encoder steps left + */ + int16_t getCountsAndResetLeft() final + { + int16_t stepsLeft = m_stepsLeft; + + m_stepsLeft = 0; + + return stepsLeft; + } + + /** + * This function is just like getCountsRight() except it also clears the + * counts before returning. If you call this frequently enough, you will + * not have to worry about the count overflowing. + * + * @return Encoder steps right + */ + int16_t getCountsAndResetRight() final + { + int16_t stepsRight = m_stepsRight; + + m_stepsRight = 0; + + return stepsRight; + } + + /* ---------- Test Interface ---------- */ + + /** + * Set encoder steps left. + * + * @param[in] steps Encoder steps + */ + void setCountsLeft(int16_t steps) final + { + m_stepsLeft = steps; + } + + /** + * Set encoder steps right. + * + * @param[in] steps Encoder steps + */ + void setCountsRight(int16_t steps) final + { + m_stepsRight = steps; + } + +private: + int16_t m_stepsLeft; /**< Encoder steps left */ + int16_t m_stepsRight; /**< Encoder steps right*/ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* ENCODERS_H */ +/** @} */ diff --git a/lib/HALTest/IMU.cpp b/lib/HALTest/IMU.cpp index 4f04f5ef..6458aafe 100644 --- a/lib/HALTest/IMU.cpp +++ b/lib/HALTest/IMU.cpp @@ -1,75 +1,75 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief 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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief 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 81d97165..55b1efbb 100644 --- a/lib/HALTest/IMU.h +++ b/lib/HALTest/IMU.h @@ -1,192 +1,192 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief 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() final - { - return true; - } - - /** - * Enables all of the inertial sensors with a default configuration. - */ - void enableDefault() final - { - }; - - /** - * Configures the sensors with settings optimized for turn sensing. - */ - void configureForTurnSensing() final - { - } - - /** - * Takes a reading from the accelerometer and makes the measurements available in a. - */ - void readAccelerometer() final - { - } - - /** - * Takes a reading from the gyro and makes the measurements available in g. - */ - void readGyro() final - { - } - - /** - * Takes a reading from the magnetometer and makes the measurements available in m. - */ - void readMagnetometer() final - { - } - - /** - * Indicates whether the accelerometer has new measurement data ready. - * - * @return True if there is new accelerometer data available; false otherwise. - */ - bool accelerometerDataReady() final - { - return true; - } - - /** - * Indicates whether the gyro has new measurement data ready. - * - * @return True if there is new gyro data available; false otherwise. - */ - bool gyroDataReady() final - { - return true; - } - - /** - * Indicates whether the magnetometer has new measurement data ready. - * - * @return True if there is new magnetometer data available; false otherwise. - */ - bool magnetometerDataReady() final - { - 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) final - { - } - - /** - * 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) final - { - } - - /** - * 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) final - { - } - - /** - * Calibrate the IMU. - */ - virtual void calibrate() final - { - } - -private: -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* IMU_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief 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() final + { + return true; + } + + /** + * Enables all of the inertial sensors with a default configuration. + */ + void enableDefault() final + { + }; + + /** + * Configures the sensors with settings optimized for turn sensing. + */ + void configureForTurnSensing() final + { + } + + /** + * Takes a reading from the accelerometer and makes the measurements available in a. + */ + void readAccelerometer() final + { + } + + /** + * Takes a reading from the gyro and makes the measurements available in g. + */ + void readGyro() final + { + } + + /** + * Takes a reading from the magnetometer and makes the measurements available in m. + */ + void readMagnetometer() final + { + } + + /** + * Indicates whether the accelerometer has new measurement data ready. + * + * @return True if there is new accelerometer data available; false otherwise. + */ + bool accelerometerDataReady() final + { + return true; + } + + /** + * Indicates whether the gyro has new measurement data ready. + * + * @return True if there is new gyro data available; false otherwise. + */ + bool gyroDataReady() final + { + return true; + } + + /** + * Indicates whether the magnetometer has new measurement data ready. + * + * @return True if there is new magnetometer data available; false otherwise. + */ + bool magnetometerDataReady() final + { + 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) final + { + } + + /** + * 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) final + { + } + + /** + * 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) final + { + } + + /** + * Calibrate the IMU. + */ + virtual void calibrate() final + { + } + +private: +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* IMU_H */ +/** @} */ diff --git a/lib/HALTest/LedGreen.cpp b/lib/HALTest/LedGreen.cpp index 98af3626..00d00a8d 100644 --- a/lib/HALTest/LedGreen.cpp +++ b/lib/HALTest/LedGreen.cpp @@ -1,75 +1,75 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Green LED realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "LedGreen.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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Green LED realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "LedGreen.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/LedGreen.h b/lib/HALTest/LedGreen.h index 35cc915f..7ded74c4 100644 --- a/lib/HALTest/LedGreen.h +++ b/lib/HALTest/LedGreen.h @@ -1,91 +1,91 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Green LED realization - * @author Andreas Merkle - * - * @addtogroup HALTarget - * - * @{ - */ - -#ifndef LEDGREEN_H -#define LEDGREEN_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ILed.h" - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class provides access to the Zumo target green LED. */ -class LedGreen : public ILed -{ -public: - /** - * Constructs the green LED adapter. - */ - LedGreen() : ILed() - { - } - - /** - * Destroys the green LED adapter. - */ - ~LedGreen() - { - } - - /** - * Enables/Disables the LED. - * - * @param[in] enableIt Enable LED with true, disable it with false. - */ - void enable(bool enableIt) final - { - } - -private: -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* LEDGREEN_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Green LED realization + * @author Andreas Merkle + * + * @addtogroup HALTarget + * + * @{ + */ + +#ifndef LEDGREEN_H +#define LEDGREEN_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ILed.h" + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** This class provides access to the Zumo target green LED. */ +class LedGreen : public ILed +{ +public: + /** + * Constructs the green LED adapter. + */ + LedGreen() : ILed() + { + } + + /** + * Destroys the green LED adapter. + */ + ~LedGreen() + { + } + + /** + * Enables/Disables the LED. + * + * @param[in] enableIt Enable LED with true, disable it with false. + */ + void enable(bool enableIt) final + { + } + +private: +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* LEDGREEN_H */ +/** @} */ diff --git a/lib/HALTest/LedRed.cpp b/lib/HALTest/LedRed.cpp index 3c3b13da..4ea34bd8 100644 --- a/lib/HALTest/LedRed.cpp +++ b/lib/HALTest/LedRed.cpp @@ -1,75 +1,75 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Red LED realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "LedRed.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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Red LED realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "LedRed.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/LedRed.h b/lib/HALTest/LedRed.h index fede9681..622e4be6 100644 --- a/lib/HALTest/LedRed.h +++ b/lib/HALTest/LedRed.h @@ -1,91 +1,91 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Red LED realization - * @author Andreas Merkle - * - * @addtogroup HALTarget - * - * @{ - */ - -#ifndef LEDRED_H -#define LEDRED_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ILed.h" - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class provides access to the Zumo target red LED. */ -class LedRed : public ILed -{ -public: - /** - * Constructs the red LED adapter. - */ - LedRed() : ILed() - { - } - - /** - * Destroys the red LED adapter. - */ - ~LedRed() - { - } - - /** - * Enables/Disables the LED. - * - * @param[in] enableIt Enable LED with true, disable it with false. - */ - void enable(bool enableIt) final - { - } - -private: -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* LEDRED_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Red LED realization + * @author Andreas Merkle + * + * @addtogroup HALTarget + * + * @{ + */ + +#ifndef LEDRED_H +#define LEDRED_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ILed.h" + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** This class provides access to the Zumo target red LED. */ +class LedRed : public ILed +{ +public: + /** + * Constructs the red LED adapter. + */ + LedRed() : ILed() + { + } + + /** + * Destroys the red LED adapter. + */ + ~LedRed() + { + } + + /** + * Enables/Disables the LED. + * + * @param[in] enableIt Enable LED with true, disable it with false. + */ + void enable(bool enableIt) final + { + } + +private: +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* LEDRED_H */ +/** @} */ diff --git a/lib/HALTest/LedYellow.cpp b/lib/HALTest/LedYellow.cpp index e7c7ced2..daac91da 100644 --- a/lib/HALTest/LedYellow.cpp +++ b/lib/HALTest/LedYellow.cpp @@ -1,75 +1,75 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Yellow LED realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "LedYellow.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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Yellow LED realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "LedYellow.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/LedYellow.h b/lib/HALTest/LedYellow.h index bbb1a741..8166c4f8 100644 --- a/lib/HALTest/LedYellow.h +++ b/lib/HALTest/LedYellow.h @@ -1,91 +1,91 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Yellow LED realization - * @author Andreas Merkle - * - * @addtogroup HALTarget - * - * @{ - */ - -#ifndef LEDYELLOW_H -#define LEDYELLOW_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ILed.h" - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class provides access to the Zumo target yellow LED. */ -class LedYellow : public ILed -{ -public: - /** - * Constructs the yellow LED adapter. - */ - LedYellow() : ILed() - { - } - - /** - * Destroys the yellow LED adapter. - */ - ~LedYellow() - { - } - - /** - * Enables/Disables the LED. - * - * @param[in] enableIt Enable LED with true, disable it with false. - */ - void enable(bool enableIt) final - { - } - -private: -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* LEDYELLOW_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Yellow LED realization + * @author Andreas Merkle + * + * @addtogroup HALTarget + * + * @{ + */ + +#ifndef LEDYELLOW_H +#define LEDYELLOW_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ILed.h" + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** This class provides access to the Zumo target yellow LED. */ +class LedYellow : public ILed +{ +public: + /** + * Constructs the yellow LED adapter. + */ + LedYellow() : ILed() + { + } + + /** + * Destroys the yellow LED adapter. + */ + ~LedYellow() + { + } + + /** + * Enables/Disables the LED. + * + * @param[in] enableIt Enable LED with true, disable it with false. + */ + void enable(bool enableIt) final + { + } + +private: +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* LEDYELLOW_H */ +/** @} */ diff --git a/lib/HALTest/LineSensors.cpp b/lib/HALTest/LineSensors.cpp index d3ed139c..5cb5f3c9 100644 --- a/lib/HALTest/LineSensors.cpp +++ b/lib/HALTest/LineSensors.cpp @@ -1,80 +1,80 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Line sensors array realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "LineSensors.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -bool LineSensors::isCalibrationSuccessful() -{ - return false; -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Line sensors array realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "LineSensors.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +bool LineSensors::isCalibrationSuccessful() +{ + return false; +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALTest/LineSensors.h b/lib/HALTest/LineSensors.h index 1553ed44..68c2a659 100644 --- a/lib/HALTest/LineSensors.h +++ b/lib/HALTest/LineSensors.h @@ -1,190 +1,190 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Line sensors array realization - * @author Andreas Merkle - * - * @addtogroup HALTarget - * - * @{ - */ - -#ifndef LINESENSORS_H -#define LINESENSORS_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ILineSensors.h" - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class provides access to the Zumo target line sensors. */ -class LineSensors : public ILineSensors -{ -public: - /** - * Constructs the line sensors adapter. - */ - LineSensors() : ILineSensors() - { - } - - /** - * Destroys the line sensors adapter. - */ - ~LineSensors() - { - } - - /** - * Initializes the line sensors. - */ - void init() final - { - } - - /** - * Reads the sensors for calibration. Call this method several times during - * turning the sensors over the line to determine the minimum and maximum - * values. - * - * The calibration factors are stored internally. - */ - void calibrate() final - { - } - - /** - * Determines the deviation and returns an estimated position of the robot - * with respect to a line. The estimate is made using a weighted average of - * the sensor indices multiplied by 1000, so that a return value of 0 - * indicates that the line is directly below sensor 0, a return value of - * 1000 indicates that the line is directly below sensor 1, 2000 - * indicates that it's below sensor 2000, etc. Intermediate values - * indicate that the line is between two sensors. The formula is: - * - * 0*value0 + 1000*value1 + 2000*value2 + ... - * -------------------------------------------- - * value0 + value1 + value2 + ... - * - * This function assumes a dark line (high values) surrounded by white - * (low values). - */ - int16_t readLine() final - { - return 0; - } - - /** - * Get last line sensor values. - * - * @return Line sensor values - */ - const uint16_t* getSensorValues() final - { - return 0; - } - - /** - * Checks whether the calibration was successful or not. - * It assumes that the environment brightness compensation is active. - * - * @return If successful, it will return true otherwise false. - */ - bool isCalibrationSuccessful() final; - - /** - * It will return the index of the sensor, which caused to fail the calibration. - * If calibration was successful, it will return 0xFF. - * If calibration was not not done yet, it will return 0xFE. - * - * @return Sensor index, starting with 0. Note the other cases in description. - */ - uint8_t getCalibErrorInfo() const final - { - return 0; - } - - /** - * Get number of used line sensors. - * - * @return Number of used line sensors - */ - uint8_t getNumLineSensors() const final - { - return MAX_SENSORS; - } - - /** - * Get max. value of a single line sensor in digits. - * The sensor value is indirect proportional to the reflectance. - * - * @return Max. line sensor value - */ - int16_t getSensorValueMax() const final - { - return SENSOR_MAX_VALUE; - } - - /** - * Resets the maximum and minimum values measured by each sensor. - */ - void resetCalibration() final - { - } - -private: - /** - * Number of used line sensors. This depends on the Zumo hardware configuration. - */ - static const uint8_t MAX_SENSORS = 5; - - /** - * Max. value of a single line sensor in digits (calibration already considered). - * It depends on the Zumo32U4LineSensors implementation. - * See Zumo32U4\QTRSensors.cpp @ readCalibrated() - */ - static const int16_t SENSOR_MAX_VALUE = 1000; -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* LINESENSORS_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Line sensors array realization + * @author Andreas Merkle + * + * @addtogroup HALTarget + * + * @{ + */ + +#ifndef LINESENSORS_H +#define LINESENSORS_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ILineSensors.h" + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** This class provides access to the Zumo target line sensors. */ +class LineSensors : public ILineSensors +{ +public: + /** + * Constructs the line sensors adapter. + */ + LineSensors() : ILineSensors() + { + } + + /** + * Destroys the line sensors adapter. + */ + ~LineSensors() + { + } + + /** + * Initializes the line sensors. + */ + void init() final + { + } + + /** + * Reads the sensors for calibration. Call this method several times during + * turning the sensors over the line to determine the minimum and maximum + * values. + * + * The calibration factors are stored internally. + */ + void calibrate() final + { + } + + /** + * Determines the deviation and returns an estimated position of the robot + * with respect to a line. The estimate is made using a weighted average of + * the sensor indices multiplied by 1000, so that a return value of 0 + * indicates that the line is directly below sensor 0, a return value of + * 1000 indicates that the line is directly below sensor 1, 2000 + * indicates that it's below sensor 2000, etc. Intermediate values + * indicate that the line is between two sensors. The formula is: + * + * 0*value0 + 1000*value1 + 2000*value2 + ... + * -------------------------------------------- + * value0 + value1 + value2 + ... + * + * This function assumes a dark line (high values) surrounded by white + * (low values). + */ + int16_t readLine() final + { + return 0; + } + + /** + * Get last line sensor values. + * + * @return Line sensor values + */ + const uint16_t* getSensorValues() final + { + return 0; + } + + /** + * Checks whether the calibration was successful or not. + * It assumes that the environment brightness compensation is active. + * + * @return If successful, it will return true otherwise false. + */ + bool isCalibrationSuccessful() final; + + /** + * It will return the index of the sensor, which caused to fail the calibration. + * If calibration was successful, it will return 0xFF. + * If calibration was not not done yet, it will return 0xFE. + * + * @return Sensor index, starting with 0. Note the other cases in description. + */ + uint8_t getCalibErrorInfo() const final + { + return 0; + } + + /** + * Get number of used line sensors. + * + * @return Number of used line sensors + */ + uint8_t getNumLineSensors() const final + { + return MAX_SENSORS; + } + + /** + * Get max. value of a single line sensor in digits. + * The sensor value is indirect proportional to the reflectance. + * + * @return Max. line sensor value + */ + int16_t getSensorValueMax() const final + { + return SENSOR_MAX_VALUE; + } + + /** + * Resets the maximum and minimum values measured by each sensor. + */ + void resetCalibration() final + { + } + +private: + /** + * Number of used line sensors. This depends on the Zumo hardware configuration. + */ + static const uint8_t MAX_SENSORS = 5; + + /** + * Max. value of a single line sensor in digits (calibration already considered). + * It depends on the Zumo32U4LineSensors implementation. + * See Zumo32U4\QTRSensors.cpp @ readCalibrated() + */ + static const int16_t SENSOR_MAX_VALUE = 1000; +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* LINESENSORS_H */ +/** @} */ diff --git a/lib/HALTest/Motors.cpp b/lib/HALTest/Motors.cpp index 12d26064..8ff04e38 100644 --- a/lib/HALTest/Motors.cpp +++ b/lib/HALTest/Motors.cpp @@ -1,75 +1,75 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Motors realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "LineSensors.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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Motors realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "LineSensors.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/Motors.h b/lib/HALTest/Motors.h index c975ad29..39882c90 100644 --- a/lib/HALTest/Motors.h +++ b/lib/HALTest/Motors.h @@ -1,166 +1,166 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Motors realization - * @author Andreas Merkle - * - * @addtogroup HALTarget - * - * @{ - */ - -#ifndef MOTORS_H -#define MOTORS_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "IMotors.h" -#include "IMotorsTest.h" - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class provides access to the Zumo target motors. */ -class Motors : public IMotors, public IMotorsTest -{ -public: - /** - * Constructs the motors adapter. - */ - Motors() : IMotors(), m_speedLeft(0), m_speedRight(0) - { - } - - /** - * Destroys the motors adapter. - */ - ~Motors() - { - } - - /** - * Initializes the motors. - */ - void init() final - { - /* Nothing to do. */ - } - - /** - * Sets the speeds for both motors. - * - * @param[in] leftSpeed A number from -400 to 400 representing the speed and - * direction of the right motor. Values of -400 or less result in full speed - * reverse, and values of 400 or more result in full speed forward. - * @param[in] rightSpeed A number from -400 to 400 representing the speed and - * direction of the right motor. Values of -400 or less result in full speed - * reverse, and values of 400 or more result in full speed forward. - */ - void setSpeeds(int16_t leftSpeed, int16_t rightSpeed) final - { - m_speedLeft = leftSpeed; - m_speedRight = rightSpeed; - } - - /** - * Get maximum speed of the motors in digits. - * - * @return Max. speed in digits - */ - int16_t getMaxSpeed() const final - { - return MAX_SPEED; - } - - /** - * Get the current speed of the left motor. - * - * @return The left motor speed in digits. - */ - int16_t getLeftSpeed() final - { - return m_speedLeft; - } - - /** - * Get the current speed of the right motor. - * - * @return The right motor speed in digits. - */ - int16_t getRightSpeed() final - { - return m_speedRight; - } - - /* ---------- Test Interface ---------- */ - - /** - * Set speed of the left motor. - * - * @param[in] speed Speed in digits. - */ - void setLeftSpeed(int16_t speed) final - { - m_speedLeft = speed; - } - - /** - * Set speed of the right motor. - * - * @param[in] speed Speed in digits. - */ - void setRightSpeed(int16_t speed) final - { - m_speedRight = speed; - } - -private: - /** - * The maximum speed of a single motor in PWM digits. - */ - static const int16_t MAX_SPEED = 400; - - int16_t m_speedLeft; /**< Left motor speed in digits. */ - int16_t m_speedRight; /**< Right motor speed in digits. */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* MOTORS_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Motors realization + * @author Andreas Merkle + * + * @addtogroup HALTarget + * + * @{ + */ + +#ifndef MOTORS_H +#define MOTORS_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "IMotors.h" +#include "IMotorsTest.h" + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** This class provides access to the Zumo target motors. */ +class Motors : public IMotors, public IMotorsTest +{ +public: + /** + * Constructs the motors adapter. + */ + Motors() : IMotors(), m_speedLeft(0), m_speedRight(0) + { + } + + /** + * Destroys the motors adapter. + */ + ~Motors() + { + } + + /** + * Initializes the motors. + */ + void init() final + { + /* Nothing to do. */ + } + + /** + * Sets the speeds for both motors. + * + * @param[in] leftSpeed A number from -400 to 400 representing the speed and + * direction of the right motor. Values of -400 or less result in full speed + * reverse, and values of 400 or more result in full speed forward. + * @param[in] rightSpeed A number from -400 to 400 representing the speed and + * direction of the right motor. Values of -400 or less result in full speed + * reverse, and values of 400 or more result in full speed forward. + */ + void setSpeeds(int16_t leftSpeed, int16_t rightSpeed) final + { + m_speedLeft = leftSpeed; + m_speedRight = rightSpeed; + } + + /** + * Get maximum speed of the motors in digits. + * + * @return Max. speed in digits + */ + int16_t getMaxSpeed() const final + { + return MAX_SPEED; + } + + /** + * Get the current speed of the left motor. + * + * @return The left motor speed in digits. + */ + int16_t getLeftSpeed() final + { + return m_speedLeft; + } + + /** + * Get the current speed of the right motor. + * + * @return The right motor speed in digits. + */ + int16_t getRightSpeed() final + { + return m_speedRight; + } + + /* ---------- Test Interface ---------- */ + + /** + * Set speed of the left motor. + * + * @param[in] speed Speed in digits. + */ + void setLeftSpeed(int16_t speed) final + { + m_speedLeft = speed; + } + + /** + * Set speed of the right motor. + * + * @param[in] speed Speed in digits. + */ + void setRightSpeed(int16_t speed) final + { + m_speedRight = speed; + } + +private: + /** + * The maximum speed of a single motor in PWM digits. + */ + static const int16_t MAX_SPEED = 400; + + int16_t m_speedLeft; /**< Left motor speed in digits. */ + int16_t m_speedRight; /**< Right motor speed in digits. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* MOTORS_H */ +/** @} */ diff --git a/lib/HALTest/ProximitySensors.cpp b/lib/HALTest/ProximitySensors.cpp index f4a7c1a2..2f39262c 100644 --- a/lib/HALTest/ProximitySensors.cpp +++ b/lib/HALTest/ProximitySensors.cpp @@ -1,75 +1,75 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Proximity sensors realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ProximitySensors.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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Proximity sensors realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ProximitySensors.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/ProximitySensors.h b/lib/HALTest/ProximitySensors.h index 50ea4815..1b532a96 100644 --- a/lib/HALTest/ProximitySensors.h +++ b/lib/HALTest/ProximitySensors.h @@ -1,139 +1,139 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Proximity sensors realization - * @author Andreas Merkle - * - * @addtogroup HALInterfaces - * - * @{ - */ -#ifndef PROXIMITYSENSORS_H -#define PROXIMITYSENSORS_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The proximity sensors realization for testing purposes. */ -class ProximitySensors : public IProximitySensors -{ -public: - /** - * Constructs the interface. - */ - ProximitySensors() : IProximitySensors() - { - } - - /** - * Destroys the interface. - */ - virtual ~ProximitySensors() - { - } - - /** - * Initialize only the front proximity sensor. - */ - void initFrontSensor() final - { - } - - /** - * Returns the number of sensors. - * - * @return Number of sensors - */ - uint8_t getNumSensors() const final - { - return 1U; - } - - /** - * Emits IR pulses and gets readings from the sensors. - */ - void read() final - { - } - - /** - * Returns the number of brightness levels for the left LEDs that - * activated the front proximity sensor. - * - * @return Number of brightness levels - */ - uint8_t countsFrontWithLeftLeds() const final - { - return 0; - } - - /** - * Returns the number of brightness levels for the right LEDs that - * activated the front proximity sensor. - * - * @return Number of brightness levels - */ - uint8_t countsFrontWithRightLeds() const final - { - return 0; - } - - /** - * Returns the number of brightness levels. - * - * @return Number of brightness levels. - */ - uint8_t getNumBrightnessLevels() const final - { - return 1; - } - -protected: -private: -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* PROXIMITYSENSORS_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Proximity sensors realization + * @author Andreas Merkle + * + * @addtogroup HALInterfaces + * + * @{ + */ +#ifndef PROXIMITYSENSORS_H +#define PROXIMITYSENSORS_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The proximity sensors realization for testing purposes. */ +class ProximitySensors : public IProximitySensors +{ +public: + /** + * Constructs the interface. + */ + ProximitySensors() : IProximitySensors() + { + } + + /** + * Destroys the interface. + */ + virtual ~ProximitySensors() + { + } + + /** + * Initialize only the front proximity sensor. + */ + void initFrontSensor() final + { + } + + /** + * Returns the number of sensors. + * + * @return Number of sensors + */ + uint8_t getNumSensors() const final + { + return 1U; + } + + /** + * Emits IR pulses and gets readings from the sensors. + */ + void read() final + { + } + + /** + * Returns the number of brightness levels for the left LEDs that + * activated the front proximity sensor. + * + * @return Number of brightness levels + */ + uint8_t countsFrontWithLeftLeds() const final + { + return 0; + } + + /** + * Returns the number of brightness levels for the right LEDs that + * activated the front proximity sensor. + * + * @return Number of brightness levels + */ + uint8_t countsFrontWithRightLeds() const final + { + return 0; + } + + /** + * Returns the number of brightness levels. + * + * @return Number of brightness levels. + */ + uint8_t getNumBrightnessLevels() const final + { + return 1; + } + +protected: +private: +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* PROXIMITYSENSORS_H */ +/** @} */ diff --git a/lib/HALTest/RobotConstants.h b/lib/HALTest/RobotConstants.h index a709b2de..90ffb4c9 100644 --- a/lib/HALTest/RobotConstants.h +++ b/lib/HALTest/RobotConstants.h @@ -1,102 +1,102 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Robot specific constants - * @author Andreas Merkle - * - * @addtogroup HALInterfaces - * - * @{ - */ - -#ifndef ROBOTCONSTANTS_H -#define ROBOTCONSTANTS_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** - * Abstracts the physical robot constants. - */ -namespace RobotConstants -{ - /** - * Gear ratio multiplied with 1000. - */ - static const uint32_t GEAR_RATIO = 75810; - - /** - * Encoder resolution in counts per revolution of the motor shaft. - */ - static const uint32_t ENCODER_RESOLUTION = 12; - - /** - * Calibrated wheel diameter in mm. - * This means the real wheel diameter was adapted after calibration drive. - */ - static const uint32_t WHEEL_DIAMETER = 36; - - /** - * Wheel circumference in um. - */ - static const uint32_t WHEEL_CIRCUMFERENCE = - static_cast(static_cast(WHEEL_DIAMETER) * PI * 1000.0f); - - /** - * Wheel base in mm. - * Distance between the left wheel center to the right wheel center. - */ - static const uint32_t WHEEL_BASE = 85; - - /** - * Number of encoder steps per m. - */ - static const uint32_t ENCODER_STEPS_PER_M = (ENCODER_RESOLUTION * GEAR_RATIO * 1000U) / WHEEL_CIRCUMFERENCE; - -}; /* namespace RobotConstants */ - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* ROBOTCONSTANTS_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Robot specific constants + * @author Andreas Merkle + * + * @addtogroup HALInterfaces + * + * @{ + */ + +#ifndef ROBOTCONSTANTS_H +#define ROBOTCONSTANTS_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** + * Abstracts the physical robot constants. + */ +namespace RobotConstants +{ + /** + * Gear ratio multiplied with 1000. + */ + static const uint32_t GEAR_RATIO = 75810; + + /** + * Encoder resolution in counts per revolution of the motor shaft. + */ + static const uint32_t ENCODER_RESOLUTION = 12; + + /** + * Calibrated wheel diameter in mm. + * This means the real wheel diameter was adapted after calibration drive. + */ + static const uint32_t WHEEL_DIAMETER = 36; + + /** + * Wheel circumference in um. + */ + static const uint32_t WHEEL_CIRCUMFERENCE = + static_cast(static_cast(WHEEL_DIAMETER) * PI * 1000.0f); + + /** + * Wheel base in mm. + * Distance between the left wheel center to the right wheel center. + */ + static const uint32_t WHEEL_BASE = 85; + + /** + * Number of encoder steps per m. + */ + static const uint32_t ENCODER_STEPS_PER_M = (ENCODER_RESOLUTION * GEAR_RATIO * 1000U) / WHEEL_CIRCUMFERENCE; + +}; /* namespace RobotConstants */ + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* ROBOTCONSTANTS_H */ +/** @} */ diff --git a/lib/HALTest/Settings.cpp b/lib/HALTest/Settings.cpp index e5d2205d..a2f9d3c3 100644 --- a/lib/HALTest/Settings.cpp +++ b/lib/HALTest/Settings.cpp @@ -1,89 +1,89 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Settings realization - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "Settings.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void Settings::init() -{ -} - -int16_t Settings::getMaxSpeed() const -{ - return m_maxSpeed; -} - -void Settings::setMaxSpeed(int16_t maxSpeed) -{ - m_maxSpeed = maxSpeed; -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Settings realization + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "Settings.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void Settings::init() +{ +} + +int16_t Settings::getMaxSpeed() const +{ + return m_maxSpeed; +} + +void Settings::setMaxSpeed(int16_t maxSpeed) +{ + m_maxSpeed = maxSpeed; +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/HALTest/Settings.h b/lib/HALTest/Settings.h index b3ca00ea..c1ef8cdf 100644 --- a/lib/HALTest/Settings.h +++ b/lib/HALTest/Settings.h @@ -1,111 +1,111 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Settings realization - * @author Andreas Merkle - * - * @addtogroup HALTarget - * - * @{ - */ - -#ifndef SETTINGS_H -#define SETTINGS_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "ISettings.h" - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class handles settings. */ -class Settings : public ISettings -{ -public: - /** - * Constructs the settings adapter. - */ - Settings() : ISettings(), m_maxSpeed(DEFAULT_MAX_SPEED) - { - } - - /** - * Destroys the button A adapter. - */ - ~Settings() - { - } - - /** - * Initialize the settings. - * - * If the settings are invalid or not compatible to the settings, the - * default values will be written! - */ - void init() final; - - /** - * Get the max. speed. - * - * @return Max. speed in steps/s. - */ - int16_t getMaxSpeed() const final; - - /** - * Set the max. speed. - * - * @param[in] maxSpeed Max. speed in steps/s. - */ - void setMaxSpeed(int16_t maxSpeed) final; - -private: - - /** - * Max. speed default values in steps/s. - */ - static const int16_t DEFAULT_MAX_SPEED = 0; - - int16_t m_maxSpeed; /**< Max. speed in steps/s. */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* SETTINGS_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Settings realization + * @author Andreas Merkle + * + * @addtogroup HALTarget + * + * @{ + */ + +#ifndef SETTINGS_H +#define SETTINGS_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "ISettings.h" + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** This class handles settings. */ +class Settings : public ISettings +{ +public: + /** + * Constructs the settings adapter. + */ + Settings() : ISettings(), m_maxSpeed(DEFAULT_MAX_SPEED) + { + } + + /** + * Destroys the button A adapter. + */ + ~Settings() + { + } + + /** + * Initialize the settings. + * + * If the settings are invalid or not compatible to the settings, the + * default values will be written! + */ + void init() final; + + /** + * Get the max. speed. + * + * @return Max. speed in steps/s. + */ + int16_t getMaxSpeed() const final; + + /** + * Set the max. speed. + * + * @param[in] maxSpeed Max. speed in steps/s. + */ + void setMaxSpeed(int16_t maxSpeed) final; + +private: + + /** + * Max. speed default values in steps/s. + */ + static const int16_t DEFAULT_MAX_SPEED = 0; + + int16_t m_maxSpeed; /**< Max. speed in steps/s. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* SETTINGS_H */ +/** @} */ diff --git a/lib/HALTest/library.json b/lib/HALTest/library.json index 5021d49b..f386d92b 100644 --- a/lib/HALTest/library.json +++ b/lib/HALTest/library.json @@ -1,19 +1,19 @@ -{ - "name": "HALTest", - "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": "HALInterfacesTest" - }], - "frameworks": "*", - "platforms": "*" +{ + "name": "HALTest", + "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": "HALInterfacesTest" + }], + "frameworks": "*", + "platforms": "*" } \ No newline at end of file diff --git a/lib/HALTestSim/library.json b/lib/HALTestSim/library.json index 67479021..71f6e4f9 100644 --- a/lib/HALTestSim/library.json +++ b/lib/HALTestSim/library.json @@ -1,19 +1,19 @@ -{ - "name": "HALTestSim", - "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": "HALTest" - }], - "frameworks": "*", - "platforms": "*" +{ + "name": "HALTestSim", + "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": "HALTest" + }], + "frameworks": "*", + "platforms": "*" } \ No newline at end of file diff --git a/lib/Service/DifferentialDrive.cpp b/lib/Service/DifferentialDrive.cpp index f760159e..c8699c23 100644 --- a/lib/Service/DifferentialDrive.cpp +++ b/lib/Service/DifferentialDrive.cpp @@ -1,286 +1,286 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Differential drive - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void DifferentialDrive::enable() -{ - m_linearSpeedCenterSetPoint = 0; - m_linearSpeedLeftSetPoint = 0; - m_linearSpeedRightSetPoint = 0; - m_angularSpeedSetPoint = 0; - - m_motorSpeedLeftPID.clear(); - m_motorSpeedRightPID.clear(); - - m_isEnabled = true; -} - -void DifferentialDrive::disable() -{ - IMotors& motors = Board::getInstance().getMotors(); - - /* Ensure that the motors are stopped, otherwise it may happen that the - * set points of 0 by the PID controllers are not reached yet. - */ - motors.setSpeeds(0, 0); - - m_linearSpeedCenterSetPoint = 0; - m_linearSpeedLeftSetPoint = 0; - m_linearSpeedRightSetPoint = 0; - m_angularSpeedSetPoint = 0; - - m_isEnabled = false; -} - -int16_t DifferentialDrive::getMaxMotorSpeed() const -{ - return m_maxMotorSpeed; -} - -void DifferentialDrive::setMaxMotorSpeed(int16_t maxMotorSpeed) -{ - m_maxMotorSpeed = maxMotorSpeed; - - m_motorSpeedLeftPID.setLimits(-m_maxMotorSpeed, m_maxMotorSpeed); - m_motorSpeedRightPID.setLimits(-m_maxMotorSpeed, m_maxMotorSpeed); -} - -int16_t DifferentialDrive::getLinearSpeed() const -{ - return m_linearSpeedCenterSetPoint; -} - -void DifferentialDrive::setLinearSpeed(int16_t linearSpeed) -{ - m_linearSpeedCenterSetPoint = constrain(linearSpeed, -m_maxMotorSpeed, m_maxMotorSpeed); - calculateLinearSpeedLeftRight(m_linearSpeedCenterSetPoint, m_angularSpeedSetPoint, m_linearSpeedLeftSetPoint, - m_linearSpeedRightSetPoint); -} - -void DifferentialDrive::getLinearSpeed(int16_t& linearSpeedLeft, int16_t& linearSpeedRight) -{ - linearSpeedLeft = m_linearSpeedLeftSetPoint; - linearSpeedRight = m_linearSpeedRightSetPoint; -} - -void DifferentialDrive::setLinearSpeed(int16_t linearSpeedLeft, int16_t linearSpeedRight) -{ - m_linearSpeedLeftSetPoint = constrain(linearSpeedLeft, -m_maxMotorSpeed, m_maxMotorSpeed); - m_linearSpeedRightSetPoint = constrain(linearSpeedRight, -m_maxMotorSpeed, m_maxMotorSpeed); - calculateLinearAndAngularSpeedCenter(m_linearSpeedLeftSetPoint, m_linearSpeedRightSetPoint, - m_linearSpeedCenterSetPoint, m_angularSpeedSetPoint); -} - -int16_t DifferentialDrive::getAngularSpeed() const -{ - return m_angularSpeedSetPoint; -} - -void DifferentialDrive::setAngularSpeed(int16_t angularSpeed) -{ - m_angularSpeedSetPoint = constrain(angularSpeed, -m_maxMotorSpeed, m_maxMotorSpeed); - calculateLinearSpeedLeftRight(m_linearSpeedCenterSetPoint, m_angularSpeedSetPoint, m_linearSpeedLeftSetPoint, - m_linearSpeedRightSetPoint); -} - -void DifferentialDrive::process(uint32_t period) -{ - /* The differential drive must be enabled. - * The calibration is essential! The max. motor speed in [steps/s] is needed for closed-loop-control. - */ - if ((true == m_isEnabled) && (0 < m_maxMotorSpeed)) - { - Speedometer& speedometer = Speedometer::getInstance(); - IMotors& motors = Board::getInstance().getMotors(); - int32_t maxMotorSpeed = static_cast(m_maxMotorSpeed); /* [steps/s] */ - int32_t pwmMaxMotorSpeed = static_cast(motors.getMaxSpeed()); /* [digits] */ - int16_t pwmMotorSpeedLeft = 0; /* [digits] */ - int16_t pwmMotorSpeedRight = 0; /* [digits] */ - int16_t linearSpeedLeft = speedometer.getLinearSpeedLeft(); /* [steps/s] */ - int16_t linearSpeedRight = speedometer.getLinearSpeedRight(); /* [steps/s] */ - - m_motorSpeedLeftPID.setSampleTime(period); - m_motorSpeedRightPID.setSampleTime(period); - - /* If left motor is stopped, the PID controller shall be cleared. */ - if (0 == m_linearSpeedLeftSetPoint) - { - m_motorSpeedLeftPID.clear(); - m_lastLinearSpeedLeft = 0; - } - /* Handle left motor PID control. */ - else - { - int32_t motorSpeedLeft = - m_lastLinearSpeedLeft + - m_motorSpeedLeftPID.calculate(m_linearSpeedLeftSetPoint, linearSpeedLeft); /* [steps/s] */ - - /* Limit to max. motor speed in [steps/s] */ - motorSpeedLeft = constrain(motorSpeedLeft, -m_maxMotorSpeed, m_maxMotorSpeed); - - /* For the velocity PID remember the last PID output value. */ - m_lastLinearSpeedLeft = motorSpeedLeft; - - /* Convert speed from [steps/s] to [digits]. */ - pwmMotorSpeedLeft = static_cast(motorSpeedLeft * pwmMaxMotorSpeed / maxMotorSpeed); /* [digits] */ - } - - /* If right motor is stopped, the PID controller shall be cleared. */ - if (0 == m_linearSpeedRightSetPoint) - { - m_motorSpeedRightPID.clear(); - m_lastLinearSpeedRight = 0; - } - /* Handle right motor PID control. */ - else - { - int32_t motorSpeedRight = - m_lastLinearSpeedRight + - m_motorSpeedRightPID.calculate(m_linearSpeedRightSetPoint, linearSpeedRight); /* [steps/s] */ - - /* Limit to max. motor speed in [steps/s] */ - motorSpeedRight = constrain(motorSpeedRight, -m_maxMotorSpeed, m_maxMotorSpeed); - - /* For the velocity PID remember the last PID output value. */ - m_lastLinearSpeedRight = motorSpeedRight; - - /* Convert speed from [steps/s] to [digits]. */ - pwmMotorSpeedRight = - static_cast(motorSpeedRight * pwmMaxMotorSpeed / maxMotorSpeed); /* [digits] */ - } - - motors.setSpeeds(pwmMotorSpeedLeft, pwmMotorSpeedRight); - } -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -DifferentialDrive::DifferentialDrive() : - m_isInit(false), - m_isEnabled(false), - m_maxMotorSpeed(0), - m_linearSpeedCenterSetPoint(0), - m_angularSpeedSetPoint(0), - m_linearSpeedLeftSetPoint(0), - m_linearSpeedRightSetPoint(0), - m_motorSpeedLeftPID(), - m_motorSpeedRightPID(), - m_lastLinearSpeedLeft(0), - m_lastLinearSpeedRight(0) -{ - m_motorSpeedLeftPID.setPFactor(PID_P_NUMERATOR, PID_P_DENOMINATOR); - m_motorSpeedLeftPID.setIFactor(PID_I_NUMERATOR, PID_I_DENOMINATOR); - m_motorSpeedLeftPID.setDFactor(PID_D_NUMERATOR, PID_D_DENOMINATOR); - - m_motorSpeedRightPID.setPFactor(PID_P_NUMERATOR, PID_P_DENOMINATOR); - m_motorSpeedRightPID.setIFactor(PID_I_NUMERATOR, PID_I_DENOMINATOR); - m_motorSpeedRightPID.setDFactor(PID_D_NUMERATOR, PID_D_DENOMINATOR); -} - -void DifferentialDrive::calculateLinearSpeedLeftRight(int16_t linearSpeedCenter, int16_t angularSpeed, - int16_t& linearSpeedLeft, int16_t& linearSpeedRight) -{ - int32_t linearSpeedCenter32 = static_cast(linearSpeedCenter); /* [steps/s] */ - int32_t angularSpeed32 = static_cast(angularSpeed); /* [mrad/s] */ - int32_t wheelBase32 = static_cast(RobotConstants::WHEEL_BASE); /* [mm] */ - - /* angular speed = 2 * (linear speed right - linear speed left ) / wheel base - * linear speed right - linear speed left = angular speed * wheel base / 2 - * - * linear speed right = - linear speed left - */ - - linearSpeedLeft = linearSpeedCenter32 - (angularSpeed32 * wheelBase32) / 2; - linearSpeedRight = linearSpeedCenter32 + (angularSpeed32 * wheelBase32) / 2; -} - -void DifferentialDrive::calculateLinearAndAngularSpeedCenter(int16_t linearSpeedLeft, int16_t linearSpeedRight, - int16_t& linearSpeedCenter, int16_t& angularSpeed) -{ - int32_t linearSpeedLeft32 = static_cast(linearSpeedLeft); /* [steps/s] */ - int32_t linearSpeedRight32 = static_cast(linearSpeedRight); /* [steps/s] */ - int32_t wheelBase32 = static_cast(RobotConstants::WHEEL_BASE); /* [mm] */ - int32_t linearSpeedCenter32 = (linearSpeedRight32 + linearSpeedLeft32) / 2; /* [steps/s] */ - int32_t angularSpeed32 = (2 * (linearSpeedRight32 - linearSpeedLeft32)) / wheelBase32; /* [mrad/s] */ - - /* linear speed = (linear speed right + linear speed left) / 2 - * - * angular speed = 2 * (linear speed right - linear speed left ) / wheel base - */ - - linearSpeedCenter = static_cast(linearSpeedCenter32); - angularSpeed = static_cast(angularSpeed32); -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Differential drive + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void DifferentialDrive::enable() +{ + m_linearSpeedCenterSetPoint = 0; + m_linearSpeedLeftSetPoint = 0; + m_linearSpeedRightSetPoint = 0; + m_angularSpeedSetPoint = 0; + + m_motorSpeedLeftPID.clear(); + m_motorSpeedRightPID.clear(); + + m_isEnabled = true; +} + +void DifferentialDrive::disable() +{ + IMotors& motors = Board::getInstance().getMotors(); + + /* Ensure that the motors are stopped, otherwise it may happen that the + * set points of 0 by the PID controllers are not reached yet. + */ + motors.setSpeeds(0, 0); + + m_linearSpeedCenterSetPoint = 0; + m_linearSpeedLeftSetPoint = 0; + m_linearSpeedRightSetPoint = 0; + m_angularSpeedSetPoint = 0; + + m_isEnabled = false; +} + +int16_t DifferentialDrive::getMaxMotorSpeed() const +{ + return m_maxMotorSpeed; +} + +void DifferentialDrive::setMaxMotorSpeed(int16_t maxMotorSpeed) +{ + m_maxMotorSpeed = maxMotorSpeed; + + m_motorSpeedLeftPID.setLimits(-m_maxMotorSpeed, m_maxMotorSpeed); + m_motorSpeedRightPID.setLimits(-m_maxMotorSpeed, m_maxMotorSpeed); +} + +int16_t DifferentialDrive::getLinearSpeed() const +{ + return m_linearSpeedCenterSetPoint; +} + +void DifferentialDrive::setLinearSpeed(int16_t linearSpeed) +{ + m_linearSpeedCenterSetPoint = constrain(linearSpeed, -m_maxMotorSpeed, m_maxMotorSpeed); + calculateLinearSpeedLeftRight(m_linearSpeedCenterSetPoint, m_angularSpeedSetPoint, m_linearSpeedLeftSetPoint, + m_linearSpeedRightSetPoint); +} + +void DifferentialDrive::getLinearSpeed(int16_t& linearSpeedLeft, int16_t& linearSpeedRight) +{ + linearSpeedLeft = m_linearSpeedLeftSetPoint; + linearSpeedRight = m_linearSpeedRightSetPoint; +} + +void DifferentialDrive::setLinearSpeed(int16_t linearSpeedLeft, int16_t linearSpeedRight) +{ + m_linearSpeedLeftSetPoint = constrain(linearSpeedLeft, -m_maxMotorSpeed, m_maxMotorSpeed); + m_linearSpeedRightSetPoint = constrain(linearSpeedRight, -m_maxMotorSpeed, m_maxMotorSpeed); + calculateLinearAndAngularSpeedCenter(m_linearSpeedLeftSetPoint, m_linearSpeedRightSetPoint, + m_linearSpeedCenterSetPoint, m_angularSpeedSetPoint); +} + +int16_t DifferentialDrive::getAngularSpeed() const +{ + return m_angularSpeedSetPoint; +} + +void DifferentialDrive::setAngularSpeed(int16_t angularSpeed) +{ + m_angularSpeedSetPoint = constrain(angularSpeed, -m_maxMotorSpeed, m_maxMotorSpeed); + calculateLinearSpeedLeftRight(m_linearSpeedCenterSetPoint, m_angularSpeedSetPoint, m_linearSpeedLeftSetPoint, + m_linearSpeedRightSetPoint); +} + +void DifferentialDrive::process(uint32_t period) +{ + /* The differential drive must be enabled. + * The calibration is essential! The max. motor speed in [steps/s] is needed for closed-loop-control. + */ + if ((true == m_isEnabled) && (0 < m_maxMotorSpeed)) + { + Speedometer& speedometer = Speedometer::getInstance(); + IMotors& motors = Board::getInstance().getMotors(); + int32_t maxMotorSpeed = static_cast(m_maxMotorSpeed); /* [steps/s] */ + int32_t pwmMaxMotorSpeed = static_cast(motors.getMaxSpeed()); /* [digits] */ + int16_t pwmMotorSpeedLeft = 0; /* [digits] */ + int16_t pwmMotorSpeedRight = 0; /* [digits] */ + int16_t linearSpeedLeft = speedometer.getLinearSpeedLeft(); /* [steps/s] */ + int16_t linearSpeedRight = speedometer.getLinearSpeedRight(); /* [steps/s] */ + + m_motorSpeedLeftPID.setSampleTime(period); + m_motorSpeedRightPID.setSampleTime(period); + + /* If left motor is stopped, the PID controller shall be cleared. */ + if (0 == m_linearSpeedLeftSetPoint) + { + m_motorSpeedLeftPID.clear(); + m_lastLinearSpeedLeft = 0; + } + /* Handle left motor PID control. */ + else + { + int32_t motorSpeedLeft = + m_lastLinearSpeedLeft + + m_motorSpeedLeftPID.calculate(m_linearSpeedLeftSetPoint, linearSpeedLeft); /* [steps/s] */ + + /* Limit to max. motor speed in [steps/s] */ + motorSpeedLeft = constrain(motorSpeedLeft, -m_maxMotorSpeed, m_maxMotorSpeed); + + /* For the velocity PID remember the last PID output value. */ + m_lastLinearSpeedLeft = motorSpeedLeft; + + /* Convert speed from [steps/s] to [digits]. */ + pwmMotorSpeedLeft = static_cast(motorSpeedLeft * pwmMaxMotorSpeed / maxMotorSpeed); /* [digits] */ + } + + /* If right motor is stopped, the PID controller shall be cleared. */ + if (0 == m_linearSpeedRightSetPoint) + { + m_motorSpeedRightPID.clear(); + m_lastLinearSpeedRight = 0; + } + /* Handle right motor PID control. */ + else + { + int32_t motorSpeedRight = + m_lastLinearSpeedRight + + m_motorSpeedRightPID.calculate(m_linearSpeedRightSetPoint, linearSpeedRight); /* [steps/s] */ + + /* Limit to max. motor speed in [steps/s] */ + motorSpeedRight = constrain(motorSpeedRight, -m_maxMotorSpeed, m_maxMotorSpeed); + + /* For the velocity PID remember the last PID output value. */ + m_lastLinearSpeedRight = motorSpeedRight; + + /* Convert speed from [steps/s] to [digits]. */ + pwmMotorSpeedRight = + static_cast(motorSpeedRight * pwmMaxMotorSpeed / maxMotorSpeed); /* [digits] */ + } + + motors.setSpeeds(pwmMotorSpeedLeft, pwmMotorSpeedRight); + } +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +DifferentialDrive::DifferentialDrive() : + m_isInit(false), + m_isEnabled(false), + m_maxMotorSpeed(0), + m_linearSpeedCenterSetPoint(0), + m_angularSpeedSetPoint(0), + m_linearSpeedLeftSetPoint(0), + m_linearSpeedRightSetPoint(0), + m_motorSpeedLeftPID(), + m_motorSpeedRightPID(), + m_lastLinearSpeedLeft(0), + m_lastLinearSpeedRight(0) +{ + m_motorSpeedLeftPID.setPFactor(PID_P_NUMERATOR, PID_P_DENOMINATOR); + m_motorSpeedLeftPID.setIFactor(PID_I_NUMERATOR, PID_I_DENOMINATOR); + m_motorSpeedLeftPID.setDFactor(PID_D_NUMERATOR, PID_D_DENOMINATOR); + + m_motorSpeedRightPID.setPFactor(PID_P_NUMERATOR, PID_P_DENOMINATOR); + m_motorSpeedRightPID.setIFactor(PID_I_NUMERATOR, PID_I_DENOMINATOR); + m_motorSpeedRightPID.setDFactor(PID_D_NUMERATOR, PID_D_DENOMINATOR); +} + +void DifferentialDrive::calculateLinearSpeedLeftRight(int16_t linearSpeedCenter, int16_t angularSpeed, + int16_t& linearSpeedLeft, int16_t& linearSpeedRight) +{ + int32_t linearSpeedCenter32 = static_cast(linearSpeedCenter); /* [steps/s] */ + int32_t angularSpeed32 = static_cast(angularSpeed); /* [mrad/s] */ + int32_t wheelBase32 = static_cast(RobotConstants::WHEEL_BASE); /* [mm] */ + + /* angular speed = 2 * (linear speed right - linear speed left ) / wheel base + * linear speed right - linear speed left = angular speed * wheel base / 2 + * + * linear speed right = - linear speed left + */ + + linearSpeedLeft = linearSpeedCenter32 - (angularSpeed32 * wheelBase32) / 2; + linearSpeedRight = linearSpeedCenter32 + (angularSpeed32 * wheelBase32) / 2; +} + +void DifferentialDrive::calculateLinearAndAngularSpeedCenter(int16_t linearSpeedLeft, int16_t linearSpeedRight, + int16_t& linearSpeedCenter, int16_t& angularSpeed) +{ + int32_t linearSpeedLeft32 = static_cast(linearSpeedLeft); /* [steps/s] */ + int32_t linearSpeedRight32 = static_cast(linearSpeedRight); /* [steps/s] */ + int32_t wheelBase32 = static_cast(RobotConstants::WHEEL_BASE); /* [mm] */ + int32_t linearSpeedCenter32 = (linearSpeedRight32 + linearSpeedLeft32) / 2; /* [steps/s] */ + int32_t angularSpeed32 = (2 * (linearSpeedRight32 - linearSpeedLeft32)) / wheelBase32; /* [mrad/s] */ + + /* linear speed = (linear speed right + linear speed left) / 2 + * + * angular speed = 2 * (linear speed right - linear speed left ) / wheel base + */ + + linearSpeedCenter = static_cast(linearSpeedCenter32); + angularSpeed = static_cast(angularSpeed32); +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/Service/DifferentialDrive.h b/lib/Service/DifferentialDrive.h index b0665b00..f6be652a 100644 --- a/lib/Service/DifferentialDrive.h +++ b/lib/Service/DifferentialDrive.h @@ -1,275 +1,275 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Differential drive - * @author Andreas Merkle - * - * @addtogroup Service - * - * @{ - */ - -#ifndef DIFFERENTIAL_DRIVE_H -#define DIFFERENTIAL_DRIVE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** - * The differential drive control considers a two wheeled robot. - * - * Drive the robot by controlling the center linear speed or the - * angular speed. - * - * The left and right speed can be controlled separately as well. - * - * All values used for control and measurement are in [steps/s] or [mrad/s]. - * - * Calculations are performed in fixed point arithmetic for better performance. - */ -class DifferentialDrive -{ -public: - /** - * Get the instance of the differential drive control. - * - * @return Instance - */ - static DifferentialDrive& getInstance() - { - static DifferentialDrive instance; /* idiom */ - - return instance; - } - - /** - * Enable the differential drive control. - * It will control the motors during processing if the max. speed is available too. - * - * The linear and angular speeds will be set to 0. This avoid any - * unexpected driving behaviour. The internal PID controllers will - * be reset. - */ - void enable(); - - /** - * Disable the differential drive control. - * It will stop the motors to avoid any unexpected driving behaviour. - * After that the motors are not controlled anymore. - * - * Important: Processing the differential drive control shall not be stopped. - */ - void disable(); - - /** - * Get the max. motor speed in [steps/s]. - * If 0 is returned, it means there was no calibration yet. - * - * @return Max. motor speed [steps/s] - */ - int16_t getMaxMotorSpeed() const; - - /** - * Set the max. motor speed in [steps/s]. - * Determine the max. motor speed by calibration. - * - * @param[in] maxMotorSpeed Max. motor speed in [steps/s] - */ - void setMaxMotorSpeed(int16_t maxMotorSpeed); - - /** - * Get the linear speed center set point. - * Note, this is the speed which is commanded and not the measured one. - * - * @return Linear speed center [steps/s] - */ - int16_t getLinearSpeed() const; - - /** - * Set the linear speed center in [steps/s]. - * The speed will be limited to the max. motor speed. - * - * @param[in] linearSpeed Linear speed center [steps/s] - */ - void setLinearSpeed(int16_t linearSpeed); - - /** - * Gett the linear speed left and right set points. - * Note, these are the speeds which are commanded and not the measured ones. - * - * @param[in] linearSpeedLeft Linear speed left set point [steps/s] - * @param[in] linearSpeedRight Linear speed right set point [steps/s] - */ - void getLinearSpeed(int16_t& linearSpeedLeft, int16_t& linearSpeedRight); - - /** - * Set the linear speed left and right in [steps/s]. - * The speed will be limited to the max. motor speed. - * - * If left and right speed values are equal, it will drive straight forward - * otherwise will turn into left or right direction. - * - * @param[in] linearSpeedLeft Linear speed left [steps/s] - * @param[in] linearSpeedRight Linear speed right [steps/s] - */ - void setLinearSpeed(int16_t linearSpeedLeft, int16_t linearSpeedRight); - - /** - * Get the angular speed set point. - * Note, this is the speed which is commanded and not the measured one. - * - * @return Angular speed [mrad/s] - */ - int16_t getAngularSpeed() const; - - /** - * Set the angular speed in [mrad/s]. - * - * @param[in] angularSpeed Angular speed [mrad/s] - */ - void setAngularSpeed(int16_t angularSpeed); - - /** - * Process the differential drive periodically. - * - * @param[in] period Calling period in [ms] - */ - void process(uint32_t period); - -private: - /** - * The PID proportional factor numerator for the speed control. - */ - static const int16_t PID_P_NUMERATOR = 1; - - /** - * The PID proportional factor denominator for the speed control. - */ - static const int16_t PID_P_DENOMINATOR = 5; - - /** - * The PID integral factor numerator for the speed control. - */ - static const int16_t PID_I_NUMERATOR = 0; - - /** - * The PID integral factor denominator for the speed control. - */ - static const int16_t PID_I_DENOMINATOR = 1; - - /** - * The PID derivative factor numerator for the speed control. - */ - static const int16_t PID_D_NUMERATOR = 2; - - /** - * The PID derivative factor denominator for the speed control. - */ - static const int16_t PID_D_DENOMINATOR = 1; - - int16_t m_isInit; /**< Used to determine the initialization in the first time process() is called. */ - bool m_isEnabled; /**< Enable/Disable the differential drive control. */ - - int16_t m_maxMotorSpeed; /**< Max. motor speed [steps/s] */ - - int16_t m_linearSpeedCenterSetPoint; /**< Linear speed central in [steps/s] set point */ - int16_t m_angularSpeedSetPoint; /**< Angular speed central in [mrad/s set] point */ - - int16_t m_linearSpeedLeftSetPoint; /**< Linear speed left in [steps/s] set point */ - int16_t m_linearSpeedRightSetPoint; /**< Linear speed right in [steps/s] set point */ - - PIDController m_motorSpeedLeftPID; /**< PID controller for the left motor speed. */ - PIDController m_motorSpeedRightPID; /**< PID controller for the right motor speed. */ - - int32_t m_lastLinearSpeedLeft; /**< Last linear speed left PID output in [steps/s]. */ - int32_t m_lastLinearSpeedRight; /**< Last linear speed right PID output in [steps/s]. */ - - /** - * Construct differential drive control. - * It is disabled by default. - */ - DifferentialDrive(); - - /** - * Destroy differential drive control. - */ - ~DifferentialDrive() - { - /* Never called. */ - } - - /* Not allowed. */ - DifferentialDrive(const DifferentialDrive& diffDrive); /**< Copy construction of an instance. */ - DifferentialDrive& operator=(const DifferentialDrive& diffDrive); /**< Assignment of an instance. */ - - /** - * Calculate the linear speed left and right from the linear speed center and - * the angular speed. - * - * @param[in] linearSpeedCenter Linear speed center in [steps/s] - * @param[in] angularSpeed Angular speed in [mrad/s] - * @param[out] linearSpeedLeft Linear speed left in [steps/s] - * @param[out] linearSpeedRight Linear speed right in [steps/s] - */ - void calculateLinearSpeedLeftRight(int16_t linearSpeedCenter, int16_t angularSpeed, int16_t& linearSpeedLeft, - int16_t& linearSpeedRight); - - /** - * Calculate the linear speed center and the angular speed from the - * linear speed left and right. - * - * @param[in] linearSpeedLeft Linear speed left in [steps/s] - * @param[in] linearSpeedRight Linear speed right in [steps/s] - * @param[out] linearSpeedCenter Linear speed center in [steps/s] - * @param[out] angularSpeed Angular speed in [mrad/s] - */ - void calculateLinearAndAngularSpeedCenter(int16_t linearSpeedLeft, int16_t linearSpeedRight, - int16_t& linearSpeedCenter, int16_t& angularSpeed); -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* DIFFERENTIAL_DRIVE_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Differential drive + * @author Andreas Merkle + * + * @addtogroup Service + * + * @{ + */ + +#ifndef DIFFERENTIAL_DRIVE_H +#define DIFFERENTIAL_DRIVE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** + * The differential drive control considers a two wheeled robot. + * + * Drive the robot by controlling the center linear speed or the + * angular speed. + * + * The left and right speed can be controlled separately as well. + * + * All values used for control and measurement are in [steps/s] or [mrad/s]. + * + * Calculations are performed in fixed point arithmetic for better performance. + */ +class DifferentialDrive +{ +public: + /** + * Get the instance of the differential drive control. + * + * @return Instance + */ + static DifferentialDrive& getInstance() + { + static DifferentialDrive instance; /* idiom */ + + return instance; + } + + /** + * Enable the differential drive control. + * It will control the motors during processing if the max. speed is available too. + * + * The linear and angular speeds will be set to 0. This avoid any + * unexpected driving behaviour. The internal PID controllers will + * be reset. + */ + void enable(); + + /** + * Disable the differential drive control. + * It will stop the motors to avoid any unexpected driving behaviour. + * After that the motors are not controlled anymore. + * + * Important: Processing the differential drive control shall not be stopped. + */ + void disable(); + + /** + * Get the max. motor speed in [steps/s]. + * If 0 is returned, it means there was no calibration yet. + * + * @return Max. motor speed [steps/s] + */ + int16_t getMaxMotorSpeed() const; + + /** + * Set the max. motor speed in [steps/s]. + * Determine the max. motor speed by calibration. + * + * @param[in] maxMotorSpeed Max. motor speed in [steps/s] + */ + void setMaxMotorSpeed(int16_t maxMotorSpeed); + + /** + * Get the linear speed center set point. + * Note, this is the speed which is commanded and not the measured one. + * + * @return Linear speed center [steps/s] + */ + int16_t getLinearSpeed() const; + + /** + * Set the linear speed center in [steps/s]. + * The speed will be limited to the max. motor speed. + * + * @param[in] linearSpeed Linear speed center [steps/s] + */ + void setLinearSpeed(int16_t linearSpeed); + + /** + * Gett the linear speed left and right set points. + * Note, these are the speeds which are commanded and not the measured ones. + * + * @param[in] linearSpeedLeft Linear speed left set point [steps/s] + * @param[in] linearSpeedRight Linear speed right set point [steps/s] + */ + void getLinearSpeed(int16_t& linearSpeedLeft, int16_t& linearSpeedRight); + + /** + * Set the linear speed left and right in [steps/s]. + * The speed will be limited to the max. motor speed. + * + * If left and right speed values are equal, it will drive straight forward + * otherwise will turn into left or right direction. + * + * @param[in] linearSpeedLeft Linear speed left [steps/s] + * @param[in] linearSpeedRight Linear speed right [steps/s] + */ + void setLinearSpeed(int16_t linearSpeedLeft, int16_t linearSpeedRight); + + /** + * Get the angular speed set point. + * Note, this is the speed which is commanded and not the measured one. + * + * @return Angular speed [mrad/s] + */ + int16_t getAngularSpeed() const; + + /** + * Set the angular speed in [mrad/s]. + * + * @param[in] angularSpeed Angular speed [mrad/s] + */ + void setAngularSpeed(int16_t angularSpeed); + + /** + * Process the differential drive periodically. + * + * @param[in] period Calling period in [ms] + */ + void process(uint32_t period); + +private: + /** + * The PID proportional factor numerator for the speed control. + */ + static const int16_t PID_P_NUMERATOR = 1; + + /** + * The PID proportional factor denominator for the speed control. + */ + static const int16_t PID_P_DENOMINATOR = 5; + + /** + * The PID integral factor numerator for the speed control. + */ + static const int16_t PID_I_NUMERATOR = 0; + + /** + * The PID integral factor denominator for the speed control. + */ + static const int16_t PID_I_DENOMINATOR = 1; + + /** + * The PID derivative factor numerator for the speed control. + */ + static const int16_t PID_D_NUMERATOR = 2; + + /** + * The PID derivative factor denominator for the speed control. + */ + static const int16_t PID_D_DENOMINATOR = 1; + + int16_t m_isInit; /**< Used to determine the initialization in the first time process() is called. */ + bool m_isEnabled; /**< Enable/Disable the differential drive control. */ + + int16_t m_maxMotorSpeed; /**< Max. motor speed [steps/s] */ + + int16_t m_linearSpeedCenterSetPoint; /**< Linear speed central in [steps/s] set point */ + int16_t m_angularSpeedSetPoint; /**< Angular speed central in [mrad/s set] point */ + + int16_t m_linearSpeedLeftSetPoint; /**< Linear speed left in [steps/s] set point */ + int16_t m_linearSpeedRightSetPoint; /**< Linear speed right in [steps/s] set point */ + + PIDController m_motorSpeedLeftPID; /**< PID controller for the left motor speed. */ + PIDController m_motorSpeedRightPID; /**< PID controller for the right motor speed. */ + + int32_t m_lastLinearSpeedLeft; /**< Last linear speed left PID output in [steps/s]. */ + int32_t m_lastLinearSpeedRight; /**< Last linear speed right PID output in [steps/s]. */ + + /** + * Construct differential drive control. + * It is disabled by default. + */ + DifferentialDrive(); + + /** + * Destroy differential drive control. + */ + ~DifferentialDrive() + { + /* Never called. */ + } + + /* Not allowed. */ + DifferentialDrive(const DifferentialDrive& diffDrive); /**< Copy construction of an instance. */ + DifferentialDrive& operator=(const DifferentialDrive& diffDrive); /**< Assignment of an instance. */ + + /** + * Calculate the linear speed left and right from the linear speed center and + * the angular speed. + * + * @param[in] linearSpeedCenter Linear speed center in [steps/s] + * @param[in] angularSpeed Angular speed in [mrad/s] + * @param[out] linearSpeedLeft Linear speed left in [steps/s] + * @param[out] linearSpeedRight Linear speed right in [steps/s] + */ + void calculateLinearSpeedLeftRight(int16_t linearSpeedCenter, int16_t angularSpeed, int16_t& linearSpeedLeft, + int16_t& linearSpeedRight); + + /** + * Calculate the linear speed center and the angular speed from the + * linear speed left and right. + * + * @param[in] linearSpeedLeft Linear speed left in [steps/s] + * @param[in] linearSpeedRight Linear speed right in [steps/s] + * @param[out] linearSpeedCenter Linear speed center in [steps/s] + * @param[out] angularSpeed Angular speed in [mrad/s] + */ + void calculateLinearAndAngularSpeedCenter(int16_t linearSpeedLeft, int16_t linearSpeedRight, + int16_t& linearSpeedCenter, int16_t& angularSpeed); +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* DIFFERENTIAL_DRIVE_H */ +/** @} */ diff --git a/lib/Service/FPMath.cpp b/lib/Service/FPMath.cpp index a5f5d9a1..fed28805 100644 --- a/lib/Service/FPMath.cpp +++ b/lib/Service/FPMath.cpp @@ -1,75 +1,75 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Fixpoint math - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "FPMath.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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Fixpoint math + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "FPMath.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/Service/FPMath.h b/lib/Service/FPMath.h index 75df3989..5ac8d667 100644 --- a/lib/Service/FPMath.h +++ b/lib/Service/FPMath.h @@ -1,78 +1,78 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Fixpoint math - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef FPMATH_H -#define FPMATH_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/** Fixpoint PI [mrad] */ -#define FP_PI() (static_cast(1000.0f * PI)) - -/** Fixpoint 2 * PI [mrad] */ -#define FP_2PI() (static_cast(2000.0f * PI)) - -/** Fixpoint 4 * PI [mrad] */ -#define FP_4PI() (static_cast(4000.0f * PI)) - -/** Convert value from mrad to deg. */ -#define MRAD2DEG(__mrad) ((static_cast(__mrad) * static_cast(360)) / FP_2PI()) - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/****************************************************************************** - * Functions - *****************************************************************************/ - -namespace FPMath -{ - -} - -#endif /* FPMATH_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Fixpoint math + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef FPMATH_H +#define FPMATH_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/** Fixpoint PI [mrad] */ +#define FP_PI() (static_cast(1000.0f * PI)) + +/** Fixpoint 2 * PI [mrad] */ +#define FP_2PI() (static_cast(2000.0f * PI)) + +/** Fixpoint 4 * PI [mrad] */ +#define FP_4PI() (static_cast(4000.0f * PI)) + +/** Convert value from mrad to deg. */ +#define MRAD2DEG(__mrad) ((static_cast(__mrad) * static_cast(360)) / FP_2PI()) + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/****************************************************************************** + * Functions + *****************************************************************************/ + +namespace FPMath +{ + +} + +#endif /* FPMATH_H */ +/** @} */ diff --git a/lib/Service/IState.h b/lib/Service/IState.h index db0934e6..f2fce388 100644 --- a/lib/Service/IState.h +++ b/lib/Service/IState.h @@ -1,102 +1,102 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Abstract state interface - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef ISTATE_H -#define ISTATE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/* Forward declaration */ -class StateMachine; - -/** State interface */ -class IState -{ -public: - /** - * Default constructor. - */ - IState() - { - } - - /** - * Default destructor. - */ - virtual ~IState() - { - } - - /** - * If the state is entered, this method will called once. - */ - virtual void entry() = 0; - - /** - * Processing the state. - * - * @param[in] sm State machine, which is calling this state. - */ - virtual void process(StateMachine& sm) = 0; - - /** - * If the state is left, this method will be called once. - */ - virtual void exit() = 0; - -protected: -private: -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* ISTATE_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Abstract state interface + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef ISTATE_H +#define ISTATE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/* Forward declaration */ +class StateMachine; + +/** State interface */ +class IState +{ +public: + /** + * Default constructor. + */ + IState() + { + } + + /** + * Default destructor. + */ + virtual ~IState() + { + } + + /** + * If the state is entered, this method will called once. + */ + virtual void entry() = 0; + + /** + * Processing the state. + * + * @param[in] sm State machine, which is calling this state. + */ + virtual void process(StateMachine& sm) = 0; + + /** + * If the state is left, this method will be called once. + */ + virtual void exit() = 0; + +protected: +private: +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* ISTATE_H */ +/** @} */ diff --git a/lib/Service/Logging.cpp b/lib/Service/Logging.cpp index 4d5050c5..0ec6457d 100644 --- a/lib/Service/Logging.cpp +++ b/lib/Service/Logging.cpp @@ -1,163 +1,163 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Logging - * @author Gabryel Reyes - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "Logging.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/** - * Is logging enabled? - */ -static bool gIsLogEnabled = true; - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -bool Logging::isEnabled() -{ - return gIsLogEnabled; -} - -void Logging::enable() -{ - gIsLogEnabled = true; -} - -void Logging::disable() -{ - gIsLogEnabled = false; -} - -void Logging::printHead(const char* filename, int lineNumber, Logging::LogLevel level) -{ - if (true == isEnabled()) - { - const char* levelStr = "U"; - - switch (level) - { - case Logging::LOG_LEVEL_FATAL: - levelStr = "F"; - break; - - case Logging::LOG_LEVEL_ERROR: - levelStr = "E"; - break; - - case Logging::LOG_LEVEL_WARNING: - levelStr = "W"; - break; - - case Logging::LOG_LEVEL_INFO: - levelStr = "I"; - break; - - case Logging::LOG_LEVEL_DEBUG: - levelStr = "D"; - break; - - default: - break; - } - - Serial.print(static_cast(millis())); - Serial.print(" "); - Serial.print(levelStr); - Serial.print(" "); - Serial.print(filename); - Serial.print(":"); - Serial.print(lineNumber); - Serial.print(" "); - } -} - -void Logging::printMsg(const char* message) -{ - if (true == isEnabled()) - { - Serial.print(message); - } -} - -void Logging::printTail() -{ - if (true == isEnabled()) - { - Serial.print("\n"); - } -} - -void Logging::print(const char* filename, int lineNumber, Logging::LogLevel level, const char* message) -{ - if (true == isEnabled()) - { - printHead(filename, lineNumber, level); - Serial.println(message); - } -} - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Logging + * @author Gabryel Reyes + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "Logging.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/** + * Is logging enabled? + */ +static bool gIsLogEnabled = true; + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +bool Logging::isEnabled() +{ + return gIsLogEnabled; +} + +void Logging::enable() +{ + gIsLogEnabled = true; +} + +void Logging::disable() +{ + gIsLogEnabled = false; +} + +void Logging::printHead(const char* filename, int lineNumber, Logging::LogLevel level) +{ + if (true == isEnabled()) + { + const char* levelStr = "U"; + + switch (level) + { + case Logging::LOG_LEVEL_FATAL: + levelStr = "F"; + break; + + case Logging::LOG_LEVEL_ERROR: + levelStr = "E"; + break; + + case Logging::LOG_LEVEL_WARNING: + levelStr = "W"; + break; + + case Logging::LOG_LEVEL_INFO: + levelStr = "I"; + break; + + case Logging::LOG_LEVEL_DEBUG: + levelStr = "D"; + break; + + default: + break; + } + + Serial.print(static_cast(millis())); + Serial.print(" "); + Serial.print(levelStr); + Serial.print(" "); + Serial.print(filename); + Serial.print(":"); + Serial.print(lineNumber); + Serial.print(" "); + } +} + +void Logging::printMsg(const char* message) +{ + if (true == isEnabled()) + { + Serial.print(message); + } +} + +void Logging::printTail() +{ + if (true == isEnabled()) + { + Serial.print("\n"); + } +} + +void Logging::print(const char* filename, int lineNumber, Logging::LogLevel level, const char* message) +{ + if (true == isEnabled()) + { + printHead(filename, lineNumber, level); + Serial.println(message); + } +} + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/Service/Logging.h b/lib/Service/Logging.h index 1dabafcf..258569d4 100644 --- a/lib/Service/Logging.h +++ b/lib/Service/Logging.h @@ -1,348 +1,348 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Logging - * @author Gabryel Reyes - * - * @addtogroup Service - * - * @{ - */ - -#ifndef LOGGING_H -#define LOGGING_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -#ifndef LOG_FATAL_ENABLE -/** Enable/disable fatal log messages. */ -#define LOG_FATAL_ENABLE (1) -#endif /* LOG_FATAL_ENABLE */ - -#ifndef LOG_ERROR_ENABLE -/** Enable/disable error log messages. */ -#define LOG_ERROR_ENABLE (1) -#endif /* LOG_ERROR_ENABLE */ - -#ifndef LOG_WARNING_ENABLE -/** Enable/disable warning log messages. */ -#define LOG_WARNING_ENABLE (1) -#endif /* LOG_WARNING_ENABLE */ - -#ifndef LOG_INFO_ENABLE -/** Enable/disable info log messages. */ -#define LOG_INFO_ENABLE (1) -#endif /* LOG_INFO_ENABLE */ - -#ifndef LOG_DEBUG_ENABLE -/** Enable/disable debug log messages. */ -#define LOG_DEBUG_ENABLE (0) -#endif /* LOG_DEBUG_ENABLE */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -#if LOG_FATAL_ENABLE || LOG_ERROR_ENABLE || LOG_WARNING_ENABLE || LOG_INFO_ENABLE || LOG_DEBUG_ENABLE - -/** Define the logging tag to know in which file the log message is located. */ -#define LOG_TAG(_tag) static const char* LOG_TAG = _tag - -#else /* LOG_FATAL_ENABLE || LOG_ERROR_ENABLE || LOG_WARNING_ENABLE || LOG_INFO_ENABLE || LOG_DEBUG_ENABLE */ - -/** Define the logging tag to know in which file the log message is located. */ -#define LOG_TAG(_tag) - -#endif /* LOG_FATAL_ENABLE || LOG_ERROR_ENABLE || LOG_WARNING_ENABLE || LOG_INFO_ENABLE || LOG_DEBUG_ENABLE */ - -#if (0 == LOG_FATAL_ENABLE) - -/** Log fatal error message. */ -#define LOG_FATAL(_msg) -/** Log fatal error message with additional value. */ -#define LOG_FATAL_VAL(_msg, _val) -/** Log fatal error header. */ -#define LOG_FATAL_HEAD() -/** Log fatal error message, without line feed. */ -#define LOG_FATAL_MSG(_msg) -/** Log fatal error tail. */ -#define LOG_FATAL_TAIL() - -#else /* (0 == LOG_FATAL_ENABLE) */ - -/** Log fatal error message. */ -#define LOG_FATAL(_msg) Logging::print(LOG_TAG, __LINE__, Logging::LOG_LEVEL_FATAL, (_msg)) - -/** Log fatal error message with additional value. */ -#define LOG_FATAL_VAL(_msg, _val) Logging::print<_val>(LOG_TAG, __LINE__, Logging::LOG_LEVEL_FATAL, (_msg), (_val)) - -/** Log fatal error header. */ -#define LOG_FATAL_HEAD() Logging::printHead(LOG_TAG, __LINE__, Logging::LOG_LEVEL_FATAL) - -/** Log fatal error message, without line feed. */ -#define LOG_FATAL_MSG(_msg) Logging::printMsg(_msg) - -/** Log fatal error tail. */ -#define LOG_FATAL_TAIL() Logging::printTail() - -#endif /* (0 == LOG_FATAL_ENABLE) */ - -#if (0 == LOG_ERROR_ENABLE) - -/** Log error message. */ -#define LOG_ERROR(_msg) -/** Log error message with additional value. */ -#define LOG_ERROR_VAL(_msg, _val) -/** Log error header. */ -#define LOG_ERROR_HEAD() -/** Log error error message, without line feed. */ -#define LOG_ERROR_MSG(_msg) -/** Log error tail. */ -#define LOG_ERROR_TAIL() - -#else /* (0 == LOG_ERROR_ENABLE) */ - -/** Log error message. */ -#define LOG_ERROR(_msg) Logging::print(LOG_TAG, __LINE__, Logging::LOG_LEVEL_ERROR, (_msg)) - -/** Log error message with additional value. */ -#define LOG_ERROR_VAL(_msg, _val) Logging::print(LOG_TAG, __LINE__, Logging::LOG_LEVEL_ERROR, (_msg), (_val)) - -/** Log error header. */ -#define LOG_ERROR_HEAD() Logging::printHead(LOG_TAG, __LINE__, Logging::LOG_LEVEL_ERROR) - -/** Log error error message, without line feed. */ -#define LOG_ERROR_MSG(_msg) Logging::printMsg(_msg) - -/** Log error tail. */ -#define LOG_ERROR_TAIL() Logging::printTail() - -#endif /* (0 == LOG_ERROR_ENABLE) */ - -#if (0 == LOG_WARNING_ENABLE) - -/** Log warning message. */ -#define LOG_WARNING(_msg) -/** Log warning message with additional value. */ -#define LOG_WARNING_VAL(_msg, _val) -/** Log warning header. */ -#define LOG_WARNING_HEAD() -/** Log warning message, without line feed. */ -#define LOG_WARNING_MSG(_msg) -/** Log warning tail. */ -#define LOG_WARNING_TAIL() - -#else /* (0 == LOG_WARNING_ENABLE) */ - -/** Log warning message. */ -#define LOG_WARNING(_msg) Logging::print(LOG_TAG, __LINE__, Logging::LOG_LEVEL_WARNING, (_msg)) - -/** Log warning message with additional value. */ -#define LOG_WARNING_VAL(_msg, _val) Logging::print(LOG_TAG, __LINE__, Logging::LOG_LEVEL_WARNING, (_msg), (_val)) - -/** Log warning header. */ -#define LOG_WARNING_HEAD() Logging::printHead(LOG_TAG, __LINE__, Logging::LOG_LEVEL_WARNING) - -/** Log warning message, without line feed. */ -#define LOG_WARNING_MSG(_msg) Logging::printMsg(_msg) - -/** Log warning tail. */ -#define LOG_WARNING_TAIL() Logging::printTail() - -#endif /* (0 == LOG_WARNING_ENABLE) */ - -#if (0 == LOG_INFO_ENABLE) - -/** Log info message. */ -#define LOG_INFO(_msg) -/** Log info message with additional value. */ -#define LOG_INFO_VAL(_msg, _val) -/** Log info header. */ -#define LOG_INFO_HEAD() -/** Log info message, without line feed. */ -#define LOG_INFO_MSG(_msg) -/** Log info tail. */ -#define LOG_INFO_TAIL() - -#else /* (0 == LOG_INFO_ENABLE) */ - -/** Log info message. */ -#define LOG_INFO(_msg) Logging::print(LOG_TAG, __LINE__, Logging::LOG_LEVEL_INFO, (_msg)) - -/** Log info message with additional value. */ -#define LOG_INFO_VAL(_msg, _val) Logging::print(LOG_TAG, __LINE__, Logging::LOG_LEVEL_INFO, (_msg), (_val)) - -/** Log info header. */ -#define LOG_INFO_HEAD() Logging::printHead(LOG_TAG, __LINE__, Logging::LOG_LEVEL_INFO) - -/** Log info message, without line feed. */ -#define LOG_INFO_MSG(_msg) Logging::printMsg(_msg) - -/** Log info tail. */ -#define LOG_INFO_TAIL() Logging::printTail() - -#endif /* (0 == LOG_INFO_ENABLE) */ - -#if (0 == LOG_DEBUG_ENABLE) - -/** Log debug message. */ -#define LOG_DEBUG(_msg) -/** Log debug message with additional value. */ -#define LOG_DEBUG_VAL(_msg, _val) -/** Log debug header. */ -#define LOG_DEBUG_HEAD() -/** Log debug message, without line feed. */ -#define LOG_DEBUG_MSG(_msg) -/** Log debug tail. */ -#define LOG_DEBUG_TAIL() - -#else /* (0 == LOG_DEBUG_ENABLE) */ - -/** Log debug message. */ -#define LOG_DEBUG(_msg) Logging::print(LOG_TAG, __LINE__, Logging::LOG_LEVEL_DEBUG, (_msg)) - -/** Log debug message with additional value. */ -#define LOG_DEBUG_VAL(_msg, _val) Logging::print(LOG_TAG, __LINE__, Logging::LOG_LEVEL_DEBUG, (_msg), (_val)) - -/** Log debug header. */ -#define LOG_DEBUG_HEAD() Logging::printHead(LOG_TAG, __LINE__, Logging::LOG_LEVEL_DEBUG) - -/** Log debug message, without line feed. */ -#define LOG_DEBUG_MSG(_msg) Logging::printMsg(_msg) - -/** Log debug tail. */ -#define LOG_DEBUG_TAIL() Logging::printTail() - -#endif /* (0 == LOG_DEBUG_ENABLE) */ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** - * Logging class for log messages depending on the previously set log level. - */ -namespace Logging -{ - /** - * Enumeration to distinguish between different levels of severity. - */ - enum LogLevel - { - LOG_LEVEL_FATAL = - 0, /**< Any error that is forcing a shutdown of service or application, because there is no way out. */ - LOG_LEVEL_ERROR, /**< Any error that is fatal for the operating, but not for the service or application. */ - LOG_LEVEL_WARNING, /**< Anything that shows the user to pay attention, but can be automatically be recovered. */ - LOG_LEVEL_INFO, /**< General useful information for the user. */ - LOG_LEVEL_DEBUG, /**< A diagnostic message helpful for the developer. */ - }; - - /** - * Is logging enabled? - * - * @return If enabled, it will return true otherwise false. - */ - bool isEnabled(); - - /** - * Enable logging. - */ - void enable(); - - /** - * Disable logging. - */ - void disable(); - - /** - * Print log message header. - * Use printMsg() to log the message itself. - * - * @param[in] filename The name of the file, where the log message is located. - * @param[in] lineNumber The line number in the file, where the log message is located. - * @param[in] level The severity level. - */ - void printHead(const char* filename, int lineNumber, LogLevel level); - - /** - * Print message without line feed. - * It can be used several times to concatenate a message without additional buffering. - * - * @param[in] message The message itself. - */ - void printMsg(const char* message); - - /** - * Print tail of log message. - */ - void printTail(); - - /** - * Print log message. - * - * @param[in] filename The name of the file, where the log message is located. - * @param[in] lineNumber The line number in the file, where the log message is located. - * @param[in] level The severity level. - * @param[in] message The message itself. - */ - void print(const char* filename, int lineNumber, LogLevel level, const char* message); - - /** - * Print log message. - * - * @param[in] filename The name of the file, where the log message is located. - * @param[in] lineNumber The line number in the file, where the log message is located. - * @param[in] level The severity level. - * @param[in] message The message itself. - * @param[in] value The value to print after the message. - */ - template - void print(const char* filename, int lineNumber, LogLevel level, const char* message, T value) - { - if (true == isEnabled()) - { - printHead(filename, lineNumber, level); - Serial.print(message); - Serial.println(value); - } - } - -}; /* namespace Logging */ - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* __LOGGING_H__ */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Logging + * @author Gabryel Reyes + * + * @addtogroup Service + * + * @{ + */ + +#ifndef LOGGING_H +#define LOGGING_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +#ifndef LOG_FATAL_ENABLE +/** Enable/disable fatal log messages. */ +#define LOG_FATAL_ENABLE (1) +#endif /* LOG_FATAL_ENABLE */ + +#ifndef LOG_ERROR_ENABLE +/** Enable/disable error log messages. */ +#define LOG_ERROR_ENABLE (1) +#endif /* LOG_ERROR_ENABLE */ + +#ifndef LOG_WARNING_ENABLE +/** Enable/disable warning log messages. */ +#define LOG_WARNING_ENABLE (1) +#endif /* LOG_WARNING_ENABLE */ + +#ifndef LOG_INFO_ENABLE +/** Enable/disable info log messages. */ +#define LOG_INFO_ENABLE (1) +#endif /* LOG_INFO_ENABLE */ + +#ifndef LOG_DEBUG_ENABLE +/** Enable/disable debug log messages. */ +#define LOG_DEBUG_ENABLE (0) +#endif /* LOG_DEBUG_ENABLE */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +#if LOG_FATAL_ENABLE || LOG_ERROR_ENABLE || LOG_WARNING_ENABLE || LOG_INFO_ENABLE || LOG_DEBUG_ENABLE + +/** Define the logging tag to know in which file the log message is located. */ +#define LOG_TAG(_tag) static const char* LOG_TAG = _tag + +#else /* LOG_FATAL_ENABLE || LOG_ERROR_ENABLE || LOG_WARNING_ENABLE || LOG_INFO_ENABLE || LOG_DEBUG_ENABLE */ + +/** Define the logging tag to know in which file the log message is located. */ +#define LOG_TAG(_tag) + +#endif /* LOG_FATAL_ENABLE || LOG_ERROR_ENABLE || LOG_WARNING_ENABLE || LOG_INFO_ENABLE || LOG_DEBUG_ENABLE */ + +#if (0 == LOG_FATAL_ENABLE) + +/** Log fatal error message. */ +#define LOG_FATAL(_msg) +/** Log fatal error message with additional value. */ +#define LOG_FATAL_VAL(_msg, _val) +/** Log fatal error header. */ +#define LOG_FATAL_HEAD() +/** Log fatal error message, without line feed. */ +#define LOG_FATAL_MSG(_msg) +/** Log fatal error tail. */ +#define LOG_FATAL_TAIL() + +#else /* (0 == LOG_FATAL_ENABLE) */ + +/** Log fatal error message. */ +#define LOG_FATAL(_msg) Logging::print(LOG_TAG, __LINE__, Logging::LOG_LEVEL_FATAL, (_msg)) + +/** Log fatal error message with additional value. */ +#define LOG_FATAL_VAL(_msg, _val) Logging::print<_val>(LOG_TAG, __LINE__, Logging::LOG_LEVEL_FATAL, (_msg), (_val)) + +/** Log fatal error header. */ +#define LOG_FATAL_HEAD() Logging::printHead(LOG_TAG, __LINE__, Logging::LOG_LEVEL_FATAL) + +/** Log fatal error message, without line feed. */ +#define LOG_FATAL_MSG(_msg) Logging::printMsg(_msg) + +/** Log fatal error tail. */ +#define LOG_FATAL_TAIL() Logging::printTail() + +#endif /* (0 == LOG_FATAL_ENABLE) */ + +#if (0 == LOG_ERROR_ENABLE) + +/** Log error message. */ +#define LOG_ERROR(_msg) +/** Log error message with additional value. */ +#define LOG_ERROR_VAL(_msg, _val) +/** Log error header. */ +#define LOG_ERROR_HEAD() +/** Log error error message, without line feed. */ +#define LOG_ERROR_MSG(_msg) +/** Log error tail. */ +#define LOG_ERROR_TAIL() + +#else /* (0 == LOG_ERROR_ENABLE) */ + +/** Log error message. */ +#define LOG_ERROR(_msg) Logging::print(LOG_TAG, __LINE__, Logging::LOG_LEVEL_ERROR, (_msg)) + +/** Log error message with additional value. */ +#define LOG_ERROR_VAL(_msg, _val) Logging::print(LOG_TAG, __LINE__, Logging::LOG_LEVEL_ERROR, (_msg), (_val)) + +/** Log error header. */ +#define LOG_ERROR_HEAD() Logging::printHead(LOG_TAG, __LINE__, Logging::LOG_LEVEL_ERROR) + +/** Log error error message, without line feed. */ +#define LOG_ERROR_MSG(_msg) Logging::printMsg(_msg) + +/** Log error tail. */ +#define LOG_ERROR_TAIL() Logging::printTail() + +#endif /* (0 == LOG_ERROR_ENABLE) */ + +#if (0 == LOG_WARNING_ENABLE) + +/** Log warning message. */ +#define LOG_WARNING(_msg) +/** Log warning message with additional value. */ +#define LOG_WARNING_VAL(_msg, _val) +/** Log warning header. */ +#define LOG_WARNING_HEAD() +/** Log warning message, without line feed. */ +#define LOG_WARNING_MSG(_msg) +/** Log warning tail. */ +#define LOG_WARNING_TAIL() + +#else /* (0 == LOG_WARNING_ENABLE) */ + +/** Log warning message. */ +#define LOG_WARNING(_msg) Logging::print(LOG_TAG, __LINE__, Logging::LOG_LEVEL_WARNING, (_msg)) + +/** Log warning message with additional value. */ +#define LOG_WARNING_VAL(_msg, _val) Logging::print(LOG_TAG, __LINE__, Logging::LOG_LEVEL_WARNING, (_msg), (_val)) + +/** Log warning header. */ +#define LOG_WARNING_HEAD() Logging::printHead(LOG_TAG, __LINE__, Logging::LOG_LEVEL_WARNING) + +/** Log warning message, without line feed. */ +#define LOG_WARNING_MSG(_msg) Logging::printMsg(_msg) + +/** Log warning tail. */ +#define LOG_WARNING_TAIL() Logging::printTail() + +#endif /* (0 == LOG_WARNING_ENABLE) */ + +#if (0 == LOG_INFO_ENABLE) + +/** Log info message. */ +#define LOG_INFO(_msg) +/** Log info message with additional value. */ +#define LOG_INFO_VAL(_msg, _val) +/** Log info header. */ +#define LOG_INFO_HEAD() +/** Log info message, without line feed. */ +#define LOG_INFO_MSG(_msg) +/** Log info tail. */ +#define LOG_INFO_TAIL() + +#else /* (0 == LOG_INFO_ENABLE) */ + +/** Log info message. */ +#define LOG_INFO(_msg) Logging::print(LOG_TAG, __LINE__, Logging::LOG_LEVEL_INFO, (_msg)) + +/** Log info message with additional value. */ +#define LOG_INFO_VAL(_msg, _val) Logging::print(LOG_TAG, __LINE__, Logging::LOG_LEVEL_INFO, (_msg), (_val)) + +/** Log info header. */ +#define LOG_INFO_HEAD() Logging::printHead(LOG_TAG, __LINE__, Logging::LOG_LEVEL_INFO) + +/** Log info message, without line feed. */ +#define LOG_INFO_MSG(_msg) Logging::printMsg(_msg) + +/** Log info tail. */ +#define LOG_INFO_TAIL() Logging::printTail() + +#endif /* (0 == LOG_INFO_ENABLE) */ + +#if (0 == LOG_DEBUG_ENABLE) + +/** Log debug message. */ +#define LOG_DEBUG(_msg) +/** Log debug message with additional value. */ +#define LOG_DEBUG_VAL(_msg, _val) +/** Log debug header. */ +#define LOG_DEBUG_HEAD() +/** Log debug message, without line feed. */ +#define LOG_DEBUG_MSG(_msg) +/** Log debug tail. */ +#define LOG_DEBUG_TAIL() + +#else /* (0 == LOG_DEBUG_ENABLE) */ + +/** Log debug message. */ +#define LOG_DEBUG(_msg) Logging::print(LOG_TAG, __LINE__, Logging::LOG_LEVEL_DEBUG, (_msg)) + +/** Log debug message with additional value. */ +#define LOG_DEBUG_VAL(_msg, _val) Logging::print(LOG_TAG, __LINE__, Logging::LOG_LEVEL_DEBUG, (_msg), (_val)) + +/** Log debug header. */ +#define LOG_DEBUG_HEAD() Logging::printHead(LOG_TAG, __LINE__, Logging::LOG_LEVEL_DEBUG) + +/** Log debug message, without line feed. */ +#define LOG_DEBUG_MSG(_msg) Logging::printMsg(_msg) + +/** Log debug tail. */ +#define LOG_DEBUG_TAIL() Logging::printTail() + +#endif /* (0 == LOG_DEBUG_ENABLE) */ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** + * Logging class for log messages depending on the previously set log level. + */ +namespace Logging +{ + /** + * Enumeration to distinguish between different levels of severity. + */ + enum LogLevel + { + LOG_LEVEL_FATAL = + 0, /**< Any error that is forcing a shutdown of service or application, because there is no way out. */ + LOG_LEVEL_ERROR, /**< Any error that is fatal for the operating, but not for the service or application. */ + LOG_LEVEL_WARNING, /**< Anything that shows the user to pay attention, but can be automatically be recovered. */ + LOG_LEVEL_INFO, /**< General useful information for the user. */ + LOG_LEVEL_DEBUG, /**< A diagnostic message helpful for the developer. */ + }; + + /** + * Is logging enabled? + * + * @return If enabled, it will return true otherwise false. + */ + bool isEnabled(); + + /** + * Enable logging. + */ + void enable(); + + /** + * Disable logging. + */ + void disable(); + + /** + * Print log message header. + * Use printMsg() to log the message itself. + * + * @param[in] filename The name of the file, where the log message is located. + * @param[in] lineNumber The line number in the file, where the log message is located. + * @param[in] level The severity level. + */ + void printHead(const char* filename, int lineNumber, LogLevel level); + + /** + * Print message without line feed. + * It can be used several times to concatenate a message without additional buffering. + * + * @param[in] message The message itself. + */ + void printMsg(const char* message); + + /** + * Print tail of log message. + */ + void printTail(); + + /** + * Print log message. + * + * @param[in] filename The name of the file, where the log message is located. + * @param[in] lineNumber The line number in the file, where the log message is located. + * @param[in] level The severity level. + * @param[in] message The message itself. + */ + void print(const char* filename, int lineNumber, LogLevel level, const char* message); + + /** + * Print log message. + * + * @param[in] filename The name of the file, where the log message is located. + * @param[in] lineNumber The line number in the file, where the log message is located. + * @param[in] level The severity level. + * @param[in] message The message itself. + * @param[in] value The value to print after the message. + */ + template + void print(const char* filename, int lineNumber, LogLevel level, const char* message, T value) + { + if (true == isEnabled()) + { + printHead(filename, lineNumber, level); + Serial.print(message); + Serial.println(value); + } + } + +}; /* namespace Logging */ + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* __LOGGING_H__ */ +/** @} */ diff --git a/lib/Service/MovAvg.hpp b/lib/Service/MovAvg.hpp index 85e8043e..79678b81 100644 --- a/lib/Service/MovAvg.hpp +++ b/lib/Service/MovAvg.hpp @@ -1,164 +1,164 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Moving average - * @author Andreas Merkle - * - * @addtogroup Service - * - * @{ - */ - -#ifndef MOVAVG_H -#define MOVAVG_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** - * This class implements a moving average algorithm. - * It is designed for fix point integers. - * - * @tparam T The data type of the moving average result and input values. - * @tparam length The number of values, which are considered in the moving average calculation. - */ -template -class MovAvg -{ -public: - /** - * Constructs the moving average calculator. - * The default result will be 0. - */ - MovAvg() : m_values(), m_wrIdx(0), m_written(0), m_sum(0) - { - clear(); - } - - /** - * Destroys the moving average calculator. - */ - ~MovAvg() - { - } - - /** - * Clears the internal list of values. - */ - void clear() - { - uint8_t idx = 0; - - for (idx = 0; idx < length; ++idx) - { - m_values[idx] = 0; - } - - m_wrIdx = 0; - m_written = 0; - m_sum = 0; - } - - /** - * Write a value to the moving average calculator and returns the new - * result. - * - * @param[in] value New value, which shall be considered. - * - * @return Moving average result - */ - T write(T value) - { - T oldValue = m_values[m_wrIdx]; - - m_values[m_wrIdx] = value; - - ++m_wrIdx; - if (length <= m_wrIdx) - { - m_wrIdx = 0; - } - - m_sum -= oldValue; - m_sum += value; - - if (length > m_written) - { - ++m_written; - } - - return m_sum / m_written; - } - - /** - * Get current moving average result. - * - * @return Moving average result - */ - T getResult() const - { - T result = 0; - - if (0 < m_written) - { - result = m_sum / m_written; - } - - return result; - } - -private: - T m_values[length]; /**< List of values, used for moving average calculation. */ - uint8_t m_wrIdx; /**< Write index to list of values */ - uint8_t m_written; /**< The number of written values to the list of values, till length is reached. */ - T m_sum; /**< Sum of all values */ - - /* Not allowed. */ - MovAvg(const MovAvg& avg); /**< Copy construction of an instance. */ - MovAvg& operator=(const MovAvg& avg); /**< Assignment of an instance. */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* MOVAVG_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Moving average + * @author Andreas Merkle + * + * @addtogroup Service + * + * @{ + */ + +#ifndef MOVAVG_H +#define MOVAVG_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** + * This class implements a moving average algorithm. + * It is designed for fix point integers. + * + * @tparam T The data type of the moving average result and input values. + * @tparam length The number of values, which are considered in the moving average calculation. + */ +template +class MovAvg +{ +public: + /** + * Constructs the moving average calculator. + * The default result will be 0. + */ + MovAvg() : m_values(), m_wrIdx(0), m_written(0), m_sum(0) + { + clear(); + } + + /** + * Destroys the moving average calculator. + */ + ~MovAvg() + { + } + + /** + * Clears the internal list of values. + */ + void clear() + { + uint8_t idx = 0; + + for (idx = 0; idx < length; ++idx) + { + m_values[idx] = 0; + } + + m_wrIdx = 0; + m_written = 0; + m_sum = 0; + } + + /** + * Write a value to the moving average calculator and returns the new + * result. + * + * @param[in] value New value, which shall be considered. + * + * @return Moving average result + */ + T write(T value) + { + T oldValue = m_values[m_wrIdx]; + + m_values[m_wrIdx] = value; + + ++m_wrIdx; + if (length <= m_wrIdx) + { + m_wrIdx = 0; + } + + m_sum -= oldValue; + m_sum += value; + + if (length > m_written) + { + ++m_written; + } + + return m_sum / m_written; + } + + /** + * Get current moving average result. + * + * @return Moving average result + */ + T getResult() const + { + T result = 0; + + if (0 < m_written) + { + result = m_sum / m_written; + } + + return result; + } + +private: + T m_values[length]; /**< List of values, used for moving average calculation. */ + uint8_t m_wrIdx; /**< Write index to list of values */ + uint8_t m_written; /**< The number of written values to the list of values, till length is reached. */ + T m_sum; /**< Sum of all values */ + + /* Not allowed. */ + MovAvg(const MovAvg& avg); /**< Copy construction of an instance. */ + MovAvg& operator=(const MovAvg& avg); /**< Assignment of an instance. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* MOVAVG_H */ +/** @} */ diff --git a/lib/Service/Odometry.cpp b/lib/Service/Odometry.cpp index 1da5e8db..5544ca19 100644 --- a/lib/Service/Odometry.cpp +++ b/lib/Service/Odometry.cpp @@ -1,274 +1,274 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Odometry - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/* Initialize static constant data. */ -const uint16_t Odometry::STEPS_THRESHOLD = static_cast(RobotConstants::ENCODER_STEPS_PER_M / 100U); - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void Odometry::process() -{ - int32_t relStepsLeft = m_relEncoders.getCountsLeft(); /* [steps] */ - int32_t relStepsRight = m_relEncoders.getCountsRight(); /* [steps] */ - uint16_t absStepsLeft = abs(relStepsLeft); /* Positive amount of delta steps left */ - uint16_t absStepsRight = abs(relStepsRight); /* Positive amount of delta steps right*/ - bool isNoMovement = detectStandStill(absStepsLeft, absStepsRight); - - /* Orientation shall not be calculated from stand still to driving, - * because there can not be any orientation change before. - */ - if (false == isNoMovement) - { - /* The calculation of the orientation/movement depends on the driven distance. - * If a distance threshold is exceeded, it will be calculated. - */ - if ((STEPS_THRESHOLD <= absStepsLeft) || (STEPS_THRESHOLD <= absStepsRight)) - { - int16_t stepsCenter = static_cast((relStepsLeft + relStepsRight) / 2); /* [steps] */ - int16_t dXSteps = 0; /* [steps] */ - int16_t dYSteps = 0; /* [steps] */ - int32_t deltaPosX = 0; /* [mm] */ - int32_t deltaPosY = 0; /* [mm] */ - - /* Calculate mileage in steps to avoid loosing precision by division. */ - m_mileage = calculateMileage(m_mileage, stepsCenter); - - m_orientation = calculateOrientation(m_orientation, relStepsLeft, relStepsRight); - - /* Calculate delta position in steps to avoid loosing precision by divison. */ - calculateDeltaPos(stepsCenter, m_orientation, dXSteps, dYSteps); - m_countingXSteps += dXSteps * 1000; /* Multiply with 1000 for higher precision. */ - m_countingYSteps += dYSteps * 1000; /* Multiply with 1000 for higher precision. */ - - /* For large areas, its important to have the position in mm and not in steps. - * Therefore the position in mm is continously calculated from the counted steps - * on each axis. - */ - deltaPosX = Util::divRoundUp(m_countingXSteps, static_cast(RobotConstants::ENCODER_STEPS_PER_M)); - deltaPosY = Util::divRoundUp(m_countingYSteps, static_cast(RobotConstants::ENCODER_STEPS_PER_M)); - - m_posX += deltaPosX; - m_countingXSteps -= deltaPosX * static_cast(RobotConstants::ENCODER_STEPS_PER_M); - - m_posY += deltaPosY; - m_countingYSteps -= deltaPosY * static_cast(RobotConstants::ENCODER_STEPS_PER_M); - - /* Reset to be able to calculate the next delta. */ - absStepsLeft = 0U; /* [steps] */ - absStepsRight = 0U; /* [steps] */ - m_relEncoders.clear(); - } - } - m_lastAbsRelEncStepsLeft = absStepsLeft; /* [steps] */ - m_lastAbsRelEncStepsRight = absStepsRight; /* [steps] */ -} - -uint32_t Odometry::getMileageCenter() const -{ - int16_t relStepsLeft = m_relEncoders.getCountsLeft(); /* [steps] */ - int16_t relStepsRight = m_relEncoders.getCountsRight(); /* [steps] */ - int16_t stepsCenter = (relStepsLeft + relStepsRight) / 2; /* [steps] */ - uint32_t mileage = 1000U * calculateMileage(m_mileage, stepsCenter); /* 1000 * [steps] */ - uint32_t mileageMM = Util::divRoundUp(mileage, RobotConstants::ENCODER_STEPS_PER_M); /* [mm] */ - - return mileageMM; -} - -int32_t Odometry::getOrientation() const -{ - int16_t relStepsLeft = m_relEncoders.getCountsLeft(); /* [steps] */ - int16_t relStepsRight = m_relEncoders.getCountsRight(); /* [steps] */ - - /* For higher accuracy use the current relative steps left and right. - * The m_orientation will only be updated every STEPS_THRESHOLD, which - * will reset the relative encoders to 0. - */ - return calculateOrientation(m_orientation, relStepsLeft, relStepsRight); -} - -void Odometry::clearPosition() -{ - m_relEncoders.clear(); - m_lastAbsRelEncStepsLeft = 0; - m_lastAbsRelEncStepsRight = 0; - m_posX = 0; - m_posY = 0; - m_countingXSteps = 0; - m_countingYSteps = 0; -} - -void Odometry::clearMileage() -{ - m_mileage = 0; -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -bool Odometry::detectStandStill(uint16_t absStepsLeft, uint16_t absStepsRight) -{ - bool isStandStill = false; - - /* No encoder (left/right) change detected? */ - if (absStepsLeft == m_lastAbsRelEncStepsLeft) - { - if (absStepsRight == m_lastAbsRelEncStepsRight) - { - isStandStill = true; - } - } - - /* Is robot moving? */ - if (false == isStandStill) - { - m_isStandstill = false; - m_timer.stop(); - } - /* Robot seems to not to move anymore, lets debounce. */ - else if (false == m_isStandstill) - { - /* The first time of no movement, start the debounce timer. */ - if (false == m_timer.isRunning()) - { - m_timer.start(STANDSTILL_DETECTION_PERIOD); - } - /* If over the while debounce time there is no movement, it will be considered as standstill. */ - else if (true == m_timer.isTimeout()) - { - m_isStandstill = true; - } - } - /* Robot standstill. */ - else - { - /* Nothing to do. */ - ; - } - - return m_isStandstill; -} - -int32_t Odometry::calculateMileage(uint32_t mileage, int16_t stepsCenter) const -{ - return mileage + static_cast(abs(stepsCenter)); -} - -int32_t Odometry::calculateOrientation(int32_t orientation, int16_t stepsLeft, int16_t stepsRight) const -{ - /* The alpha is approximated for performance reason. */ - int32_t stepsLeft32 = static_cast(stepsLeft); - int32_t stepsRight32 = static_cast(stepsRight); - int32_t alpha = (stepsRight32 - stepsLeft32) * 1000; /* 1000 * [steps] */ - - alpha = Util::divRoundUp(alpha, static_cast(RobotConstants::ENCODER_STEPS_PER_M)); /* 1000 * [m] */ - alpha *= 1000; /* 1000 * [mm] */ - alpha = Util::divRoundUp(alpha, static_cast(RobotConstants::WHEEL_BASE)); /* [mrad] */ - - /* Calculate orientation */ - orientation += alpha; - orientation %= FP_2PI(); /* -2*PI < orientation < +2*PI */ - - return orientation; -} - -void Odometry::calculateDeltaPos(int16_t stepsCenter, int32_t orientation, int16_t& dXSteps, int16_t& dYSteps) const -{ - float fDistCenter = static_cast(stepsCenter); /* [steps] */ - float fOrientation = static_cast(orientation) / 1000.0F; /* [rad] */ - float fDeltaPosX = fDistCenter * cosf(fOrientation); /* [steps] */ - float fDeltaPosY = fDistCenter * sinf(fOrientation); /* [steps] */ - - /* Round because the cast will just cut the fractional part. */ - if (0.0F <= fDeltaPosX) - { - fDeltaPosX += 0.5F; - } - else - { - fDeltaPosX -= 0.5F; - } - - /* Round because the cast will just cut the fractional part. */ - if (0.0F <= fDeltaPosY) - { - fDeltaPosY += 0.5F; - } - else - { - fDeltaPosY -= 0.5F; - } - - dXSteps = static_cast(fDeltaPosX); /* [steps] */ - dYSteps = static_cast(fDeltaPosY); /* [steps] */ -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Odometry + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/* Initialize static constant data. */ +const uint16_t Odometry::STEPS_THRESHOLD = static_cast(RobotConstants::ENCODER_STEPS_PER_M / 100U); + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void Odometry::process() +{ + int32_t relStepsLeft = m_relEncoders.getCountsLeft(); /* [steps] */ + int32_t relStepsRight = m_relEncoders.getCountsRight(); /* [steps] */ + uint16_t absStepsLeft = abs(relStepsLeft); /* Positive amount of delta steps left */ + uint16_t absStepsRight = abs(relStepsRight); /* Positive amount of delta steps right*/ + bool isNoMovement = detectStandStill(absStepsLeft, absStepsRight); + + /* Orientation shall not be calculated from stand still to driving, + * because there can not be any orientation change before. + */ + if (false == isNoMovement) + { + /* The calculation of the orientation/movement depends on the driven distance. + * If a distance threshold is exceeded, it will be calculated. + */ + if ((STEPS_THRESHOLD <= absStepsLeft) || (STEPS_THRESHOLD <= absStepsRight)) + { + int16_t stepsCenter = static_cast((relStepsLeft + relStepsRight) / 2); /* [steps] */ + int16_t dXSteps = 0; /* [steps] */ + int16_t dYSteps = 0; /* [steps] */ + int32_t deltaPosX = 0; /* [mm] */ + int32_t deltaPosY = 0; /* [mm] */ + + /* Calculate mileage in steps to avoid loosing precision by division. */ + m_mileage = calculateMileage(m_mileage, stepsCenter); + + m_orientation = calculateOrientation(m_orientation, relStepsLeft, relStepsRight); + + /* Calculate delta position in steps to avoid loosing precision by divison. */ + calculateDeltaPos(stepsCenter, m_orientation, dXSteps, dYSteps); + m_countingXSteps += dXSteps * 1000; /* Multiply with 1000 for higher precision. */ + m_countingYSteps += dYSteps * 1000; /* Multiply with 1000 for higher precision. */ + + /* For large areas, its important to have the position in mm and not in steps. + * Therefore the position in mm is continously calculated from the counted steps + * on each axis. + */ + deltaPosX = Util::divRoundUp(m_countingXSteps, static_cast(RobotConstants::ENCODER_STEPS_PER_M)); + deltaPosY = Util::divRoundUp(m_countingYSteps, static_cast(RobotConstants::ENCODER_STEPS_PER_M)); + + m_posX += deltaPosX; + m_countingXSteps -= deltaPosX * static_cast(RobotConstants::ENCODER_STEPS_PER_M); + + m_posY += deltaPosY; + m_countingYSteps -= deltaPosY * static_cast(RobotConstants::ENCODER_STEPS_PER_M); + + /* Reset to be able to calculate the next delta. */ + absStepsLeft = 0U; /* [steps] */ + absStepsRight = 0U; /* [steps] */ + m_relEncoders.clear(); + } + } + m_lastAbsRelEncStepsLeft = absStepsLeft; /* [steps] */ + m_lastAbsRelEncStepsRight = absStepsRight; /* [steps] */ +} + +uint32_t Odometry::getMileageCenter() const +{ + int16_t relStepsLeft = m_relEncoders.getCountsLeft(); /* [steps] */ + int16_t relStepsRight = m_relEncoders.getCountsRight(); /* [steps] */ + int16_t stepsCenter = (relStepsLeft + relStepsRight) / 2; /* [steps] */ + uint32_t mileage = 1000U * calculateMileage(m_mileage, stepsCenter); /* 1000 * [steps] */ + uint32_t mileageMM = Util::divRoundUp(mileage, RobotConstants::ENCODER_STEPS_PER_M); /* [mm] */ + + return mileageMM; +} + +int32_t Odometry::getOrientation() const +{ + int16_t relStepsLeft = m_relEncoders.getCountsLeft(); /* [steps] */ + int16_t relStepsRight = m_relEncoders.getCountsRight(); /* [steps] */ + + /* For higher accuracy use the current relative steps left and right. + * The m_orientation will only be updated every STEPS_THRESHOLD, which + * will reset the relative encoders to 0. + */ + return calculateOrientation(m_orientation, relStepsLeft, relStepsRight); +} + +void Odometry::clearPosition() +{ + m_relEncoders.clear(); + m_lastAbsRelEncStepsLeft = 0; + m_lastAbsRelEncStepsRight = 0; + m_posX = 0; + m_posY = 0; + m_countingXSteps = 0; + m_countingYSteps = 0; +} + +void Odometry::clearMileage() +{ + m_mileage = 0; +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +bool Odometry::detectStandStill(uint16_t absStepsLeft, uint16_t absStepsRight) +{ + bool isStandStill = false; + + /* No encoder (left/right) change detected? */ + if (absStepsLeft == m_lastAbsRelEncStepsLeft) + { + if (absStepsRight == m_lastAbsRelEncStepsRight) + { + isStandStill = true; + } + } + + /* Is robot moving? */ + if (false == isStandStill) + { + m_isStandstill = false; + m_timer.stop(); + } + /* Robot seems to not to move anymore, lets debounce. */ + else if (false == m_isStandstill) + { + /* The first time of no movement, start the debounce timer. */ + if (false == m_timer.isRunning()) + { + m_timer.start(STANDSTILL_DETECTION_PERIOD); + } + /* If over the while debounce time there is no movement, it will be considered as standstill. */ + else if (true == m_timer.isTimeout()) + { + m_isStandstill = true; + } + } + /* Robot standstill. */ + else + { + /* Nothing to do. */ + ; + } + + return m_isStandstill; +} + +int32_t Odometry::calculateMileage(uint32_t mileage, int16_t stepsCenter) const +{ + return mileage + static_cast(abs(stepsCenter)); +} + +int32_t Odometry::calculateOrientation(int32_t orientation, int16_t stepsLeft, int16_t stepsRight) const +{ + /* The alpha is approximated for performance reason. */ + int32_t stepsLeft32 = static_cast(stepsLeft); + int32_t stepsRight32 = static_cast(stepsRight); + int32_t alpha = (stepsRight32 - stepsLeft32) * 1000; /* 1000 * [steps] */ + + alpha = Util::divRoundUp(alpha, static_cast(RobotConstants::ENCODER_STEPS_PER_M)); /* 1000 * [m] */ + alpha *= 1000; /* 1000 * [mm] */ + alpha = Util::divRoundUp(alpha, static_cast(RobotConstants::WHEEL_BASE)); /* [mrad] */ + + /* Calculate orientation */ + orientation += alpha; + orientation %= FP_2PI(); /* -2*PI < orientation < +2*PI */ + + return orientation; +} + +void Odometry::calculateDeltaPos(int16_t stepsCenter, int32_t orientation, int16_t& dXSteps, int16_t& dYSteps) const +{ + float fDistCenter = static_cast(stepsCenter); /* [steps] */ + float fOrientation = static_cast(orientation) / 1000.0F; /* [rad] */ + float fDeltaPosX = fDistCenter * cosf(fOrientation); /* [steps] */ + float fDeltaPosY = fDistCenter * sinf(fOrientation); /* [steps] */ + + /* Round because the cast will just cut the fractional part. */ + if (0.0F <= fDeltaPosX) + { + fDeltaPosX += 0.5F; + } + else + { + fDeltaPosX -= 0.5F; + } + + /* Round because the cast will just cut the fractional part. */ + if (0.0F <= fDeltaPosY) + { + fDeltaPosY += 0.5F; + } + else + { + fDeltaPosY -= 0.5F; + } + + dXSteps = static_cast(fDeltaPosX); /* [steps] */ + dYSteps = static_cast(fDeltaPosY); /* [steps] */ +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/Service/Odometry.h b/lib/Service/Odometry.h index 2106532c..1a88c947 100644 --- a/lib/Service/Odometry.h +++ b/lib/Service/Odometry.h @@ -1,303 +1,303 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Odometry unit - * @author Andreas Merkle - * - * @addtogroup Service - * - * @{ - */ - -#ifndef ODOMETRY_H -#define ODOMETRY_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** - * This class provides odometry data, based on the encoder informations. - * Odometry data: - * - Orientation - * - Position - * - Mileage - * - * Its following the REP-103: https://ros.org/reps/rep-0103.html - * - * That means the cartesian representation of geographic locations use the - * east-north-up (ENU) convention: - * - X east - * - Y north - * - Z up - * - * Rotation only about the Z axis (yaw) is supported. - * The yaw relates to the X axis. That means if the robot is heading to the - * north, the yaw will be 90°. - */ -class Odometry -{ -public: - /** - * Get odometry instance. - * - * @return Odometry instance. - */ - static Odometry& getInstance() - { - static Odometry instance; /* idiom */ - - return instance; - } - - /** - * Process the odometry unit periodically. - */ - void process(); - - /** - * Get center mileage in mm. - * - * @return Mileage in mm - */ - uint32_t getMileageCenter() const; - - /** - * Get current orientation in mrad. - * - * @return Orientation in mrad - */ - int32_t getOrientation() const; - - /** - * Get absolute position in coordinatesystem. - * The x- and y-axis unit is mm. - * - * @param[out] posX x-coordinate - * @param[out] posY y-coordinate - */ - void getPosition(int32_t& posX, int32_t& posY) const - { - posX = m_posX; - posY = m_posY; - } - - /** - * Set absolute position in coordinate system. - * The x- and y-axis unit is mm. - * - * @param[in] posX x-coordinate - * @param[in] posY y-coordinate - */ - void setPosition(int32_t posX, int32_t posY) - { - m_posX = posX; - m_posY = posY; - } - - /** - * Set the orientation. - * Use this to align the Y axis to north. - * - * @param[in] orientation The orientation in mrad. - */ - void setOrientation(int32_t orientation) - { - m_orientation = orientation; - m_orientation %= FP_2PI(); - } - - /** - * Clear the position by setting (x, y) to (0, 0) mm. - */ - void clearPosition(); - - /** - * Clear mileage by setting it to 0 mm. - */ - void clearMileage(); - - /** - * Is standstill detected? - * - * @return If standstill, it will return true otherwise false. - */ - bool isStandStill() - { - return m_isStandstill; - } - -private: - /** - * If at least one side moved about 1 cm, a new calculation shall be done. - * Don't use values lower than 1 cm, because the accuracy of the orientation - * will decrease massively. - * - * Use a multiple of RobotConstants::ENCODER_STEPS_PER_M for better - * accuracy. - */ - static const uint16_t STEPS_THRESHOLD; - - /** - * Time period in ms for standstill detection. - * If there is no encoder change during this period, it is assumed that - * the robot stopped. - */ - static const uint32_t STANDSTILL_DETECTION_PERIOD = 10; - - /** - * Last number of relative encoder steps left. Its used to avoid permanent - * clearing of the relative encoders. - */ - uint16_t m_lastAbsRelEncStepsLeft; - - /** - * Last number of relative encoder steps right. Its used to avoid permanent - * clearing of the relative encoders. - */ - uint16_t m_lastAbsRelEncStepsRight; - - /** Mileage in encoder steps. */ - uint32_t m_mileage; - - /** Relative encoders left/right. */ - RelativeEncoders m_relEncoders; - - /** Absolute orientation in mrad. 0 mrad means the robot drives parallel to the y-axis. */ - int32_t m_orientation; - - /** Absolute position on x-axis. Unit is mm. */ - int32_t m_posX; - - /** Absolute position on y-axis. Unit is mm. */ - int32_t m_posY; - - /** Counting encoder steps on x-axis. Unit is in encoder steps * 1000 (higher precision). */ - int32_t m_countingXSteps; - - /** Counting encoder steps on y-axis. Unit is in encoder steps * 1000 (higher precision). */ - int32_t m_countingYSteps; - - /** Timer used to detect standstill. */ - SimpleTimer m_timer; - - /** Is robot stopped? */ - bool m_isStandstill; - - /** - * Construct the odometry instance. - */ - Odometry() : - m_lastAbsRelEncStepsLeft(0), - m_lastAbsRelEncStepsRight(0), - m_mileage(0), - m_relEncoders(Board::getInstance().getEncoders()), - m_orientation(FP_PI() / 2), /* 90° - heading to north */ - m_posX(0), - m_posY(0), - m_countingXSteps(0), - m_countingYSteps(0), - m_timer(), - m_isStandstill(true) - { - } - - /** - * Destroy the odometry instance. - */ - ~Odometry() - { - } - - /* Not allowed. */ - Odometry(const Odometry& value); /**< Copy construction of an instance. */ - Odometry& operator=(const Odometry& value); /**< Assignment of an instance. */ - - /** - * Is the robot standstill? - * - * @param[in] absStepsLeft Absolute steps left - * @param[in] absStepsRight Absolute steps right - */ - - bool detectStandStill(uint16_t absStepsLeft, uint16_t absStepsRight); - - /** - * Calculate the mileage in encoder steps. - * - * @param[in] mileage Mileage in encoder steps - * @param[in] stepsCenter Number of steps center - * - * @return Mileage in encoder steps - */ - int32_t calculateMileage(uint32_t mileage, int16_t stepsCenter) const; - - /** - * Calculate the orientation in mrad. - * - * @param[in] orientation Orientation in mrad - * @param[in] stepsLeft Number of encoder steps left - * @param[in] stepsRight Number of encoder steps right - * - * @return Orientation in mrad - */ - int32_t calculateOrientation(int32_t orientation, int16_t stepsLeft, int16_t stepsRight) const; - - /** - * Calculate the vector from last position to new position. - * - * @param[in] stepsCenter Number of steps center - * @param[in] orientation Orientation in mrad - * @param[out] dXSteps Delta x-position on x-axis in steps - * @param[out] dYSteps Delta y-position on y-axis in steps - */ - void calculateDeltaPos(int16_t stepsCenter, int32_t orientation, int16_t& dXSteps, int16_t& dYSteps) const; -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* ODOMETRY_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Odometry unit + * @author Andreas Merkle + * + * @addtogroup Service + * + * @{ + */ + +#ifndef ODOMETRY_H +#define ODOMETRY_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** + * This class provides odometry data, based on the encoder informations. + * Odometry data: + * - Orientation + * - Position + * - Mileage + * + * Its following the REP-103: https://ros.org/reps/rep-0103.html + * + * That means the cartesian representation of geographic locations use the + * east-north-up (ENU) convention: + * - X east + * - Y north + * - Z up + * + * Rotation only about the Z axis (yaw) is supported. + * The yaw relates to the X axis. That means if the robot is heading to the + * north, the yaw will be 90°. + */ +class Odometry +{ +public: + /** + * Get odometry instance. + * + * @return Odometry instance. + */ + static Odometry& getInstance() + { + static Odometry instance; /* idiom */ + + return instance; + } + + /** + * Process the odometry unit periodically. + */ + void process(); + + /** + * Get center mileage in mm. + * + * @return Mileage in mm + */ + uint32_t getMileageCenter() const; + + /** + * Get current orientation in mrad. + * + * @return Orientation in mrad + */ + int32_t getOrientation() const; + + /** + * Get absolute position in coordinatesystem. + * The x- and y-axis unit is mm. + * + * @param[out] posX x-coordinate + * @param[out] posY y-coordinate + */ + void getPosition(int32_t& posX, int32_t& posY) const + { + posX = m_posX; + posY = m_posY; + } + + /** + * Set absolute position in coordinate system. + * The x- and y-axis unit is mm. + * + * @param[in] posX x-coordinate + * @param[in] posY y-coordinate + */ + void setPosition(int32_t posX, int32_t posY) + { + m_posX = posX; + m_posY = posY; + } + + /** + * Set the orientation. + * Use this to align the Y axis to north. + * + * @param[in] orientation The orientation in mrad. + */ + void setOrientation(int32_t orientation) + { + m_orientation = orientation; + m_orientation %= FP_2PI(); + } + + /** + * Clear the position by setting (x, y) to (0, 0) mm. + */ + void clearPosition(); + + /** + * Clear mileage by setting it to 0 mm. + */ + void clearMileage(); + + /** + * Is standstill detected? + * + * @return If standstill, it will return true otherwise false. + */ + bool isStandStill() + { + return m_isStandstill; + } + +private: + /** + * If at least one side moved about 1 cm, a new calculation shall be done. + * Don't use values lower than 1 cm, because the accuracy of the orientation + * will decrease massively. + * + * Use a multiple of RobotConstants::ENCODER_STEPS_PER_M for better + * accuracy. + */ + static const uint16_t STEPS_THRESHOLD; + + /** + * Time period in ms for standstill detection. + * If there is no encoder change during this period, it is assumed that + * the robot stopped. + */ + static const uint32_t STANDSTILL_DETECTION_PERIOD = 10; + + /** + * Last number of relative encoder steps left. Its used to avoid permanent + * clearing of the relative encoders. + */ + uint16_t m_lastAbsRelEncStepsLeft; + + /** + * Last number of relative encoder steps right. Its used to avoid permanent + * clearing of the relative encoders. + */ + uint16_t m_lastAbsRelEncStepsRight; + + /** Mileage in encoder steps. */ + uint32_t m_mileage; + + /** Relative encoders left/right. */ + RelativeEncoders m_relEncoders; + + /** Absolute orientation in mrad. 0 mrad means the robot drives parallel to the y-axis. */ + int32_t m_orientation; + + /** Absolute position on x-axis. Unit is mm. */ + int32_t m_posX; + + /** Absolute position on y-axis. Unit is mm. */ + int32_t m_posY; + + /** Counting encoder steps on x-axis. Unit is in encoder steps * 1000 (higher precision). */ + int32_t m_countingXSteps; + + /** Counting encoder steps on y-axis. Unit is in encoder steps * 1000 (higher precision). */ + int32_t m_countingYSteps; + + /** Timer used to detect standstill. */ + SimpleTimer m_timer; + + /** Is robot stopped? */ + bool m_isStandstill; + + /** + * Construct the odometry instance. + */ + Odometry() : + m_lastAbsRelEncStepsLeft(0), + m_lastAbsRelEncStepsRight(0), + m_mileage(0), + m_relEncoders(Board::getInstance().getEncoders()), + m_orientation(FP_PI() / 2), /* 90° - heading to north */ + m_posX(0), + m_posY(0), + m_countingXSteps(0), + m_countingYSteps(0), + m_timer(), + m_isStandstill(true) + { + } + + /** + * Destroy the odometry instance. + */ + ~Odometry() + { + } + + /* Not allowed. */ + Odometry(const Odometry& value); /**< Copy construction of an instance. */ + Odometry& operator=(const Odometry& value); /**< Assignment of an instance. */ + + /** + * Is the robot standstill? + * + * @param[in] absStepsLeft Absolute steps left + * @param[in] absStepsRight Absolute steps right + */ + + bool detectStandStill(uint16_t absStepsLeft, uint16_t absStepsRight); + + /** + * Calculate the mileage in encoder steps. + * + * @param[in] mileage Mileage in encoder steps + * @param[in] stepsCenter Number of steps center + * + * @return Mileage in encoder steps + */ + int32_t calculateMileage(uint32_t mileage, int16_t stepsCenter) const; + + /** + * Calculate the orientation in mrad. + * + * @param[in] orientation Orientation in mrad + * @param[in] stepsLeft Number of encoder steps left + * @param[in] stepsRight Number of encoder steps right + * + * @return Orientation in mrad + */ + int32_t calculateOrientation(int32_t orientation, int16_t stepsLeft, int16_t stepsRight) const; + + /** + * Calculate the vector from last position to new position. + * + * @param[in] stepsCenter Number of steps center + * @param[in] orientation Orientation in mrad + * @param[out] dXSteps Delta x-position on x-axis in steps + * @param[out] dYSteps Delta y-position on y-axis in steps + */ + void calculateDeltaPos(int16_t stepsCenter, int32_t orientation, int16_t& dXSteps, int16_t& dYSteps) const; +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* ODOMETRY_H */ +/** @} */ diff --git a/lib/Service/PIDController.cpp b/lib/Service/PIDController.cpp index 0511e361..9f49996b 100644 --- a/lib/Service/PIDController.cpp +++ b/lib/Service/PIDController.cpp @@ -1,75 +1,75 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief PID regulator - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief PID regulator + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/Service/PIDController.h b/lib/Service/PIDController.h index 4b548c6f..d3401d10 100644 --- a/lib/Service/PIDController.h +++ b/lib/Service/PIDController.h @@ -1,671 +1,671 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief PID regulator - * @author Andreas Merkle - * - * @addtogroup Service - * - * @{ - */ - -#ifndef PIDCONTROLLER_H -#define PIDCONTROLLER_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** - * Meta template to get the min. or max. value of a template type. - * - * @tparam T The datatype, where the min. or max. value is needed. - */ -template -struct Type -{ - /* Empty, to get compiler error for unsupported datatypes. */ -}; - -/** - * Meta template specialization to get the min. or max. value of a int8_t. - * - * @tparam T The datatype, where the min. or max. value is needed. - */ -template<> -struct Type -{ - /** Enumeration used to determine the min. value. */ - enum Min - { - MIN_RESULT = INT8_MIN /**< Minimum result of datatype */ - }; - - /** Enumeration used to determine the max. value. */ - enum Max - { - MAX_RESULT = INT8_MAX /**< Maximum result of datatype. */ - }; -}; - -/** - * Meta template specialization to get the min. or max. value of a int16_t. - * - * @tparam T The datatype, where the min. or max. value is needed. - */ -template<> -struct Type -{ - /** Enumeration used to determine the min. value. */ - enum Min - { - MIN_RESULT = INT16_MIN /**< Minimum result of datatype */ - }; - - /** Enumeration used to determine the max. value. */ - enum Max - { - MAX_RESULT = INT16_MAX /**< Maximum result of datatype. */ - }; -}; - -/** - * Meta template specialization to get the min. or max. value of a int32_t. - * - * @tparam T The datatype, where the min. or max. value is needed. - */ -template<> -struct Type -{ - /** Enumeration used to determine the min. value. */ - enum Min - { - MIN_RESULT = INT32_MIN /**< Minimum result of datatype */ - }; - - /** Enumeration used to determine the max. value. */ - enum Max - { - MAX_RESULT = INT32_MAX /**< Maximum result of datatype. */ - }; -}; - -/** - * A proportional–integral–derivative controller (PID controller). - * It uses fixed point arithmetic for better performance. - * - * e(t) = sp(t) - pv(t) - * co(t) = Kp * e(t) + Ki * Integral(e(t) dt) + Kd * e(t) / dt - * - * e(t): Error - * sp(t): Set Point - * pv(t): Process Value - * co(t): Controller Output - * - * Derivate on error: Kd * e(t) / dt - * Derivate on measurement: Kd * -pv(t) - * - * @tparam T The datatype used for PID calculation. - */ -template -class PIDController -{ -public: - /** - * Constructs the PID controller. - * Derivative on measurement is disabled. - */ - PIDController() : - m_kPNumerator(0), - m_kPDenominator(1), - m_kINumerator(0), - m_kIDenominator(1), - m_kDNumerator(0), - m_kDDenominator(1), - m_kINumeratorDT(0), - m_kIDenominatorDT(SAMPLE_TIME_DEFAULT), - m_kDNumeratorDT(0), - m_kDDenominatorDT(SAMPLE_TIME_DEFAULT), - m_min(Type::MIN_RESULT), - m_max(Type::MAX_RESULT), - m_lastError(0), - m_integral(0), - m_lastOutput(0), - m_sampleTime(SAMPLE_TIME_DEFAULT), - m_resync(false), - m_isDerivativeOnMeasurement(false), - m_lastProcessValue(0) - { - denominatorsShallNotBeZero(); - } - - /** - * Constructs the PID controller. - * Note, the factors are sample time (dT) depended. - * Derivative on measurement is disabled. - * - * @param[in] kPNumerator Numerator of proportional factor - * @param[in] kPDenominator Denominator of proportional factor - * @param[in] kINumerator Numerator of integral factor - * @param[in] kIDenominator Denominator of integral factor - * @param[in] kDNumerator Numerator of derivative factor - * @param[in] kDDenominator Denominator of derivative factor - * @param[in] max Max. output value, used for limiting. - * @param[in] min Min. output value, used for limiting. - */ - PIDController(T kPNumerator, T kPDenominator, T kINumerator, T kIDenominator, T kDNumerator, T kDDenominator, T max, - T min) : - m_kPNumerator(kPNumerator), - m_kPDenominator(kPDenominator), - m_kINumerator(kINumerator), - m_kIDenominator(kIDenominator), - m_kDNumerator(kDNumerator), - m_kDDenominator(kDDenominator), - m_kINumeratorDT(kINumerator), - m_kIDenominatorDT(kIDenominator * SAMPLE_TIME_DEFAULT), - m_kDNumeratorDT(kDNumerator), - m_kDDenominatorDT(kDDenominator * SAMPLE_TIME_DEFAULT), - m_min(min), - m_max(max), - m_lastError(0), - m_integral(0), - m_lastOutput(0), - m_sampleTime(SAMPLE_TIME_DEFAULT), - m_resync(false), - m_isDerivativeOnMeasurement(false), - m_lastProcessValue(0) - { - denominatorsShallNotBeZero(); - reduceFraction(m_kPNumerator, m_kPDenominator); - reduceFraction(m_kINumerator, m_kIDenominator); - reduceFraction(m_kDNumerator, m_kDDenominator); - reduceFraction(m_kINumeratorDT, m_kIDenominatorDT); - reduceFraction(m_kDNumeratorDT, m_kDDenominatorDT); - } - - /** - * Destroys the PID controller. - */ - ~PIDController() - { - } - - /** - * Constructs the PID controller by copying another one. - * - * @param[in] ctrl PID controller, which to copy - */ - PIDController(const PIDController& ctrl) : - m_kPNumerator(ctrl.m_kPNumerator), - m_kPDenominator(ctrl.m_kPDenominator), - m_kINumerator(ctrl.m_kINumerator), - m_kIDenominator(ctrl.m_kIDenominator), - m_kDNumerator(ctrl.m_kDNumerator), - m_kDDenominator(ctrl.m_kDDenominator), - m_kINumeratorDT(ctrl.m_kINumeratorDT), - m_kIDenominatorDT(ctrl.m_kIDenominatorDT), - m_kDNumeratorDT(ctrl.m_kDNumeratorDT), - m_kDDenominatorDT(ctrl.m_kDDenominatorDT), - m_min(ctrl.m_min), - m_max(ctrl.m_max), - m_lastError(ctrl.m_lastError), - m_integral(ctrl.m_integral), - m_lastOutput(ctrl.m_lastOutput), - m_sampleTime(ctrl.m_sampleTime), - m_resync(ctrl.m_resync), - m_isDerivativeOnMeasurement(ctrl.m_isDerivativeOnMeasurement), - m_lastProcessValue(ctrl.m_lastProcessValue) - { - } - - /** - * Assign a PID controller. - * - * @param[in] ctrl PID controller, which to assign - * - * @return PID controller - */ - PIDController& operator=(const PIDController& ctrl) - { - if (this != &ctrl) - { - m_kPNumerator = ctrl.m_kPNumerator; - m_kPDenominator = ctrl.m_kPDenominator; - m_kINumerator = ctrl.m_kINumerator; - m_kIDenominator = ctrl.m_kIDenominator; - m_kDNumerator = ctrl.m_kDNumerator; - m_kDDenominator = ctrl.m_kDDenominator; - m_kINumeratorDT = ctrl.m_kINumeratorDT; - m_kIDenominatorDT = ctrl.m_kIDenominatorDT; - m_kDNumeratorDT = ctrl.m_kDNumeratorDT; - m_kDDenominatorDT = ctrl.m_kDDenominatorDT; - m_min = ctrl.m_min; - m_max = ctrl.m_max; - m_lastError = ctrl.m_lastError; - m_integral = ctrl.m_integral; - m_lastOutput = ctrl.m_lastOutput; - m_sampleTime = ctrl.m_sampleTime; - m_resync = ctrl.m_resync; - m_isDerivativeOnMeasurement = ctrl.m_isDerivativeOnMeasurement; - m_lastProcessValue = ctrl.m_lastProcessValue; - } - - return *this; - } - - /** - * Process the PID controller and calculate the output according to the - * measured process value. The process value is handled periodically, set - * by sample time. If the calculation is called more often, the last - * controller output will be returned, until next sample point. This way - * it shall ensure a constant dT. - * - * @param[in] setpoint Setpoint - * @param[in] processValue Process value - * - * @return PID controller output - */ - T calculate(T setpoint, T processValue) - { - T output; - - if (true == m_resync) - { - m_integral = m_lastOutput; - m_lastError = setpoint - processValue; - m_resync = false; - m_lastProcessValue = processValue; - } - - { - T error = setpoint - processValue; - T proportional = (m_kPNumerator * error) / m_kPDenominator; - T integral = (m_kINumeratorDT * (m_integral + error)) / m_kIDenominatorDT; - T derivative = 0; - - /* Avoid integral windup. */ - integral = constrain(integral, m_min, m_max); - - if (false == m_isDerivativeOnMeasurement) - { - derivative = ((error - m_lastError) * m_kDNumeratorDT) / m_kDDenominatorDT; - } - else - { - derivative = ((m_lastProcessValue - processValue) * m_kDNumeratorDT) / m_kDDenominatorDT; - } - - output = proportional + integral + derivative; - - /* Limit the controller output */ - output = constrain(output, m_min, m_max); - - m_integral = integral; - m_lastError = error; - m_lastOutput = output; - m_lastProcessValue = processValue; - } - - return output; - } - - /** - * Get proportional factor. - * - * @param[out] numerator Numerator - * @param[out] denominator Denominator - */ - void getPFactor(T& numerator, T& denominator) const - { - numerator = m_kPNumerator; - denominator = m_kPDenominator; - } - - /** - * Set proportional factor. - * - * @param[in] numerator Numerator - * @param[in] denominator Denominator (> 0) - */ - void setPFactor(T numerator, T denominator) - { - if (0 < denominator) - { - m_kPNumerator = numerator; - m_kPDenominator = denominator; - - reduceFraction(m_kPNumerator, m_kPDenominator); - } - } - - /** - * Get integral factor. - * - * @param[out] numerator Numerator - * @param[out] denominator Denominator - */ - void getIFactor(T& numerator, T& denominator) const - { - numerator = m_kINumerator; - denominator = m_kIDenominator; - } - - /** - * Set integral factor. - * Note, the sample time will be considered internally. - * - * @param[in] numerator Numerator - * @param[in] denominator Denominator (> 0) - */ - void setIFactor(T numerator, T denominator) - { - if (0 < denominator) - { - m_kINumerator = numerator; - m_kIDenominator = denominator; - - reduceFraction(m_kINumerator, m_kIDenominator); - - if (0 == m_sampleTime) - { - m_kINumeratorDT = m_kINumerator; - m_kIDenominatorDT = m_kIDenominator; - } - else - { - m_kINumeratorDT = m_kINumerator; - m_kIDenominatorDT = m_kIDenominator * m_sampleTime; - - reduceFraction(m_kINumeratorDT, m_kIDenominatorDT); - } - } - } - - /** - * Get derivative factor. - * - * @param[out] numerator Numerator - * @param[out] denominator Denominator - */ - void getDFactor(T& numerator, T& denominator) const - { - numerator = m_kDNumerator; - denominator = m_kDDenominator; - } - - /** - * Set derivative factor. - * Note, the sample time will be considered internally. - * - * @param[in] numerator Numerator - * @param[in] denominator Denominator (> 0) - */ - void setDFactor(T numerator, T denominator) - { - if (0 < denominator) - { - m_kDNumerator = numerator; - m_kDDenominator = denominator; - - reduceFraction(m_kDNumerator, m_kDDenominator); - - if (0 == m_sampleTime) - { - m_kDNumeratorDT = m_kDNumerator; - m_kDDenominatorDT = m_kDDenominator; - } - else - { - m_kDNumeratorDT = m_kDNumerator; - m_kDDenominatorDT = m_kDDenominator * m_sampleTime; - - reduceFraction(m_kDNumeratorDT, m_kDDenominatorDT); - } - } - } - - /** - * Set output limits. - * Note, a 0 means it shall not be limited. - * - * @param[in] min Min. value - * @param[in] max Max. value - */ - void setLimits(T min, T max) - { - m_min = min; - m_max = max; - } - - /** - * Clear last error and integral value. - */ - void clear() - { - m_lastError = 0; - m_integral = 0; - } - - /** - * Get sample time (dT). - * - * @return Sample time in ms - */ - uint32_t getSampleTime() const - { - return m_sampleTime; - } - - /** - * Set sample time (dT). - * Note, internally the integral and derivative factors will be automatically - * adjusted. - * - * Ensure that the calculate() method is called once in this period. - * - * @param[in] sampleTime Sample time in ms - */ - void setSampleTime(uint32_t sampleTime) - { - if (m_sampleTime != sampleTime) - { - if (0 == sampleTime) - { - m_kINumeratorDT = m_kINumerator; - m_kIDenominatorDT = m_kIDenominator; - m_kDNumeratorDT = m_kDNumerator; - m_kDDenominatorDT = m_kDDenominator; - } - else - { - m_kINumeratorDT = m_kINumerator; - m_kIDenominatorDT = m_kIDenominator * sampleTime; - m_kDNumeratorDT = m_kDNumerator; - m_kDDenominatorDT = m_kDDenominator * sampleTime; - - reduceFraction(m_kINumeratorDT, m_kIDenominatorDT); - reduceFraction(m_kDNumeratorDT, m_kDDenominatorDT); - } - - m_sampleTime = sampleTime; - } - } - - /** - * If the PID controller was not processed for more than one dT and now - * it will be processed again, it may happen that the output bumps, caused - * by the integral and derivative part. This happens especically if the - * setpoint changes as well. To avoid the output bump, call this method once. - */ - void resync() - { - m_resync = true; - } - - /** - * To avoid the derivative kick, enable derivative on measurement. - * The derivative term is based on the negative process value changes, instead - * of the error changes. - * - * @param[in] enable Enables (true) or disables (false) derivative on measurement. - */ - void setDerivativeOnMeasurement(bool enable) - { - m_isDerivativeOnMeasurement = enable; - } - - /** - * Default sample time in ms. - * Keep value lower than 128 to avoid conflict in case T is int8_t. - */ - static const uint32_t SAMPLE_TIME_DEFAULT = 10; - -protected: -private: - T m_kPNumerator; /**< Numerator of proportional factor */ - T m_kPDenominator; /**< Denominator of proportional factor */ - T m_kINumerator; /**< Numerator of integral factor */ - T m_kIDenominator; /**< Denominator of integral factor */ - T m_kDNumerator; /**< Numerator of derivative factor */ - T m_kDDenominator; /**< Denominator of derivative factor */ - - T m_kINumeratorDT; /**< Numerator of integral factor with considered sample time */ - T m_kIDenominatorDT; /**< Denominator of integral factor with considered sample time */ - T m_kDNumeratorDT; /**< Numerator of derivative factor with considered sample time */ - T m_kDDenominatorDT; /**< Denominator of derivative factor with considered sample time */ - - T m_min; /**< Min. output value, used for limiting. */ - T m_max; /**< Max. output value, used for limiting. */ - T m_lastError; /**< Last calculated error */ - T m_integral; /**< Integral value */ - T m_lastOutput; /**< Last output value, which is used till next sample time. */ - uint32_t m_sampleTime; /**< Sample time period in ms */ - bool m_resync; /**< A resync avoids a output bump */ - bool m_isDerivativeOnMeasurement; /**< Enables/Disables derivative on measurement. */ - T m_lastProcessValue; /**< Last process value is used for derivative on measurement only. */ - - /** - * Check all demoniator values for zero. - * If one is zero, it will be set to 1. - */ - void denominatorsShallNotBeZero() - { - if (0 == m_kPDenominator) - { - m_kPDenominator = 1; - } - - if (0 == m_kIDenominator) - { - m_kIDenominator = 1; - } - - if (0 == m_kDDenominator) - { - m_kDDenominator = 1; - } - - if (0 == m_kIDenominatorDT) - { - m_kIDenominatorDT = m_sampleTime; - } - - if (0 == m_kDDenominatorDT) - { - m_kDDenominatorDT = m_sampleTime; - } - } - - /** - * Calculate greatest common divisor. - * - * @param[in] number1 Number 1 - * @param[in] number2 Number 2 - * - * @return Greatest common divisor - */ - T calcGreatestCommonDivisor(T number1, T number2) const - { - if (0 > number1) - { - number1 = -number1; - } - - if (0 > number2) - { - number2 = -number2; - } - - while (0 != number2) - { - T rest = number1 % number2; - number1 = number2; - number2 = rest; - } - - return number1; - } - - /** - * Reduce fraction. - * - * @param[in,out] number1 Number 1 - * @param[in,out] number2 Number 2 - */ - void reduceFraction(T& number1, T& number2) - { - T divisor = calcGreatestCommonDivisor(number1, number2); - - if (0 == divisor) - { - divisor = 1; - } - - number1 /= divisor; - number2 /= divisor; - } -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* PIDCONTROLLER_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief PID regulator + * @author Andreas Merkle + * + * @addtogroup Service + * + * @{ + */ + +#ifndef PIDCONTROLLER_H +#define PIDCONTROLLER_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** + * Meta template to get the min. or max. value of a template type. + * + * @tparam T The datatype, where the min. or max. value is needed. + */ +template +struct Type +{ + /* Empty, to get compiler error for unsupported datatypes. */ +}; + +/** + * Meta template specialization to get the min. or max. value of a int8_t. + * + * @tparam T The datatype, where the min. or max. value is needed. + */ +template<> +struct Type +{ + /** Enumeration used to determine the min. value. */ + enum Min + { + MIN_RESULT = INT8_MIN /**< Minimum result of datatype */ + }; + + /** Enumeration used to determine the max. value. */ + enum Max + { + MAX_RESULT = INT8_MAX /**< Maximum result of datatype. */ + }; +}; + +/** + * Meta template specialization to get the min. or max. value of a int16_t. + * + * @tparam T The datatype, where the min. or max. value is needed. + */ +template<> +struct Type +{ + /** Enumeration used to determine the min. value. */ + enum Min + { + MIN_RESULT = INT16_MIN /**< Minimum result of datatype */ + }; + + /** Enumeration used to determine the max. value. */ + enum Max + { + MAX_RESULT = INT16_MAX /**< Maximum result of datatype. */ + }; +}; + +/** + * Meta template specialization to get the min. or max. value of a int32_t. + * + * @tparam T The datatype, where the min. or max. value is needed. + */ +template<> +struct Type +{ + /** Enumeration used to determine the min. value. */ + enum Min + { + MIN_RESULT = INT32_MIN /**< Minimum result of datatype */ + }; + + /** Enumeration used to determine the max. value. */ + enum Max + { + MAX_RESULT = INT32_MAX /**< Maximum result of datatype. */ + }; +}; + +/** + * A proportional–integral–derivative controller (PID controller). + * It uses fixed point arithmetic for better performance. + * + * e(t) = sp(t) - pv(t) + * co(t) = Kp * e(t) + Ki * Integral(e(t) dt) + Kd * e(t) / dt + * + * e(t): Error + * sp(t): Set Point + * pv(t): Process Value + * co(t): Controller Output + * + * Derivate on error: Kd * e(t) / dt + * Derivate on measurement: Kd * -pv(t) + * + * @tparam T The datatype used for PID calculation. + */ +template +class PIDController +{ +public: + /** + * Constructs the PID controller. + * Derivative on measurement is disabled. + */ + PIDController() : + m_kPNumerator(0), + m_kPDenominator(1), + m_kINumerator(0), + m_kIDenominator(1), + m_kDNumerator(0), + m_kDDenominator(1), + m_kINumeratorDT(0), + m_kIDenominatorDT(SAMPLE_TIME_DEFAULT), + m_kDNumeratorDT(0), + m_kDDenominatorDT(SAMPLE_TIME_DEFAULT), + m_min(Type::MIN_RESULT), + m_max(Type::MAX_RESULT), + m_lastError(0), + m_integral(0), + m_lastOutput(0), + m_sampleTime(SAMPLE_TIME_DEFAULT), + m_resync(false), + m_isDerivativeOnMeasurement(false), + m_lastProcessValue(0) + { + denominatorsShallNotBeZero(); + } + + /** + * Constructs the PID controller. + * Note, the factors are sample time (dT) depended. + * Derivative on measurement is disabled. + * + * @param[in] kPNumerator Numerator of proportional factor + * @param[in] kPDenominator Denominator of proportional factor + * @param[in] kINumerator Numerator of integral factor + * @param[in] kIDenominator Denominator of integral factor + * @param[in] kDNumerator Numerator of derivative factor + * @param[in] kDDenominator Denominator of derivative factor + * @param[in] max Max. output value, used for limiting. + * @param[in] min Min. output value, used for limiting. + */ + PIDController(T kPNumerator, T kPDenominator, T kINumerator, T kIDenominator, T kDNumerator, T kDDenominator, T max, + T min) : + m_kPNumerator(kPNumerator), + m_kPDenominator(kPDenominator), + m_kINumerator(kINumerator), + m_kIDenominator(kIDenominator), + m_kDNumerator(kDNumerator), + m_kDDenominator(kDDenominator), + m_kINumeratorDT(kINumerator), + m_kIDenominatorDT(kIDenominator * SAMPLE_TIME_DEFAULT), + m_kDNumeratorDT(kDNumerator), + m_kDDenominatorDT(kDDenominator * SAMPLE_TIME_DEFAULT), + m_min(min), + m_max(max), + m_lastError(0), + m_integral(0), + m_lastOutput(0), + m_sampleTime(SAMPLE_TIME_DEFAULT), + m_resync(false), + m_isDerivativeOnMeasurement(false), + m_lastProcessValue(0) + { + denominatorsShallNotBeZero(); + reduceFraction(m_kPNumerator, m_kPDenominator); + reduceFraction(m_kINumerator, m_kIDenominator); + reduceFraction(m_kDNumerator, m_kDDenominator); + reduceFraction(m_kINumeratorDT, m_kIDenominatorDT); + reduceFraction(m_kDNumeratorDT, m_kDDenominatorDT); + } + + /** + * Destroys the PID controller. + */ + ~PIDController() + { + } + + /** + * Constructs the PID controller by copying another one. + * + * @param[in] ctrl PID controller, which to copy + */ + PIDController(const PIDController& ctrl) : + m_kPNumerator(ctrl.m_kPNumerator), + m_kPDenominator(ctrl.m_kPDenominator), + m_kINumerator(ctrl.m_kINumerator), + m_kIDenominator(ctrl.m_kIDenominator), + m_kDNumerator(ctrl.m_kDNumerator), + m_kDDenominator(ctrl.m_kDDenominator), + m_kINumeratorDT(ctrl.m_kINumeratorDT), + m_kIDenominatorDT(ctrl.m_kIDenominatorDT), + m_kDNumeratorDT(ctrl.m_kDNumeratorDT), + m_kDDenominatorDT(ctrl.m_kDDenominatorDT), + m_min(ctrl.m_min), + m_max(ctrl.m_max), + m_lastError(ctrl.m_lastError), + m_integral(ctrl.m_integral), + m_lastOutput(ctrl.m_lastOutput), + m_sampleTime(ctrl.m_sampleTime), + m_resync(ctrl.m_resync), + m_isDerivativeOnMeasurement(ctrl.m_isDerivativeOnMeasurement), + m_lastProcessValue(ctrl.m_lastProcessValue) + { + } + + /** + * Assign a PID controller. + * + * @param[in] ctrl PID controller, which to assign + * + * @return PID controller + */ + PIDController& operator=(const PIDController& ctrl) + { + if (this != &ctrl) + { + m_kPNumerator = ctrl.m_kPNumerator; + m_kPDenominator = ctrl.m_kPDenominator; + m_kINumerator = ctrl.m_kINumerator; + m_kIDenominator = ctrl.m_kIDenominator; + m_kDNumerator = ctrl.m_kDNumerator; + m_kDDenominator = ctrl.m_kDDenominator; + m_kINumeratorDT = ctrl.m_kINumeratorDT; + m_kIDenominatorDT = ctrl.m_kIDenominatorDT; + m_kDNumeratorDT = ctrl.m_kDNumeratorDT; + m_kDDenominatorDT = ctrl.m_kDDenominatorDT; + m_min = ctrl.m_min; + m_max = ctrl.m_max; + m_lastError = ctrl.m_lastError; + m_integral = ctrl.m_integral; + m_lastOutput = ctrl.m_lastOutput; + m_sampleTime = ctrl.m_sampleTime; + m_resync = ctrl.m_resync; + m_isDerivativeOnMeasurement = ctrl.m_isDerivativeOnMeasurement; + m_lastProcessValue = ctrl.m_lastProcessValue; + } + + return *this; + } + + /** + * Process the PID controller and calculate the output according to the + * measured process value. The process value is handled periodically, set + * by sample time. If the calculation is called more often, the last + * controller output will be returned, until next sample point. This way + * it shall ensure a constant dT. + * + * @param[in] setpoint Setpoint + * @param[in] processValue Process value + * + * @return PID controller output + */ + T calculate(T setpoint, T processValue) + { + T output; + + if (true == m_resync) + { + m_integral = m_lastOutput; + m_lastError = setpoint - processValue; + m_resync = false; + m_lastProcessValue = processValue; + } + + { + T error = setpoint - processValue; + T proportional = (m_kPNumerator * error) / m_kPDenominator; + T integral = (m_kINumeratorDT * (m_integral + error)) / m_kIDenominatorDT; + T derivative = 0; + + /* Avoid integral windup. */ + integral = constrain(integral, m_min, m_max); + + if (false == m_isDerivativeOnMeasurement) + { + derivative = ((error - m_lastError) * m_kDNumeratorDT) / m_kDDenominatorDT; + } + else + { + derivative = ((m_lastProcessValue - processValue) * m_kDNumeratorDT) / m_kDDenominatorDT; + } + + output = proportional + integral + derivative; + + /* Limit the controller output */ + output = constrain(output, m_min, m_max); + + m_integral = integral; + m_lastError = error; + m_lastOutput = output; + m_lastProcessValue = processValue; + } + + return output; + } + + /** + * Get proportional factor. + * + * @param[out] numerator Numerator + * @param[out] denominator Denominator + */ + void getPFactor(T& numerator, T& denominator) const + { + numerator = m_kPNumerator; + denominator = m_kPDenominator; + } + + /** + * Set proportional factor. + * + * @param[in] numerator Numerator + * @param[in] denominator Denominator (> 0) + */ + void setPFactor(T numerator, T denominator) + { + if (0 < denominator) + { + m_kPNumerator = numerator; + m_kPDenominator = denominator; + + reduceFraction(m_kPNumerator, m_kPDenominator); + } + } + + /** + * Get integral factor. + * + * @param[out] numerator Numerator + * @param[out] denominator Denominator + */ + void getIFactor(T& numerator, T& denominator) const + { + numerator = m_kINumerator; + denominator = m_kIDenominator; + } + + /** + * Set integral factor. + * Note, the sample time will be considered internally. + * + * @param[in] numerator Numerator + * @param[in] denominator Denominator (> 0) + */ + void setIFactor(T numerator, T denominator) + { + if (0 < denominator) + { + m_kINumerator = numerator; + m_kIDenominator = denominator; + + reduceFraction(m_kINumerator, m_kIDenominator); + + if (0 == m_sampleTime) + { + m_kINumeratorDT = m_kINumerator; + m_kIDenominatorDT = m_kIDenominator; + } + else + { + m_kINumeratorDT = m_kINumerator; + m_kIDenominatorDT = m_kIDenominator * m_sampleTime; + + reduceFraction(m_kINumeratorDT, m_kIDenominatorDT); + } + } + } + + /** + * Get derivative factor. + * + * @param[out] numerator Numerator + * @param[out] denominator Denominator + */ + void getDFactor(T& numerator, T& denominator) const + { + numerator = m_kDNumerator; + denominator = m_kDDenominator; + } + + /** + * Set derivative factor. + * Note, the sample time will be considered internally. + * + * @param[in] numerator Numerator + * @param[in] denominator Denominator (> 0) + */ + void setDFactor(T numerator, T denominator) + { + if (0 < denominator) + { + m_kDNumerator = numerator; + m_kDDenominator = denominator; + + reduceFraction(m_kDNumerator, m_kDDenominator); + + if (0 == m_sampleTime) + { + m_kDNumeratorDT = m_kDNumerator; + m_kDDenominatorDT = m_kDDenominator; + } + else + { + m_kDNumeratorDT = m_kDNumerator; + m_kDDenominatorDT = m_kDDenominator * m_sampleTime; + + reduceFraction(m_kDNumeratorDT, m_kDDenominatorDT); + } + } + } + + /** + * Set output limits. + * Note, a 0 means it shall not be limited. + * + * @param[in] min Min. value + * @param[in] max Max. value + */ + void setLimits(T min, T max) + { + m_min = min; + m_max = max; + } + + /** + * Clear last error and integral value. + */ + void clear() + { + m_lastError = 0; + m_integral = 0; + } + + /** + * Get sample time (dT). + * + * @return Sample time in ms + */ + uint32_t getSampleTime() const + { + return m_sampleTime; + } + + /** + * Set sample time (dT). + * Note, internally the integral and derivative factors will be automatically + * adjusted. + * + * Ensure that the calculate() method is called once in this period. + * + * @param[in] sampleTime Sample time in ms + */ + void setSampleTime(uint32_t sampleTime) + { + if (m_sampleTime != sampleTime) + { + if (0 == sampleTime) + { + m_kINumeratorDT = m_kINumerator; + m_kIDenominatorDT = m_kIDenominator; + m_kDNumeratorDT = m_kDNumerator; + m_kDDenominatorDT = m_kDDenominator; + } + else + { + m_kINumeratorDT = m_kINumerator; + m_kIDenominatorDT = m_kIDenominator * sampleTime; + m_kDNumeratorDT = m_kDNumerator; + m_kDDenominatorDT = m_kDDenominator * sampleTime; + + reduceFraction(m_kINumeratorDT, m_kIDenominatorDT); + reduceFraction(m_kDNumeratorDT, m_kDDenominatorDT); + } + + m_sampleTime = sampleTime; + } + } + + /** + * If the PID controller was not processed for more than one dT and now + * it will be processed again, it may happen that the output bumps, caused + * by the integral and derivative part. This happens especically if the + * setpoint changes as well. To avoid the output bump, call this method once. + */ + void resync() + { + m_resync = true; + } + + /** + * To avoid the derivative kick, enable derivative on measurement. + * The derivative term is based on the negative process value changes, instead + * of the error changes. + * + * @param[in] enable Enables (true) or disables (false) derivative on measurement. + */ + void setDerivativeOnMeasurement(bool enable) + { + m_isDerivativeOnMeasurement = enable; + } + + /** + * Default sample time in ms. + * Keep value lower than 128 to avoid conflict in case T is int8_t. + */ + static const uint32_t SAMPLE_TIME_DEFAULT = 10; + +protected: +private: + T m_kPNumerator; /**< Numerator of proportional factor */ + T m_kPDenominator; /**< Denominator of proportional factor */ + T m_kINumerator; /**< Numerator of integral factor */ + T m_kIDenominator; /**< Denominator of integral factor */ + T m_kDNumerator; /**< Numerator of derivative factor */ + T m_kDDenominator; /**< Denominator of derivative factor */ + + T m_kINumeratorDT; /**< Numerator of integral factor with considered sample time */ + T m_kIDenominatorDT; /**< Denominator of integral factor with considered sample time */ + T m_kDNumeratorDT; /**< Numerator of derivative factor with considered sample time */ + T m_kDDenominatorDT; /**< Denominator of derivative factor with considered sample time */ + + T m_min; /**< Min. output value, used for limiting. */ + T m_max; /**< Max. output value, used for limiting. */ + T m_lastError; /**< Last calculated error */ + T m_integral; /**< Integral value */ + T m_lastOutput; /**< Last output value, which is used till next sample time. */ + uint32_t m_sampleTime; /**< Sample time period in ms */ + bool m_resync; /**< A resync avoids a output bump */ + bool m_isDerivativeOnMeasurement; /**< Enables/Disables derivative on measurement. */ + T m_lastProcessValue; /**< Last process value is used for derivative on measurement only. */ + + /** + * Check all demoniator values for zero. + * If one is zero, it will be set to 1. + */ + void denominatorsShallNotBeZero() + { + if (0 == m_kPDenominator) + { + m_kPDenominator = 1; + } + + if (0 == m_kIDenominator) + { + m_kIDenominator = 1; + } + + if (0 == m_kDDenominator) + { + m_kDDenominator = 1; + } + + if (0 == m_kIDenominatorDT) + { + m_kIDenominatorDT = m_sampleTime; + } + + if (0 == m_kDDenominatorDT) + { + m_kDDenominatorDT = m_sampleTime; + } + } + + /** + * Calculate greatest common divisor. + * + * @param[in] number1 Number 1 + * @param[in] number2 Number 2 + * + * @return Greatest common divisor + */ + T calcGreatestCommonDivisor(T number1, T number2) const + { + if (0 > number1) + { + number1 = -number1; + } + + if (0 > number2) + { + number2 = -number2; + } + + while (0 != number2) + { + T rest = number1 % number2; + number1 = number2; + number2 = rest; + } + + return number1; + } + + /** + * Reduce fraction. + * + * @param[in,out] number1 Number 1 + * @param[in,out] number2 Number 2 + */ + void reduceFraction(T& number1, T& number2) + { + T divisor = calcGreatestCommonDivisor(number1, number2); + + if (0 == divisor) + { + divisor = 1; + } + + number1 /= divisor; + number2 /= divisor; + } +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* PIDCONTROLLER_H */ +/** @} */ diff --git a/lib/Service/RelativeEncoders.cpp b/lib/Service/RelativeEncoders.cpp index c773adc2..1f214f4f 100644 --- a/lib/Service/RelativeEncoders.cpp +++ b/lib/Service/RelativeEncoders.cpp @@ -1,101 +1,101 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Relative encoders - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "RelativeEncoders.h" - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void RelativeEncoders::clear() -{ - m_referencePointLeft = m_absEncoders.getCountsLeft(); - m_referencePointRight = m_absEncoders.getCountsRight(); -} - -void RelativeEncoders::clearLeft() -{ - m_referencePointLeft = m_absEncoders.getCountsLeft(); -} - -void RelativeEncoders::clearRight() -{ - m_referencePointRight = m_absEncoders.getCountsRight(); -} - -int16_t RelativeEncoders::getCountsLeft() const -{ - return m_absEncoders.getCountsLeft() - m_referencePointLeft; -} - -int16_t RelativeEncoders::getCountsRight() const -{ - return m_absEncoders.getCountsRight() - m_referencePointRight; -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Relative encoders + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "RelativeEncoders.h" + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void RelativeEncoders::clear() +{ + m_referencePointLeft = m_absEncoders.getCountsLeft(); + m_referencePointRight = m_absEncoders.getCountsRight(); +} + +void RelativeEncoders::clearLeft() +{ + m_referencePointLeft = m_absEncoders.getCountsLeft(); +} + +void RelativeEncoders::clearRight() +{ + m_referencePointRight = m_absEncoders.getCountsRight(); +} + +int16_t RelativeEncoders::getCountsLeft() const +{ + return m_absEncoders.getCountsLeft() - m_referencePointLeft; +} + +int16_t RelativeEncoders::getCountsRight() const +{ + return m_absEncoders.getCountsRight() - m_referencePointRight; +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/Service/RelativeEncoders.h b/lib/Service/RelativeEncoders.h index 8d23d255..00ca2317 100644 --- a/lib/Service/RelativeEncoders.h +++ b/lib/Service/RelativeEncoders.h @@ -1,142 +1,142 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Relative encoders - * @author Andreas Merkle - * - * @addtogroup Service - * - * @{ - */ - -#ifndef RELATIVE_ENCODERS_H -#define RELATIVE_ENCODERS_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** - * The relative encoders provides always the step difference between a reference - * point (absolute number of encoder steps) to the current position (current - * number of encoder steps). - * - * Additional it provides the direction of the movement. - */ -class RelativeEncoders -{ -public: - /** - * Constructs relative encoders. - * The reference point is set to 0 steps. - * - * @param[in] absEncoders The absolute encoders interface. - */ - RelativeEncoders(IEncoders& absEncoders) : - m_absEncoders(absEncoders), - m_referencePointLeft(0), - m_referencePointRight(0) - { - } - - /** - * Destroys the relative encoder instance. - */ - ~RelativeEncoders() - { - } - - /** - * Clear the relative encoder steps left and right by setting it to 0. - */ - void clear(); - - /** - * Clear the relative encoder steps left by setting it to 0. - */ - void clearLeft(); - - /** - * Clear the relative encoder steps right by setting it to 0. - */ - void clearRight(); - - /** - * Get the relative number of encoder steps left, since last clearance. - * - * @return Delta encoder steps - */ - int16_t getCountsLeft() const; - - /** - * Get the relative number of encoder steps right, since last clearance. - * - * @return Delta encoder steps - */ - int16_t getCountsRight() const; - -private: - /** - * Absolute encoders - */ - IEncoders& m_absEncoders; - - /** - * Last absolute number of encoder steps left, which is used as reference point. - */ - int16_t m_referencePointLeft; - - /** - * Last absolute number of encoder steps right, which is used as reference point. - */ - int16_t m_referencePointRight; - - /* Not allowed. */ - RelativeEncoders(); /**< Default constructor. */ - RelativeEncoders(const RelativeEncoders& relEncoder); /**< Copy construction of an instance. */ - RelativeEncoders& operator=(const RelativeEncoders& relEncoder); /**< Assignment of an instance. */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* ENCODERS_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Relative encoders + * @author Andreas Merkle + * + * @addtogroup Service + * + * @{ + */ + +#ifndef RELATIVE_ENCODERS_H +#define RELATIVE_ENCODERS_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** + * The relative encoders provides always the step difference between a reference + * point (absolute number of encoder steps) to the current position (current + * number of encoder steps). + * + * Additional it provides the direction of the movement. + */ +class RelativeEncoders +{ +public: + /** + * Constructs relative encoders. + * The reference point is set to 0 steps. + * + * @param[in] absEncoders The absolute encoders interface. + */ + RelativeEncoders(IEncoders& absEncoders) : + m_absEncoders(absEncoders), + m_referencePointLeft(0), + m_referencePointRight(0) + { + } + + /** + * Destroys the relative encoder instance. + */ + ~RelativeEncoders() + { + } + + /** + * Clear the relative encoder steps left and right by setting it to 0. + */ + void clear(); + + /** + * Clear the relative encoder steps left by setting it to 0. + */ + void clearLeft(); + + /** + * Clear the relative encoder steps right by setting it to 0. + */ + void clearRight(); + + /** + * Get the relative number of encoder steps left, since last clearance. + * + * @return Delta encoder steps + */ + int16_t getCountsLeft() const; + + /** + * Get the relative number of encoder steps right, since last clearance. + * + * @return Delta encoder steps + */ + int16_t getCountsRight() const; + +private: + /** + * Absolute encoders + */ + IEncoders& m_absEncoders; + + /** + * Last absolute number of encoder steps left, which is used as reference point. + */ + int16_t m_referencePointLeft; + + /** + * Last absolute number of encoder steps right, which is used as reference point. + */ + int16_t m_referencePointRight; + + /* Not allowed. */ + RelativeEncoders(); /**< Default constructor. */ + RelativeEncoders(const RelativeEncoders& relEncoder); /**< Copy construction of an instance. */ + RelativeEncoders& operator=(const RelativeEncoders& relEncoder); /**< Assignment of an instance. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* ENCODERS_H */ +/** @} */ diff --git a/lib/Service/SimpleTimer.cpp b/lib/Service/SimpleTimer.cpp index 7e35dfcd..0d68e2d1 100644 --- a/lib/Service/SimpleTimer.cpp +++ b/lib/Service/SimpleTimer.cpp @@ -1,128 +1,128 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Simple timer - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void SimpleTimer::start(uint32_t duration) -{ - m_duration = duration; - m_startTimestamp = millis(); - m_isTimeout = false; - m_isRunning = true; -} - -void SimpleTimer::restart() -{ - m_startTimestamp = millis(); - m_isTimeout = false; - m_isRunning = true; -} - -void SimpleTimer::stop() -{ - m_isTimeout = false; - m_isRunning = false; -} - -bool SimpleTimer::isRunning() const -{ - return m_isRunning; -} - -bool SimpleTimer::isTimeout() -{ - bool isTimeout = false; - - if (true == m_isRunning) - { - if (false == m_isTimeout) - { - uint32_t delta = getCurrentDuration(); - - if (m_duration <= delta) - { - m_isTimeout = true; - } - } - - isTimeout = m_isTimeout; - } - - return isTimeout; -} - -uint32_t SimpleTimer::getCurrentDuration() const -{ - return millis() - m_startTimestamp; -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Simple timer + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void SimpleTimer::start(uint32_t duration) +{ + m_duration = duration; + m_startTimestamp = millis(); + m_isTimeout = false; + m_isRunning = true; +} + +void SimpleTimer::restart() +{ + m_startTimestamp = millis(); + m_isTimeout = false; + m_isRunning = true; +} + +void SimpleTimer::stop() +{ + m_isTimeout = false; + m_isRunning = false; +} + +bool SimpleTimer::isRunning() const +{ + return m_isRunning; +} + +bool SimpleTimer::isTimeout() +{ + bool isTimeout = false; + + if (true == m_isRunning) + { + if (false == m_isTimeout) + { + uint32_t delta = getCurrentDuration(); + + if (m_duration <= delta) + { + m_isTimeout = true; + } + } + + isTimeout = m_isTimeout; + } + + return isTimeout; +} + +uint32_t SimpleTimer::getCurrentDuration() const +{ + return millis() - m_startTimestamp; +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/Service/SimpleTimer.h b/lib/Service/SimpleTimer.h index 6992f083..7c01bb61 100644 --- a/lib/Service/SimpleTimer.h +++ b/lib/Service/SimpleTimer.h @@ -1,157 +1,157 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Simple timer - * @author Andreas Merkle - * - * @addtogroup Service - * - * @{ - */ - -#ifndef SIMPLETIMER_H -#define SIMPLETIMER_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class is a simple timer. */ -class SimpleTimer -{ -public: - /** - * Default constructor. - */ - SimpleTimer() : m_isRunning(false), m_isTimeout(false), m_duration(0), m_startTimestamp(0) - { - } - - /** - * Default copy constructor. - * - * @param[in] timer The timer to be copied. - */ - SimpleTimer(const SimpleTimer& timer) : - m_isRunning(timer.m_isRunning), - m_isTimeout(timer.m_isTimeout), - m_duration(timer.m_duration), - m_startTimestamp(timer.m_startTimestamp) - { - } - - /** - * Default assignment operator. - * - * @param[in] timer The timer to be assigned. - */ - SimpleTimer& operator=(const SimpleTimer& timer) - { - /* Avoid self-assignment */ - if (&timer != this) - { - m_isRunning = timer.m_isRunning; - m_isTimeout = timer.m_isTimeout; - m_duration = timer.m_duration; - m_startTimestamp = timer.m_startTimestamp; - } - - return *this; - } - - /** - * Default destructor. - */ - ~SimpleTimer() - { - } - - /** - * Start timer with the given duration in ms. - * - * @param[in] duration Duration in ms - */ - void start(uint32_t duration); - - /** - * Restart timer with the same duration as the timer was started before. - */ - void restart(); - - /** - * Stop timer. - */ - void stop(); - - /** - * Is timer running? - * - * @return If timer is running, it will return true otherwise false. - */ - bool isRunning() const; - - /** - * Is timeout? - * Note, if the timer is not started the method will return false. - * - * @return If timeout it will return true otherwise false. - */ - bool isTimeout(); - - /** - * Get current duration in ms, till the timer was started. - * It is independed of whether the timer is stopped or timeout. - */ - uint32_t getCurrentDuration() const; - -protected: -private: - bool m_isRunning; /**< Is timer running (true) or not (false). */ - bool m_isTimeout; /**< Timeout flag */ - uint32_t m_duration; /**< Duration in ms */ - uint32_t m_startTimestamp; /**< Timestamp in ms at start. */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* SIMPLETIMER_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Simple timer + * @author Andreas Merkle + * + * @addtogroup Service + * + * @{ + */ + +#ifndef SIMPLETIMER_H +#define SIMPLETIMER_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** This class is a simple timer. */ +class SimpleTimer +{ +public: + /** + * Default constructor. + */ + SimpleTimer() : m_isRunning(false), m_isTimeout(false), m_duration(0), m_startTimestamp(0) + { + } + + /** + * Default copy constructor. + * + * @param[in] timer The timer to be copied. + */ + SimpleTimer(const SimpleTimer& timer) : + m_isRunning(timer.m_isRunning), + m_isTimeout(timer.m_isTimeout), + m_duration(timer.m_duration), + m_startTimestamp(timer.m_startTimestamp) + { + } + + /** + * Default assignment operator. + * + * @param[in] timer The timer to be assigned. + */ + SimpleTimer& operator=(const SimpleTimer& timer) + { + /* Avoid self-assignment */ + if (&timer != this) + { + m_isRunning = timer.m_isRunning; + m_isTimeout = timer.m_isTimeout; + m_duration = timer.m_duration; + m_startTimestamp = timer.m_startTimestamp; + } + + return *this; + } + + /** + * Default destructor. + */ + ~SimpleTimer() + { + } + + /** + * Start timer with the given duration in ms. + * + * @param[in] duration Duration in ms + */ + void start(uint32_t duration); + + /** + * Restart timer with the same duration as the timer was started before. + */ + void restart(); + + /** + * Stop timer. + */ + void stop(); + + /** + * Is timer running? + * + * @return If timer is running, it will return true otherwise false. + */ + bool isRunning() const; + + /** + * Is timeout? + * Note, if the timer is not started the method will return false. + * + * @return If timeout it will return true otherwise false. + */ + bool isTimeout(); + + /** + * Get current duration in ms, till the timer was started. + * It is independed of whether the timer is stopped or timeout. + */ + uint32_t getCurrentDuration() const; + +protected: +private: + bool m_isRunning; /**< Is timer running (true) or not (false). */ + bool m_isTimeout; /**< Timeout flag */ + uint32_t m_duration; /**< Duration in ms */ + uint32_t m_startTimestamp; /**< Timestamp in ms at start. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* SIMPLETIMER_H */ +/** @} */ diff --git a/lib/Service/Speedometer.cpp b/lib/Service/Speedometer.cpp index c4843a32..fb8973da 100644 --- a/lib/Service/Speedometer.cpp +++ b/lib/Service/Speedometer.cpp @@ -1,230 +1,230 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Speedometer - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -Speedometer Speedometer::m_instance; - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void Speedometer::process() -{ - IMotors& motors = Board::getInstance().getMotors(); - uint32_t timestamp = millis(); /* [ms] */ - int32_t diffStepsLeft = m_relEncoders.getCountsLeft(); /* [steps] */ - int32_t diffStepsRight = m_relEncoders.getCountsRight(); /* [steps] */ - const int32_t ONE_SECOND = 1000; /* 1s in ms */ - bool resetLeft = false; - bool resetRight = false; - - if (0 == motors.getLeftSpeed()) - { - resetLeft = true; - } - else - { - Direction currentDrivingDirection = getDirectionLeft(); - - if (currentDrivingDirection != m_lastDirectionLeft) - { - resetLeft = true; - } - - m_lastDirectionLeft = currentDrivingDirection; - } - - if (0 == motors.getRightSpeed()) - { - resetRight = true; - } - else - { - Direction currentDrivingDirection = getDirectionRight(); - - if (currentDrivingDirection != m_lastDirectionRight) - { - resetLeft = true; - } - - m_lastDirectionRight = currentDrivingDirection; - } - - /* If a motor is stopped, it is assumed that the robot has no movement on the - * corresponding motor side. - * All other effects which may lead the robot to move are ignored for simplicity. - * If the application needs more accuracy, the application shall handle that - * by its own. - * - * If the driving direction changed, the relative encoders will be reset to - * avoid using a invalid driving distance for speed calculation. - */ - if (true == resetLeft) - { - m_linearSpeedLeft = 0; - m_timestampLeft = timestamp; - - m_relEncoders.clearLeft(); - } - /* Moved long enough to be able to calculate the linear speed? */ - else if (MIN_ENCODER_COUNT <= abs(diffStepsLeft)) - { - uint32_t dTimeLeft = timestamp - m_timestampLeft; /* [ms] */ - - m_linearSpeedLeft = diffStepsLeft * ONE_SECOND / static_cast(dTimeLeft); - m_timestampLeft = timestamp; - - m_relEncoders.clearLeft(); - } - else - { - ; - } - - if (true == resetRight) - { - m_linearSpeedRight = 0; - m_timestampRight = timestamp; - - m_relEncoders.clearRight(); - } - /* Moved long enough to be able to calculate the linear speed? */ - else if (MIN_ENCODER_COUNT <= abs(diffStepsRight)) - { - uint32_t dTimeRight = timestamp - m_timestampRight; /* [ms] */ - - m_linearSpeedRight = diffStepsRight * ONE_SECOND / static_cast(dTimeRight); - m_timestampRight = timestamp; - - m_relEncoders.clearRight(); - } - else - { - ; - } - } - -int16_t Speedometer::getLinearSpeedCenter() const -{ - int32_t linearSpeedLeft = static_cast(m_linearSpeedLeft); - int32_t linearSpeedRight = static_cast(m_linearSpeedRight); - int32_t linearSpeedCenter = (linearSpeedLeft + linearSpeedRight) / 2; - - return linearSpeedCenter; -} - -int16_t Speedometer::getLinearSpeedLeft() const -{ - return m_linearSpeedLeft; -} - -int16_t Speedometer::getLinearSpeedRight() const -{ - return m_linearSpeedRight; -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -Speedometer::Direction Speedometer::getDirectionLeft() -{ - IMotors& motors = Board::getInstance().getMotors(); - int16_t speed = motors.getLeftSpeed(); - - return getDirectionByMotorSpeed(speed); -} - -Speedometer::Direction Speedometer::getDirectionRight() -{ - IMotors& motors = Board::getInstance().getMotors(); - int16_t speed = motors.getRightSpeed(); - - return getDirectionByMotorSpeed(speed); -} - -Speedometer::Direction Speedometer::getDirectionByMotorSpeed(int16_t motorSpeed) -{ - Direction direction = DIRECTION_STOPPED; - - if (0 < motorSpeed) - { - direction = DIRECTION_POSTIVE; - } - else if (0 > motorSpeed) - { - direction = DIRECTION_NEGATIVE; - } - else - { - ; - } - - return direction; -} - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Speedometer + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +Speedometer Speedometer::m_instance; + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void Speedometer::process() +{ + IMotors& motors = Board::getInstance().getMotors(); + uint32_t timestamp = millis(); /* [ms] */ + int32_t diffStepsLeft = m_relEncoders.getCountsLeft(); /* [steps] */ + int32_t diffStepsRight = m_relEncoders.getCountsRight(); /* [steps] */ + const int32_t ONE_SECOND = 1000; /* 1s in ms */ + bool resetLeft = false; + bool resetRight = false; + + if (0 == motors.getLeftSpeed()) + { + resetLeft = true; + } + else + { + Direction currentDrivingDirection = getDirectionLeft(); + + if (currentDrivingDirection != m_lastDirectionLeft) + { + resetLeft = true; + } + + m_lastDirectionLeft = currentDrivingDirection; + } + + if (0 == motors.getRightSpeed()) + { + resetRight = true; + } + else + { + Direction currentDrivingDirection = getDirectionRight(); + + if (currentDrivingDirection != m_lastDirectionRight) + { + resetLeft = true; + } + + m_lastDirectionRight = currentDrivingDirection; + } + + /* If a motor is stopped, it is assumed that the robot has no movement on the + * corresponding motor side. + * All other effects which may lead the robot to move are ignored for simplicity. + * If the application needs more accuracy, the application shall handle that + * by its own. + * + * If the driving direction changed, the relative encoders will be reset to + * avoid using a invalid driving distance for speed calculation. + */ + if (true == resetLeft) + { + m_linearSpeedLeft = 0; + m_timestampLeft = timestamp; + + m_relEncoders.clearLeft(); + } + /* Moved long enough to be able to calculate the linear speed? */ + else if (MIN_ENCODER_COUNT <= abs(diffStepsLeft)) + { + uint32_t dTimeLeft = timestamp - m_timestampLeft; /* [ms] */ + + m_linearSpeedLeft = diffStepsLeft * ONE_SECOND / static_cast(dTimeLeft); + m_timestampLeft = timestamp; + + m_relEncoders.clearLeft(); + } + else + { + ; + } + + if (true == resetRight) + { + m_linearSpeedRight = 0; + m_timestampRight = timestamp; + + m_relEncoders.clearRight(); + } + /* Moved long enough to be able to calculate the linear speed? */ + else if (MIN_ENCODER_COUNT <= abs(diffStepsRight)) + { + uint32_t dTimeRight = timestamp - m_timestampRight; /* [ms] */ + + m_linearSpeedRight = diffStepsRight * ONE_SECOND / static_cast(dTimeRight); + m_timestampRight = timestamp; + + m_relEncoders.clearRight(); + } + else + { + ; + } + } + +int16_t Speedometer::getLinearSpeedCenter() const +{ + int32_t linearSpeedLeft = static_cast(m_linearSpeedLeft); + int32_t linearSpeedRight = static_cast(m_linearSpeedRight); + int32_t linearSpeedCenter = (linearSpeedLeft + linearSpeedRight) / 2; + + return linearSpeedCenter; +} + +int16_t Speedometer::getLinearSpeedLeft() const +{ + return m_linearSpeedLeft; +} + +int16_t Speedometer::getLinearSpeedRight() const +{ + return m_linearSpeedRight; +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +Speedometer::Direction Speedometer::getDirectionLeft() +{ + IMotors& motors = Board::getInstance().getMotors(); + int16_t speed = motors.getLeftSpeed(); + + return getDirectionByMotorSpeed(speed); +} + +Speedometer::Direction Speedometer::getDirectionRight() +{ + IMotors& motors = Board::getInstance().getMotors(); + int16_t speed = motors.getRightSpeed(); + + return getDirectionByMotorSpeed(speed); +} + +Speedometer::Direction Speedometer::getDirectionByMotorSpeed(int16_t motorSpeed) +{ + Direction direction = DIRECTION_STOPPED; + + if (0 < motorSpeed) + { + direction = DIRECTION_POSTIVE; + } + else if (0 > motorSpeed) + { + direction = DIRECTION_NEGATIVE; + } + else + { + ; + } + + return direction; +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/Service/Speedometer.h b/lib/Service/Speedometer.h index 96a69a2a..4d2d3f06 100644 --- a/lib/Service/Speedometer.h +++ b/lib/Service/Speedometer.h @@ -1,195 +1,195 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Speedometer - * @author Andreas Merkle - * - * @addtogroup Service - * - * @{ - */ - -#ifndef SPEEDOMETER_H -#define SPEEDOMETER_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** This class provides the linear speed in [steps/s], based on the encoder informations. */ -class Speedometer -{ -public: - /** - * Get speedometer instance. - * - * @return Speedometer instance. - */ - static Speedometer& getInstance() - { - return m_instance; - } - - /** - * Update linear speed, based on the measured encoder steps. - * Call this function cyclic. - */ - void process(); - - /** - * Get linear speed center in steps/s. - * - * @return Linear speed center in steps/s - */ - int16_t getLinearSpeedCenter() const; - - /** - * Get linear speed left in steps/s. - * - * @return Linear speed left in steps/s - */ - int16_t getLinearSpeedLeft() const; - - /** - * Get linear speed right in steps/s. - * - * @return Linear speed right in steps/s - */ - int16_t getLinearSpeedRight() const; - -private: - /** - * Direction of movement. - */ - enum Direction - { - DIRECTION_STOPPED = 0, /**< Stopped */ - DIRECTION_POSTIVE, /**< Moving to positive direction */ - DIRECTION_NEGATIVE, /**< Moving to negative direction */ - }; - - /** - * The minimum number of counted encoder steps until the speed is measured. - * It shall avoid a noisy speed. - */ - static const int32_t MIN_ENCODER_COUNT = static_cast(RobotConstants::ENCODER_RESOLUTION / 2U); - - /** Speedometer instance */ - static Speedometer m_instance; - - /** Relative encoder left/right */ - RelativeEncoders m_relEncoders; - - /** Timestamp of last left speed calculation. */ - uint32_t m_timestampLeft; - - /** Timestamp of last right speed calculation. */ - uint32_t m_timestampRight; - - /** Linear speed left in steps/s */ - int16_t m_linearSpeedLeft; - - /** Linear speed right in steps/s */ - int16_t m_linearSpeedRight; - - /** Last determined driving direction left. */ - Direction m_lastDirectionLeft; - - /** Last determined driving direction left. */ - Direction m_lastDirectionRight; - - /** - * Construct the mileage instance. - */ - Speedometer() : - m_relEncoders(Board::getInstance().getEncoders()), - m_timestampLeft(0), - m_timestampRight(0), - m_linearSpeedLeft(0), - m_linearSpeedRight(0), - m_lastDirectionLeft(DIRECTION_STOPPED), - m_lastDirectionRight(DIRECTION_STOPPED) - { - } - - /** - * Destroy the mileage instance. - */ - ~Speedometer() - { - } - - /* Not allowed. */ - Speedometer(const Speedometer& value); /**< Copy construction of an instance. */ - Speedometer& operator=(const Speedometer& value); /**< Assignment of an instance. */ - - /** - * Get the direction of movement left. - * - * @return Direction - */ - Direction getDirectionLeft(); - - /** - * Get the direction of movement right. - * - * @return Direction - */ - Direction getDirectionRight(); - - /** - * Determine the direction of the movement by motor speed. - * - * @param[in] motorSpeed Motor speed in digits - * - * @return Direction of movement. - */ - Direction getDirectionByMotorSpeed(int16_t motorSpeed); -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* SPEEDOMETER_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Speedometer + * @author Andreas Merkle + * + * @addtogroup Service + * + * @{ + */ + +#ifndef SPEEDOMETER_H +#define SPEEDOMETER_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** This class provides the linear speed in [steps/s], based on the encoder informations. */ +class Speedometer +{ +public: + /** + * Get speedometer instance. + * + * @return Speedometer instance. + */ + static Speedometer& getInstance() + { + return m_instance; + } + + /** + * Update linear speed, based on the measured encoder steps. + * Call this function cyclic. + */ + void process(); + + /** + * Get linear speed center in steps/s. + * + * @return Linear speed center in steps/s + */ + int16_t getLinearSpeedCenter() const; + + /** + * Get linear speed left in steps/s. + * + * @return Linear speed left in steps/s + */ + int16_t getLinearSpeedLeft() const; + + /** + * Get linear speed right in steps/s. + * + * @return Linear speed right in steps/s + */ + int16_t getLinearSpeedRight() const; + +private: + /** + * Direction of movement. + */ + enum Direction + { + DIRECTION_STOPPED = 0, /**< Stopped */ + DIRECTION_POSTIVE, /**< Moving to positive direction */ + DIRECTION_NEGATIVE, /**< Moving to negative direction */ + }; + + /** + * The minimum number of counted encoder steps until the speed is measured. + * It shall avoid a noisy speed. + */ + static const int32_t MIN_ENCODER_COUNT = static_cast(RobotConstants::ENCODER_RESOLUTION / 2U); + + /** Speedometer instance */ + static Speedometer m_instance; + + /** Relative encoder left/right */ + RelativeEncoders m_relEncoders; + + /** Timestamp of last left speed calculation. */ + uint32_t m_timestampLeft; + + /** Timestamp of last right speed calculation. */ + uint32_t m_timestampRight; + + /** Linear speed left in steps/s */ + int16_t m_linearSpeedLeft; + + /** Linear speed right in steps/s */ + int16_t m_linearSpeedRight; + + /** Last determined driving direction left. */ + Direction m_lastDirectionLeft; + + /** Last determined driving direction left. */ + Direction m_lastDirectionRight; + + /** + * Construct the mileage instance. + */ + Speedometer() : + m_relEncoders(Board::getInstance().getEncoders()), + m_timestampLeft(0), + m_timestampRight(0), + m_linearSpeedLeft(0), + m_linearSpeedRight(0), + m_lastDirectionLeft(DIRECTION_STOPPED), + m_lastDirectionRight(DIRECTION_STOPPED) + { + } + + /** + * Destroy the mileage instance. + */ + ~Speedometer() + { + } + + /* Not allowed. */ + Speedometer(const Speedometer& value); /**< Copy construction of an instance. */ + Speedometer& operator=(const Speedometer& value); /**< Assignment of an instance. */ + + /** + * Get the direction of movement left. + * + * @return Direction + */ + Direction getDirectionLeft(); + + /** + * Get the direction of movement right. + * + * @return Direction + */ + Direction getDirectionRight(); + + /** + * Determine the direction of the movement by motor speed. + * + * @param[in] motorSpeed Motor speed in digits + * + * @return Direction of movement. + */ + Direction getDirectionByMotorSpeed(int16_t motorSpeed); +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* SPEEDOMETER_H */ +/** @} */ diff --git a/lib/Service/StateMachine.cpp b/lib/Service/StateMachine.cpp index 02117342..682e9516 100644 --- a/lib/Service/StateMachine.cpp +++ b/lib/Service/StateMachine.cpp @@ -1,100 +1,100 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Statemachine - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -void StateMachine::process() -{ - /* Change state? */ - if (nullptr != m_nextState) - { - /* Leave current state */ - if (nullptr != m_currentState) - { - m_currentState->exit(); - } - - m_currentState = m_nextState; - m_nextState = nullptr; - - /* Enter new state */ - m_currentState->entry(); - } - - /* Process current state */ - if (nullptr != m_currentState) - { - m_currentState->process(*this); - } -} - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Statemachine + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void StateMachine::process() +{ + /* Change state? */ + if (nullptr != m_nextState) + { + /* Leave current state */ + if (nullptr != m_currentState) + { + m_currentState->exit(); + } + + m_currentState = m_nextState; + m_nextState = nullptr; + + /* Enter new state */ + m_currentState->entry(); + } + + /* Process current state */ + if (nullptr != m_currentState) + { + m_currentState->process(*this); + } +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/Service/StateMachine.h b/lib/Service/StateMachine.h index d2fed5e2..24da1d25 100644 --- a/lib/Service/StateMachine.h +++ b/lib/Service/StateMachine.h @@ -1,114 +1,114 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Statemachine - * @author Andreas Merkle - * - * @addtogroup Application - * - * @{ - */ - -#ifndef STATE_MACHINE_H -#define STATE_MACHINE_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include "IState.h" - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** The system state machine. */ -class StateMachine -{ -public: - /** - * Default constructor. - */ - StateMachine() : m_currentState(nullptr), m_nextState(nullptr) - { - } - - /** - * Default destructor. - */ - ~StateMachine() - { - } - - /** - * Set next state. - * - * @param[in] state Next state. - */ - void setState(IState* state) - { - m_nextState = state; - } - - /** - * Get current state. - * - * @return Current state. - */ - IState* getState() - { - return m_currentState; - } - - /** - * Process state machine. - */ - void process(); - -protected: -private: - IState* m_currentState; /**< Current active state */ - IState* m_nextState; /**< Next state */ - - /* Not allowed. */ - StateMachine(const StateMachine& sm); /**< Copy construction of an instance. */ - StateMachine& operator=(const StateMachine& sm); /**< Assignment of an instance. */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ - -#endif /* STATE_MACHINE_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Statemachine + * @author Andreas Merkle + * + * @addtogroup Application + * + * @{ + */ + +#ifndef STATE_MACHINE_H +#define STATE_MACHINE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "IState.h" + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The system state machine. */ +class StateMachine +{ +public: + /** + * Default constructor. + */ + StateMachine() : m_currentState(nullptr), m_nextState(nullptr) + { + } + + /** + * Default destructor. + */ + ~StateMachine() + { + } + + /** + * Set next state. + * + * @param[in] state Next state. + */ + void setState(IState* state) + { + m_nextState = state; + } + + /** + * Get current state. + * + * @return Current state. + */ + IState* getState() + { + return m_currentState; + } + + /** + * Process state machine. + */ + void process(); + +protected: +private: + IState* m_currentState; /**< Current active state */ + IState* m_nextState; /**< Next state */ + + /* Not allowed. */ + StateMachine(const StateMachine& sm); /**< Copy construction of an instance. */ + StateMachine& operator=(const StateMachine& sm); /**< Assignment of an instance. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* STATE_MACHINE_H */ +/** @} */ diff --git a/lib/Service/Util.cpp b/lib/Service/Util.cpp index fc20bd9f..aaf9f7b0 100644 --- a/lib/Service/Util.cpp +++ b/lib/Service/Util.cpp @@ -1,200 +1,200 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Utilities - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -/****************************************************************************** - * Protected Methods - *****************************************************************************/ - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -void Util::uintToStr(char* str, size_t size, uint32_t value) -{ - if ((nullptr != str) && (0 < size)) - { - size_t idx = 0; - uint8_t digits = 10; /* 0 - 4294967295 */ - uint32_t divisor = 1000000000; - - while (((size - 1) > idx) && (0 < digits)) - { - char digit = static_cast(value / divisor); - - /* No preceeding zeros. */ - if (0 == idx) - { - if ((0 < digit) || (1 == digits)) - { - str[idx] = digit + '0'; - ++idx; - } - } - else - { - str[idx] = digit + '0'; - ++idx; - } - - --digits; - value %= divisor; - divisor /= 10; - } - - str[idx] = '\0'; - } -} - -void Util::intToStr(char* str, size_t size, int32_t value) -{ - if ((nullptr != str) && (0 < size)) - { - size_t idx = 0; - uint8_t digits = 10; /* -2147483648 - 2147483647 */ - uint32_t divisor = 1000000000; - - if (0 > value) - { - str[idx] = '-'; - ++idx; - - value *= -1; - } - - while (((size - 1) > idx) && (0 < digits)) - { - char digit = static_cast(value / divisor); - - /* No preceeding zeros. */ - if (0 == idx) - { - if ((0 < digit) || (1 == digits)) - { - str[idx] = digit + '0'; - ++idx; - } - } - else - { - str[idx] = digit + '0'; - ++idx; - } - - --digits; - value %= divisor; - divisor /= 10; - } - - str[idx] = '\0'; - } -} - -uint32_t Util::divRoundUp(uint32_t numerator, uint32_t denominator) -{ - uint32_t result = numerator / denominator; - uint32_t rest = numerator % denominator; - uint32_t threshold = denominator / 2U; - - if (threshold <= rest) - { - result += 1U; - } - - return result; -} - -int32_t Util::divRoundUp(int32_t numerator, int32_t denominator) -{ - int32_t result = numerator / denominator; - int32_t rest = numerator % denominator; - int32_t threshold = denominator / 2; - - if (0 > rest) - { - rest *= -1; - } - - if (0 > threshold) - { - threshold *= -1; - } - - if (threshold <= rest) - { - if (0 > result) - { - result -= 1; - } - else - { - result += 1; - } - } - - return result; -} - -/****************************************************************************** - * Local Functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Utilities + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +void Util::uintToStr(char* str, size_t size, uint32_t value) +{ + if ((nullptr != str) && (0 < size)) + { + size_t idx = 0; + uint8_t digits = 10; /* 0 - 4294967295 */ + uint32_t divisor = 1000000000; + + while (((size - 1) > idx) && (0 < digits)) + { + char digit = static_cast(value / divisor); + + /* No preceeding zeros. */ + if (0 == idx) + { + if ((0 < digit) || (1 == digits)) + { + str[idx] = digit + '0'; + ++idx; + } + } + else + { + str[idx] = digit + '0'; + ++idx; + } + + --digits; + value %= divisor; + divisor /= 10; + } + + str[idx] = '\0'; + } +} + +void Util::intToStr(char* str, size_t size, int32_t value) +{ + if ((nullptr != str) && (0 < size)) + { + size_t idx = 0; + uint8_t digits = 10; /* -2147483648 - 2147483647 */ + uint32_t divisor = 1000000000; + + if (0 > value) + { + str[idx] = '-'; + ++idx; + + value *= -1; + } + + while (((size - 1) > idx) && (0 < digits)) + { + char digit = static_cast(value / divisor); + + /* No preceeding zeros. */ + if (0 == idx) + { + if ((0 < digit) || (1 == digits)) + { + str[idx] = digit + '0'; + ++idx; + } + } + else + { + str[idx] = digit + '0'; + ++idx; + } + + --digits; + value %= divisor; + divisor /= 10; + } + + str[idx] = '\0'; + } +} + +uint32_t Util::divRoundUp(uint32_t numerator, uint32_t denominator) +{ + uint32_t result = numerator / denominator; + uint32_t rest = numerator % denominator; + uint32_t threshold = denominator / 2U; + + if (threshold <= rest) + { + result += 1U; + } + + return result; +} + +int32_t Util::divRoundUp(int32_t numerator, int32_t denominator) +{ + int32_t result = numerator / denominator; + int32_t rest = numerator % denominator; + int32_t threshold = denominator / 2; + + if (0 > rest) + { + rest *= -1; + } + + if (0 > threshold) + { + threshold *= -1; + } + + if (threshold <= rest) + { + if (0 > result) + { + result -= 1; + } + else + { + result += 1; + } + } + + return result; +} + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/Service/Util.h b/lib/Service/Util.h index ad50a3b6..ee44e1f9 100644 --- a/lib/Service/Util.h +++ b/lib/Service/Util.h @@ -1,108 +1,108 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Utilities - * @author Andreas Merkle - * - * @addtogroup Service - * - * @{ - */ - -#ifndef UTIL_H -#define UTIL_H - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include - -/** - * Utilities - */ -namespace Util -{ - - /****************************************************************************** - * Macros - *****************************************************************************/ - - /****************************************************************************** - * Types and Classes - *****************************************************************************/ - - /****************************************************************************** - * Functions - *****************************************************************************/ - - /** - * Unsigned integer to string, without preceeding zeros. - * - * @param[out] str Destination string - * @param[in] size Size of the destination string in byte - * @param[in] value Value - */ - void uintToStr(char* str, size_t size, uint32_t value); - - /** - * Signed integer to string, without preceeding zeros. - * - * @param[out] str Destination string - * @param[in] size Size of the destination string in byte - * @param[in] value Value - */ - void intToStr(char* str, size_t size, int32_t value); - - /** - * Divide and round. - * - * @param[in] numerator The numerator. - * @param[in] denominator The denominator. - * - * @return Result - */ - uint32_t divRoundUp(uint32_t numerator, uint32_t denominator); - - /** - * Divide and round. - * - * @param[in] numerator The numerator. - * @param[in] denominator The denominator. - * - * @return Result - */ - int32_t divRoundUp(int32_t numerator, int32_t denominator); - -} /* namespace Util */ - -#endif /* UTIL_H */ -/** @} */ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Utilities + * @author Andreas Merkle + * + * @addtogroup Service + * + * @{ + */ + +#ifndef UTIL_H +#define UTIL_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include + +/** + * Utilities + */ +namespace Util +{ + + /****************************************************************************** + * Macros + *****************************************************************************/ + + /****************************************************************************** + * Types and Classes + *****************************************************************************/ + + /****************************************************************************** + * Functions + *****************************************************************************/ + + /** + * Unsigned integer to string, without preceeding zeros. + * + * @param[out] str Destination string + * @param[in] size Size of the destination string in byte + * @param[in] value Value + */ + void uintToStr(char* str, size_t size, uint32_t value); + + /** + * Signed integer to string, without preceeding zeros. + * + * @param[out] str Destination string + * @param[in] size Size of the destination string in byte + * @param[in] value Value + */ + void intToStr(char* str, size_t size, int32_t value); + + /** + * Divide and round. + * + * @param[in] numerator The numerator. + * @param[in] denominator The denominator. + * + * @return Result + */ + uint32_t divRoundUp(uint32_t numerator, uint32_t denominator); + + /** + * Divide and round. + * + * @param[in] numerator The numerator. + * @param[in] denominator The denominator. + * + * @return Result + */ + int32_t divRoundUp(int32_t numerator, int32_t denominator); + +} /* namespace Util */ + +#endif /* UTIL_H */ +/** @} */ diff --git a/lib/Service/library.json b/lib/Service/library.json index 0164e3b6..8b7582f1 100644 --- a/lib/Service/library.json +++ b/lib/Service/library.json @@ -1,17 +1,17 @@ -{ - "name": "Service", - "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" - }], - "frameworks": "*", - "platforms": "*" +{ + "name": "Service", + "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" + }], + "frameworks": "*", + "platforms": "*" } \ No newline at end of file diff --git a/platformio.ini b/platformio.ini index 2f5765b1..66a38693 100644 --- a/platformio.ini +++ b/platformio.ini @@ -1,684 +1,684 @@ -;PlatformIO Project Configuration File -; -; Build options: build flags, source filter -; Upload options: custom upload port, speed and extra flags -; Library options: dependencies, extra library storages -; Advanced options: extra scripting -; -; Please visit documentation for the other options and examples -; https://docs.platformio.org/page/projectconf.html - -; ***************************************************************************** -; PlatformIO specific configurations -; ***************************************************************************** -[platformio] -; Define default environments, which shall be automatically be built. -;default_envs = CalibTarget -;default_envs = CalibSim -;default_envs = ConvoyLeaderTarget -;default_envs = ConvoyLeaderSim -; default_envs = ConvoyFollowerSim -; default_envs = ConvoyFollowerTarget -;default_envs = LineFollowerTarget -default_envs = LineFollowerSim -;default_envs = RemoteControlTarget -;default_envs = RemoteControlSim -;default_envs = SensorFusionTarget -;default_envs = SensorFusionSim -;default_envs = TestSim - -; ***************************************************************************** -; Common configurations, which is independed of the environment. -; ***************************************************************************** -[common] -build_flags = - -DTEAM_NAME_LINE_1="\"Radon\"" - -DTEAM_NAME_LINE_2="\"Ulzer\"" - -; ***************************************************************************** -; Static check configuration -; ***************************************************************************** -[static_check_configuration] -check_tool = cppcheck, clangtidy -check_severity = medium, high -check_patterns = - include - src - lib -check_flags = - cppcheck: cppcheck: --suppress=*:*/libdeps/* --suppress=*:*lib/Webots/* --suppress=noExplicitConstructor --suppress=unusedFunction --std=c++11 - clangtidy: --header-filter='' --checks=-*,clang-analyzer-*,performance-*,portability-*,readability-uppercase-literal-suffix,readability-redundant-control-flow --warnings-as-errors=-*,clang-analyzer-*,performance-*,portability-*,readability-uppercase-literal-suffix,readability-redundant-control-flow -check_skip_packages = yes - -; ***************************************************************************** -; Target environment for Zumo32U4. -; ***************************************************************************** -[hal:Target] -platform = atmelavr @ ~4.1.0 -board = a-star32U4 -framework = arduino -build_flags = - ${common.build_flags} - -Wno-switch - -Werror -lib_deps = - pololu/Zumo32U4 @ ^2.0.1 - HALInterfaces - HALTarget -lib_ignore = - ArduinoNative - HALTest - HALSim -extra_scripts = - -monitor_speed = 115200 - -; The monitor port shows the debug output and the test output. -; If you connect the robot to your pc, this is the port you will see. -monitor_port = com11 - -; The upload port is spawned by the bootmonitor and used to update the program on the target. -; If you connect the robot to your pc, press twice reset button to jump to the bootloader, -; this is the port you will see. -upload_port = com5 - -; The test port is spawned by the bootmonitor and used to update the test on the target. -; If you connect the robot to your pc, press twice reset button to jump to the bootloader, -; this is the port you will see. -test_port = com9 - -; ***************************************************************************** -; PC target environment for Webots simulation. -; -; It is assumed that the environment variable WEBOTS_HOME is set to the -; Webots directory, e.g. WEBOTS_HOME=C:\Users\\AppData\Local\Programs\Webots -; ***************************************************************************** -[hal:Sim] -platform = native @ ~1.2.1 -build_flags = - -std=c++11 - -DTARGET_NATIVE - -I./lib/Webots/include/c - -I./lib/Webots/include/cpp - -L./lib/Webots/lib/controller - -llibController -lib_deps = - ArduinoNative - HALInterfaces - HALSim - Webots - bblanchon/ArduinoJson @ ~6.21.4 -lib_ignore = - HALTarget - HALTest -extra_scripts = - ./scripts/webots_launcher.py - pre:./scripts/create_webots_library.py - pre:./scripts/copy_sounds.py - post:./scripts/copy_webots_shared_libs.py - post:./scripts/package.py -webots_robot_name = Zumo -webots_robot_serial_rx_channel = 1 -webots_robot_serial_tx_channel = 2 - -; ***************************************************************************** -; PC target environment for tests -; ***************************************************************************** -[hal:Test] -platform = native @ ~1.2.1 -build_flags = - -std=c++11 - -DTARGET_NATIVE - -DUNIT_TEST - -Itest/common -lib_deps = - ArduinoNative - HALInterfaces - HALTest -lib_ignore = - HALSim - HALTarget - Webots -extra_scripts = - -; ***************************************************************************** -; Calibration application -; ***************************************************************************** -[app:Calib] -build_flags = - ${common.build_flags} - -DLOG_DEBUG_ENABLE -lib_deps = - APPCalib - Service -lib_ignore = - APPConvoyLeader - APPLineFollower - APPRemoteControl - APPSensorFusion - APPTest - -; ***************************************************************************** -; Calibration application specific HAL for target -; ***************************************************************************** -[hal_app:CalibTarget] -extends = hal:Target -build_flags = - ${hal:Target.build_flags} -lib_deps = - ${hal:Target.lib_deps} - HALCalibTarget -lib_ignore = - ${hal:Target.lib_ignore} -extra_scripts = - ${hal:Target.extra_scripts} - -; ***************************************************************************** -; Calibration application specific HAL for simulation -; ***************************************************************************** -[hal_app:CalibSim] -extends = hal:Sim -build_flags = - ${hal:Sim.build_flags} -lib_deps = - ${hal:Sim.lib_deps} - HALCalibSim -lib_ignore = - ${hal:Sim.lib_ignore} -extra_scripts = - ${hal:Sim.extra_scripts} - -; ***************************************************************************** -; Convoy leader application -; ***************************************************************************** -[app:ConvoyLeader] -build_flags = - ${common.build_flags} - -DLOG_DEBUG_ENABLE -lib_deps = - APPConvoyLeader - Service -lib_ignore = - APPCalib - APPConvoyFollower - APPLineFollower - APPRemoteControl - APPSensorFusion - APPTest - -; ***************************************************************************** -; Convoy leader application specific HAL for target -; ***************************************************************************** -[hal_app:ConvoyLeaderTarget] -extends = hal:Target -build_flags = - ${hal:Target.build_flags} -lib_deps = - ${hal:Target.lib_deps} - HALConvoyLeaderTarget -lib_ignore = - ${hal:Target.lib_ignore} -extra_scripts = - ${hal:Target.extra_scripts} - -; ***************************************************************************** -; Convoy leader application specific HAL for simulation -; ***************************************************************************** -[hal_app:ConvoyLeaderSim] -extends = hal:Sim -build_flags = - ${hal:Sim.build_flags} -lib_deps = - ${hal:Sim.lib_deps} - HALConvoyLeaderSim -lib_ignore = - ${hal:Sim.lib_ignore} -extra_scripts = - ${hal:Sim.extra_scripts} - -; ***************************************************************************** -; Convoy follower application -; ***************************************************************************** -[app:ConvoyFollower] -build_flags = - ${common.build_flags} - -DLOG_DEBUG_ENABLE -lib_deps = - APPConvoyFollower - Service -lib_ignore = - APPCalib - APPConvoyLeader - APPLineFollower - APPRemoteControl - APPSensorFusion - APPTest - -; ***************************************************************************** -; Convoy follower application specific HAL for target -; ***************************************************************************** -[hal_app:ConvoyFollowerTarget] -extends = hal:Target -build_flags = - ${hal:Target.build_flags} -lib_deps = - ${hal:Target.lib_deps} - HALConvoyFollowerTarget -lib_ignore = - ${hal:Target.lib_ignore} -extra_scripts = - ${hal:Target.extra_scripts} - -; ***************************************************************************** -; Convoy follower application specific HAL for simulation -; ***************************************************************************** -[hal_app:ConvoyFollowerSim] -extends = hal:Sim -build_flags = - ${hal:Sim.build_flags} -lib_deps = - ${hal:Sim.lib_deps} - HALConvoyFollowerSim -lib_ignore = - ${hal:Sim.lib_ignore} -extra_scripts = - ${hal:Sim.extra_scripts} - -; ***************************************************************************** -; Line follower application -; ***************************************************************************** -[app:LineFollower] -build_flags = - ${common.build_flags} - -DLOG_DEBUG_ENABLE -lib_deps = - APPLineFollower - Service -lib_ignore = - APPCalib - APPConvoyFollower - APPConvoyLeader - APPRemoteControl - APPSensorFusion - APPTest - -; ***************************************************************************** -; Line follower application specific HAL for target -; ***************************************************************************** -[hal_app:LineFollowerTarget] -extends = hal:Target -build_flags = - ${hal:Target.build_flags} -lib_deps = - ${hal:Target.lib_deps} - HALLineFollowerTarget -lib_ignore = - ${hal:Target.lib_ignore} -extra_scripts = - ${hal:Target.extra_scripts} - -; ***************************************************************************** -; Line follower application specific HAL for simulation -; ***************************************************************************** -[hal_app:LineFollowerSim] -extends = hal:Sim -build_flags = - ${hal:Sim.build_flags} -lib_deps = - ${hal:Sim.lib_deps} - HALLineFollowerSim -lib_ignore = - ${hal:Sim.lib_ignore} -extra_scripts = - ${hal:Sim.extra_scripts} - -; ***************************************************************************** -; Remote control application -; ***************************************************************************** -[app:RemoteControl] -build_flags = - ${common.build_flags} - -DLOG_DEBUG_ENABLE -lib_deps = - APPRemoteControl - Service -lib_ignore = - APPCalib - APPConvoyFollower - APPConvoyLeader - APPLineFollower - APPSensorFusion - APPTest - -; ***************************************************************************** -; Remote control application specific HAL for target -; ***************************************************************************** -[hal_app:RemoteControlTarget] -extends = hal:Target -build_flags = - ${hal:Target.build_flags} -lib_deps = - ${hal:Target.lib_deps} - HALRemoteControlTarget -lib_ignore = - ${hal:Target.lib_ignore} -extra_scripts = - ${hal:Target.extra_scripts} - -; ***************************************************************************** -; Remote control application specific HAL for simulation -; ***************************************************************************** -[hal_app:RemoteControlSim] -extends = hal:Sim -build_flags = - ${hal:Sim.build_flags} -lib_deps = - ${hal:Sim.lib_deps} - HALRemoteControlSim -lib_ignore = - ${hal:Sim.lib_ignore} -extra_scripts = - ${hal:Sim.extra_scripts} - -; ***************************************************************************** -; Sensor Fusion application -; ***************************************************************************** -[app:SensorFusion] -build_flags = - ${common.build_flags} - -DLOG_DEBUG_ENABLE -lib_deps = - APPSensorFusion - Service -lib_ignore = - APPCalib - APPConvoyFollower - APPConvoyLeader - APPLineFollower - APPRemoteControl - APPTest - -; ***************************************************************************** -; Sensor Fusion application specific HAL for target -; ***************************************************************************** -[hal_app:SensorFusionTarget] -extends = hal:Target -build_flags = - ${hal:Target.build_flags} -lib_deps = - ${hal:Target.lib_deps} - HALSensorFusionTarget -lib_ignore = - ${hal:Target.lib_ignore} -extra_scripts = - ${hal:Target.extra_scripts} - -; ***************************************************************************** -; Sensor Fusion application specific HAL for simulation -; ***************************************************************************** -[hal_app:SensorFusionSim] -extends = hal:Sim -build_flags = - ${hal:Sim.build_flags} -lib_deps = - ${hal:Sim.lib_deps} - HALSensorFusionSim -lib_ignore = - ${hal:Sim.lib_ignore} -extra_scripts = - ${hal:Sim.extra_scripts} - -; ***************************************************************************** -; Test application -; ***************************************************************************** -[app:Test] -build_flags = - ${common.build_flags} -lib_deps = - APPTest - Service -lib_ignore = - APPCalib - APPConvoyFollower - APPConvoyLeader - APPLineFollower - APPRemoteControl - APPSensorFusion - -; ***************************************************************************** -; Test application specific HAL for target -; ***************************************************************************** -[hal_app:TestSim] -extends = hal:Test -build_flags = - ${hal:Test.build_flags} -lib_deps = - ${hal:Test.lib_deps} - HALTestSim -lib_ignore = - ${hal:Test.lib_ignore} -extra_scripts = - ${hal:Test.extra_scripts} - -; ***************************************************************************** -; Calibration application on simulation -; ***************************************************************************** -[env:CalibSim] -extends = hal_app:CalibSim, app:Calib, static_check_configuration -build_flags = - ${hal_app:CalibSim.build_flags} - ${app:Calib.build_flags} -lib_deps = - ${hal_app:CalibSim.lib_deps} - ${app:Calib.lib_deps} -lib_ignore = - ${hal_app:CalibSim.lib_ignore} - ${app:Calib.lib_ignore} -extra_scripts = - ${hal_app:CalibSim.extra_scripts} - -; ***************************************************************************** -; Calibration application on target -; ***************************************************************************** -[env:CalibTarget] -extends = hal_app:CalibTarget, app:Calib, static_check_configuration -build_flags = - ${hal_app:CalibTarget.build_flags} - ${app:Calib.build_flags} -lib_deps = - ${hal_app:CalibTarget.lib_deps} - ${app:Calib.lib_deps} -lib_ignore = - ${hal_app:CalibTarget.lib_ignore} - ${app:Calib.lib_ignore} -extra_scripts = - ${hal_app:CalibTarget.extra_scripts} - -; ***************************************************************************** -; Convoy leader application on target -; ***************************************************************************** -[env:ConvoyLeaderTarget] -extends = hal_app:ConvoyLeaderTarget, app:ConvoyLeader, static_check_configuration -build_flags = - ${hal_app:ConvoyLeaderTarget.build_flags} - ${app:ConvoyLeader.build_flags} -lib_deps = - ${hal_app:ConvoyLeaderTarget.lib_deps} - ${app:ConvoyLeader.lib_deps} -lib_ignore = - ${hal_app:ConvoyLeaderTarget.lib_ignore} - ${app:ConvoyLeader.lib_ignore} -extra_scripts = - ${hal_app:ConvoyLeaderTarget.extra_scripts} - -; ***************************************************************************** -; Convoy leader application on simulation -; ***************************************************************************** -[env:ConvoyLeaderSim] -extends = hal_app:ConvoyLeaderSim, app:ConvoyLeader, static_check_configuration -build_flags = - ${hal_app:ConvoyLeaderSim.build_flags} - ${app:ConvoyLeader.build_flags} -lib_deps = - ${hal_app:ConvoyLeaderSim.lib_deps} - ${app:ConvoyLeader.lib_deps} -lib_ignore = - ${hal_app:ConvoyLeaderSim.lib_ignore} - ${app:ConvoyLeader.lib_ignore} -extra_scripts = - ${hal_app:ConvoyLeaderSim.extra_scripts} - -; ***************************************************************************** -; Convoy follower application on target -; ***************************************************************************** -[env:ConvoyFollowerTarget] -extends = hal_app:ConvoyFollowerTarget, app:ConvoyFollower, static_check_configuration -build_flags = - ${hal_app:ConvoyFollowerTarget.build_flags} - ${app:ConvoyFollower.build_flags} -lib_deps = - ${hal_app:ConvoyFollowerTarget.lib_deps} - ${app:ConvoyFollower.lib_deps} -lib_ignore = - ${hal_app:ConvoyFollowerTarget.lib_ignore} - ${app:ConvoyFollower.lib_ignore} -extra_scripts = - ${hal_app:ConvoyFollowerTarget.extra_scripts} - -; ***************************************************************************** -; Convoy follower application on simulation -; ***************************************************************************** -[env:ConvoyFollowerSim] -extends = hal_app:ConvoyFollowerSim, app:ConvoyFollower, static_check_configuration -build_flags = - ${hal_app:ConvoyFollowerSim.build_flags} - ${app:ConvoyFollower.build_flags} -lib_deps = - ${hal_app:ConvoyFollowerSim.lib_deps} - ${app:ConvoyFollower.lib_deps} -lib_ignore = - ${hal_app:ConvoyFollowerSim.lib_ignore} - ${app:ConvoyFollower.lib_ignore} -extra_scripts = - ${hal_app:ConvoyFollowerSim.extra_scripts} - -; ***************************************************************************** -; Line follower application on target -; ***************************************************************************** -[env:LineFollowerTarget] -extends = hal_app:LineFollowerTarget, app:LineFollower, static_check_configuration -build_flags = - ${hal_app:LineFollowerTarget.build_flags} - ${app:LineFollower.build_flags} -lib_deps = - ${hal_app:LineFollowerTarget.lib_deps} - ${app:LineFollower.lib_deps} -lib_ignore = - ${hal_app:LineFollowerTarget.lib_ignore} - ${app:LineFollower.lib_ignore} -extra_scripts = - ${hal_app:LineFollowerTarget.extra_scripts} - -; ***************************************************************************** -; Line follower application on simulation -; ***************************************************************************** -[env:LineFollowerSim] -extends = hal_app:LineFollowerSim, app:LineFollower, static_check_configuration -build_flags = - ${hal_app:LineFollowerSim.build_flags} - ${app:LineFollower.build_flags} - -D DEBUG_ALGORITHM - ;-D DEBUG_ODOMETRY -lib_deps = - ${hal_app:LineFollowerSim.lib_deps} - ${app:LineFollower.lib_deps} -lib_ignore = - ${hal_app:LineFollowerSim.lib_ignore} - ${app:LineFollower.lib_ignore} -extra_scripts = - ${hal_app:LineFollowerSim.extra_scripts} - -; ***************************************************************************** -; Remote control application on target -; ***************************************************************************** -[env:RemoteControlTarget] -extends = hal_app:RemoteControlTarget, app:RemoteControl, static_check_configuration -build_flags = - ${hal_app:RemoteControlTarget.build_flags} - ${app:RemoteControl.build_flags} -lib_deps = - ${hal_app:RemoteControlTarget.lib_deps} - ${app:RemoteControl.lib_deps} -lib_ignore = - ${hal_app:RemoteControlTarget.lib_ignore} - ${app:RemoteControl.lib_ignore} -extra_scripts = - ${hal_app:RemoteControlTarget.extra_scripts} - -; ***************************************************************************** -; Remote control application on simulation -; ***************************************************************************** -[env:RemoteControlSim] -extends = hal_app:RemoteControlSim, app:RemoteControl, static_check_configuration -build_flags = - ${hal_app:RemoteControlSim.build_flags} - ${app:RemoteControl.build_flags} -lib_deps = - ${hal_app:RemoteControlSim.lib_deps} - ${app:RemoteControl.lib_deps} -lib_ignore = - ${hal_app:RemoteControlSim.lib_ignore} - ${app:RemoteControl.lib_ignore} -extra_scripts = - ${hal_app:RemoteControlSim.extra_scripts} - -; ***************************************************************************** -; Sensor Fusion application on target -; ***************************************************************************** -[env:SensorFusionTarget] -extends = hal_app:SensorFusionTarget, app:SensorFusion, static_check_configuration -build_flags = - ${hal_app:SensorFusionTarget.build_flags} - ${app:SensorFusion.build_flags} -lib_deps = - ${hal_app:SensorFusionTarget.lib_deps} - ${app:SensorFusion.lib_deps} -lib_ignore = - ${hal_app:SensorFusionTarget.lib_ignore} - ${app:SensorFusion.lib_ignore} -extra_scripts = - ${hal_app:SensorFusionTarget.extra_scripts} - -; ***************************************************************************** -; Sensor Fusion application on simulation -; ***************************************************************************** -[env:SensorFusionSim] -extends = hal_app:SensorFusionSim, app:SensorFusion, static_check_configuration -build_flags = - ${hal_app:SensorFusionSim.build_flags} - ${app:SensorFusion.build_flags} -lib_deps = - ${hal_app:SensorFusionSim.lib_deps} - ${app:SensorFusion.lib_deps} -lib_ignore = - ${hal_app:SensorFusionSim.lib_ignore} - ${app:SensorFusion.lib_ignore} -extra_scripts = - ${hal_app:SensorFusionSim.extra_scripts} - -; ***************************************************************************** -; PC target environment for tests -; ***************************************************************************** -[env:TestSim] -extends = hal_app:TestSim, app:Test, static_check_configuration -build_flags = - ${hal_app:TestSim.build_flags} - ${app:Test.build_flags} -lib_deps = - ${hal_app:TestSim.lib_deps} - ${app:Test.lib_deps} -lib_ignore = - ${hal_app:TestSim.lib_ignore} - ${app:Test.lib_ignore} -extra_scripts = - ${hal_app:TestSim.extra_scripts} +;PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +; ***************************************************************************** +; PlatformIO specific configurations +; ***************************************************************************** +[platformio] +; Define default environments, which shall be automatically be built. +;default_envs = CalibTarget +;default_envs = CalibSim +;default_envs = ConvoyLeaderTarget +;default_envs = ConvoyLeaderSim +; default_envs = ConvoyFollowerSim +; default_envs = ConvoyFollowerTarget +;default_envs = LineFollowerTarget +default_envs = LineFollowerSim +;default_envs = RemoteControlTarget +;default_envs = RemoteControlSim +;default_envs = SensorFusionTarget +;default_envs = SensorFusionSim +;default_envs = TestSim + +; ***************************************************************************** +; Common configurations, which is independed of the environment. +; ***************************************************************************** +[common] +build_flags = + -DTEAM_NAME_LINE_1="\"Radon\"" + -DTEAM_NAME_LINE_2="\"Ulzer\"" + +; ***************************************************************************** +; Static check configuration +; ***************************************************************************** +[static_check_configuration] +check_tool = cppcheck, clangtidy +check_severity = medium, high +check_patterns = + include + src + lib +check_flags = + cppcheck: cppcheck: --suppress=*:*/libdeps/* --suppress=*:*lib/Webots/* --suppress=noExplicitConstructor --suppress=unusedFunction --std=c++11 + clangtidy: --header-filter='' --checks=-*,clang-analyzer-*,performance-*,portability-*,readability-uppercase-literal-suffix,readability-redundant-control-flow --warnings-as-errors=-*,clang-analyzer-*,performance-*,portability-*,readability-uppercase-literal-suffix,readability-redundant-control-flow +check_skip_packages = yes + +; ***************************************************************************** +; Target environment for Zumo32U4. +; ***************************************************************************** +[hal:Target] +platform = atmelavr @ ~4.1.0 +board = a-star32U4 +framework = arduino +build_flags = + ${common.build_flags} + -Wno-switch + -Werror +lib_deps = + pololu/Zumo32U4 @ ^2.0.1 + HALInterfaces + HALTarget +lib_ignore = + ArduinoNative + HALTest + HALSim +extra_scripts = + +monitor_speed = 115200 + +; The monitor port shows the debug output and the test output. +; If you connect the robot to your pc, this is the port you will see. +monitor_port = com11 + +; The upload port is spawned by the bootmonitor and used to update the program on the target. +; If you connect the robot to your pc, press twice reset button to jump to the bootloader, +; this is the port you will see. +upload_port = com5 + +; The test port is spawned by the bootmonitor and used to update the test on the target. +; If you connect the robot to your pc, press twice reset button to jump to the bootloader, +; this is the port you will see. +test_port = com9 + +; ***************************************************************************** +; PC target environment for Webots simulation. +; +; It is assumed that the environment variable WEBOTS_HOME is set to the +; Webots directory, e.g. WEBOTS_HOME=C:\Users\\AppData\Local\Programs\Webots +; ***************************************************************************** +[hal:Sim] +platform = native @ ~1.2.1 +build_flags = + -std=c++11 + -DTARGET_NATIVE + -I./lib/Webots/include/c + -I./lib/Webots/include/cpp + -L./lib/Webots/lib/controller + -llibController +lib_deps = + ArduinoNative + HALInterfaces + HALSim + Webots + bblanchon/ArduinoJson @ ~6.21.4 +lib_ignore = + HALTarget + HALTest +extra_scripts = + ./scripts/webots_launcher.py + pre:./scripts/create_webots_library.py + pre:./scripts/copy_sounds.py + post:./scripts/copy_webots_shared_libs.py + post:./scripts/package.py +webots_robot_name = Zumo +webots_robot_serial_rx_channel = 1 +webots_robot_serial_tx_channel = 2 + +; ***************************************************************************** +; PC target environment for tests +; ***************************************************************************** +[hal:Test] +platform = native @ ~1.2.1 +build_flags = + -std=c++11 + -DTARGET_NATIVE + -DUNIT_TEST + -Itest/common +lib_deps = + ArduinoNative + HALInterfaces + HALTest +lib_ignore = + HALSim + HALTarget + Webots +extra_scripts = + +; ***************************************************************************** +; Calibration application +; ***************************************************************************** +[app:Calib] +build_flags = + ${common.build_flags} + -DLOG_DEBUG_ENABLE +lib_deps = + APPCalib + Service +lib_ignore = + APPConvoyLeader + APPLineFollower + APPRemoteControl + APPSensorFusion + APPTest + +; ***************************************************************************** +; Calibration application specific HAL for target +; ***************************************************************************** +[hal_app:CalibTarget] +extends = hal:Target +build_flags = + ${hal:Target.build_flags} +lib_deps = + ${hal:Target.lib_deps} + HALCalibTarget +lib_ignore = + ${hal:Target.lib_ignore} +extra_scripts = + ${hal:Target.extra_scripts} + +; ***************************************************************************** +; Calibration application specific HAL for simulation +; ***************************************************************************** +[hal_app:CalibSim] +extends = hal:Sim +build_flags = + ${hal:Sim.build_flags} +lib_deps = + ${hal:Sim.lib_deps} + HALCalibSim +lib_ignore = + ${hal:Sim.lib_ignore} +extra_scripts = + ${hal:Sim.extra_scripts} + +; ***************************************************************************** +; Convoy leader application +; ***************************************************************************** +[app:ConvoyLeader] +build_flags = + ${common.build_flags} + -DLOG_DEBUG_ENABLE +lib_deps = + APPConvoyLeader + Service +lib_ignore = + APPCalib + APPConvoyFollower + APPLineFollower + APPRemoteControl + APPSensorFusion + APPTest + +; ***************************************************************************** +; Convoy leader application specific HAL for target +; ***************************************************************************** +[hal_app:ConvoyLeaderTarget] +extends = hal:Target +build_flags = + ${hal:Target.build_flags} +lib_deps = + ${hal:Target.lib_deps} + HALConvoyLeaderTarget +lib_ignore = + ${hal:Target.lib_ignore} +extra_scripts = + ${hal:Target.extra_scripts} + +; ***************************************************************************** +; Convoy leader application specific HAL for simulation +; ***************************************************************************** +[hal_app:ConvoyLeaderSim] +extends = hal:Sim +build_flags = + ${hal:Sim.build_flags} +lib_deps = + ${hal:Sim.lib_deps} + HALConvoyLeaderSim +lib_ignore = + ${hal:Sim.lib_ignore} +extra_scripts = + ${hal:Sim.extra_scripts} + +; ***************************************************************************** +; Convoy follower application +; ***************************************************************************** +[app:ConvoyFollower] +build_flags = + ${common.build_flags} + -DLOG_DEBUG_ENABLE +lib_deps = + APPConvoyFollower + Service +lib_ignore = + APPCalib + APPConvoyLeader + APPLineFollower + APPRemoteControl + APPSensorFusion + APPTest + +; ***************************************************************************** +; Convoy follower application specific HAL for target +; ***************************************************************************** +[hal_app:ConvoyFollowerTarget] +extends = hal:Target +build_flags = + ${hal:Target.build_flags} +lib_deps = + ${hal:Target.lib_deps} + HALConvoyFollowerTarget +lib_ignore = + ${hal:Target.lib_ignore} +extra_scripts = + ${hal:Target.extra_scripts} + +; ***************************************************************************** +; Convoy follower application specific HAL for simulation +; ***************************************************************************** +[hal_app:ConvoyFollowerSim] +extends = hal:Sim +build_flags = + ${hal:Sim.build_flags} +lib_deps = + ${hal:Sim.lib_deps} + HALConvoyFollowerSim +lib_ignore = + ${hal:Sim.lib_ignore} +extra_scripts = + ${hal:Sim.extra_scripts} + +; ***************************************************************************** +; Line follower application +; ***************************************************************************** +[app:LineFollower] +build_flags = + ${common.build_flags} + -DLOG_DEBUG_ENABLE +lib_deps = + APPLineFollower + Service +lib_ignore = + APPCalib + APPConvoyFollower + APPConvoyLeader + APPRemoteControl + APPSensorFusion + APPTest + +; ***************************************************************************** +; Line follower application specific HAL for target +; ***************************************************************************** +[hal_app:LineFollowerTarget] +extends = hal:Target +build_flags = + ${hal:Target.build_flags} +lib_deps = + ${hal:Target.lib_deps} + HALLineFollowerTarget +lib_ignore = + ${hal:Target.lib_ignore} +extra_scripts = + ${hal:Target.extra_scripts} + +; ***************************************************************************** +; Line follower application specific HAL for simulation +; ***************************************************************************** +[hal_app:LineFollowerSim] +extends = hal:Sim +build_flags = + ${hal:Sim.build_flags} +lib_deps = + ${hal:Sim.lib_deps} + HALLineFollowerSim +lib_ignore = + ${hal:Sim.lib_ignore} +extra_scripts = + ${hal:Sim.extra_scripts} + +; ***************************************************************************** +; Remote control application +; ***************************************************************************** +[app:RemoteControl] +build_flags = + ${common.build_flags} + -DLOG_DEBUG_ENABLE +lib_deps = + APPRemoteControl + Service +lib_ignore = + APPCalib + APPConvoyFollower + APPConvoyLeader + APPLineFollower + APPSensorFusion + APPTest + +; ***************************************************************************** +; Remote control application specific HAL for target +; ***************************************************************************** +[hal_app:RemoteControlTarget] +extends = hal:Target +build_flags = + ${hal:Target.build_flags} +lib_deps = + ${hal:Target.lib_deps} + HALRemoteControlTarget +lib_ignore = + ${hal:Target.lib_ignore} +extra_scripts = + ${hal:Target.extra_scripts} + +; ***************************************************************************** +; Remote control application specific HAL for simulation +; ***************************************************************************** +[hal_app:RemoteControlSim] +extends = hal:Sim +build_flags = + ${hal:Sim.build_flags} +lib_deps = + ${hal:Sim.lib_deps} + HALRemoteControlSim +lib_ignore = + ${hal:Sim.lib_ignore} +extra_scripts = + ${hal:Sim.extra_scripts} + +; ***************************************************************************** +; Sensor Fusion application +; ***************************************************************************** +[app:SensorFusion] +build_flags = + ${common.build_flags} + -DLOG_DEBUG_ENABLE +lib_deps = + APPSensorFusion + Service +lib_ignore = + APPCalib + APPConvoyFollower + APPConvoyLeader + APPLineFollower + APPRemoteControl + APPTest + +; ***************************************************************************** +; Sensor Fusion application specific HAL for target +; ***************************************************************************** +[hal_app:SensorFusionTarget] +extends = hal:Target +build_flags = + ${hal:Target.build_flags} +lib_deps = + ${hal:Target.lib_deps} + HALSensorFusionTarget +lib_ignore = + ${hal:Target.lib_ignore} +extra_scripts = + ${hal:Target.extra_scripts} + +; ***************************************************************************** +; Sensor Fusion application specific HAL for simulation +; ***************************************************************************** +[hal_app:SensorFusionSim] +extends = hal:Sim +build_flags = + ${hal:Sim.build_flags} +lib_deps = + ${hal:Sim.lib_deps} + HALSensorFusionSim +lib_ignore = + ${hal:Sim.lib_ignore} +extra_scripts = + ${hal:Sim.extra_scripts} + +; ***************************************************************************** +; Test application +; ***************************************************************************** +[app:Test] +build_flags = + ${common.build_flags} +lib_deps = + APPTest + Service +lib_ignore = + APPCalib + APPConvoyFollower + APPConvoyLeader + APPLineFollower + APPRemoteControl + APPSensorFusion + +; ***************************************************************************** +; Test application specific HAL for target +; ***************************************************************************** +[hal_app:TestSim] +extends = hal:Test +build_flags = + ${hal:Test.build_flags} +lib_deps = + ${hal:Test.lib_deps} + HALTestSim +lib_ignore = + ${hal:Test.lib_ignore} +extra_scripts = + ${hal:Test.extra_scripts} + +; ***************************************************************************** +; Calibration application on simulation +; ***************************************************************************** +[env:CalibSim] +extends = hal_app:CalibSim, app:Calib, static_check_configuration +build_flags = + ${hal_app:CalibSim.build_flags} + ${app:Calib.build_flags} +lib_deps = + ${hal_app:CalibSim.lib_deps} + ${app:Calib.lib_deps} +lib_ignore = + ${hal_app:CalibSim.lib_ignore} + ${app:Calib.lib_ignore} +extra_scripts = + ${hal_app:CalibSim.extra_scripts} + +; ***************************************************************************** +; Calibration application on target +; ***************************************************************************** +[env:CalibTarget] +extends = hal_app:CalibTarget, app:Calib, static_check_configuration +build_flags = + ${hal_app:CalibTarget.build_flags} + ${app:Calib.build_flags} +lib_deps = + ${hal_app:CalibTarget.lib_deps} + ${app:Calib.lib_deps} +lib_ignore = + ${hal_app:CalibTarget.lib_ignore} + ${app:Calib.lib_ignore} +extra_scripts = + ${hal_app:CalibTarget.extra_scripts} + +; ***************************************************************************** +; Convoy leader application on target +; ***************************************************************************** +[env:ConvoyLeaderTarget] +extends = hal_app:ConvoyLeaderTarget, app:ConvoyLeader, static_check_configuration +build_flags = + ${hal_app:ConvoyLeaderTarget.build_flags} + ${app:ConvoyLeader.build_flags} +lib_deps = + ${hal_app:ConvoyLeaderTarget.lib_deps} + ${app:ConvoyLeader.lib_deps} +lib_ignore = + ${hal_app:ConvoyLeaderTarget.lib_ignore} + ${app:ConvoyLeader.lib_ignore} +extra_scripts = + ${hal_app:ConvoyLeaderTarget.extra_scripts} + +; ***************************************************************************** +; Convoy leader application on simulation +; ***************************************************************************** +[env:ConvoyLeaderSim] +extends = hal_app:ConvoyLeaderSim, app:ConvoyLeader, static_check_configuration +build_flags = + ${hal_app:ConvoyLeaderSim.build_flags} + ${app:ConvoyLeader.build_flags} +lib_deps = + ${hal_app:ConvoyLeaderSim.lib_deps} + ${app:ConvoyLeader.lib_deps} +lib_ignore = + ${hal_app:ConvoyLeaderSim.lib_ignore} + ${app:ConvoyLeader.lib_ignore} +extra_scripts = + ${hal_app:ConvoyLeaderSim.extra_scripts} + +; ***************************************************************************** +; Convoy follower application on target +; ***************************************************************************** +[env:ConvoyFollowerTarget] +extends = hal_app:ConvoyFollowerTarget, app:ConvoyFollower, static_check_configuration +build_flags = + ${hal_app:ConvoyFollowerTarget.build_flags} + ${app:ConvoyFollower.build_flags} +lib_deps = + ${hal_app:ConvoyFollowerTarget.lib_deps} + ${app:ConvoyFollower.lib_deps} +lib_ignore = + ${hal_app:ConvoyFollowerTarget.lib_ignore} + ${app:ConvoyFollower.lib_ignore} +extra_scripts = + ${hal_app:ConvoyFollowerTarget.extra_scripts} + +; ***************************************************************************** +; Convoy follower application on simulation +; ***************************************************************************** +[env:ConvoyFollowerSim] +extends = hal_app:ConvoyFollowerSim, app:ConvoyFollower, static_check_configuration +build_flags = + ${hal_app:ConvoyFollowerSim.build_flags} + ${app:ConvoyFollower.build_flags} +lib_deps = + ${hal_app:ConvoyFollowerSim.lib_deps} + ${app:ConvoyFollower.lib_deps} +lib_ignore = + ${hal_app:ConvoyFollowerSim.lib_ignore} + ${app:ConvoyFollower.lib_ignore} +extra_scripts = + ${hal_app:ConvoyFollowerSim.extra_scripts} + +; ***************************************************************************** +; Line follower application on target +; ***************************************************************************** +[env:LineFollowerTarget] +extends = hal_app:LineFollowerTarget, app:LineFollower, static_check_configuration +build_flags = + ${hal_app:LineFollowerTarget.build_flags} + ${app:LineFollower.build_flags} +lib_deps = + ${hal_app:LineFollowerTarget.lib_deps} + ${app:LineFollower.lib_deps} +lib_ignore = + ${hal_app:LineFollowerTarget.lib_ignore} + ${app:LineFollower.lib_ignore} +extra_scripts = + ${hal_app:LineFollowerTarget.extra_scripts} + +; ***************************************************************************** +; Line follower application on simulation +; ***************************************************************************** +[env:LineFollowerSim] +extends = hal_app:LineFollowerSim, app:LineFollower, static_check_configuration +build_flags = + ${hal_app:LineFollowerSim.build_flags} + ${app:LineFollower.build_flags} + -D DEBUG_ALGORITHM + ;-D DEBUG_ODOMETRY +lib_deps = + ${hal_app:LineFollowerSim.lib_deps} + ${app:LineFollower.lib_deps} +lib_ignore = + ${hal_app:LineFollowerSim.lib_ignore} + ${app:LineFollower.lib_ignore} +extra_scripts = + ${hal_app:LineFollowerSim.extra_scripts} + +; ***************************************************************************** +; Remote control application on target +; ***************************************************************************** +[env:RemoteControlTarget] +extends = hal_app:RemoteControlTarget, app:RemoteControl, static_check_configuration +build_flags = + ${hal_app:RemoteControlTarget.build_flags} + ${app:RemoteControl.build_flags} +lib_deps = + ${hal_app:RemoteControlTarget.lib_deps} + ${app:RemoteControl.lib_deps} +lib_ignore = + ${hal_app:RemoteControlTarget.lib_ignore} + ${app:RemoteControl.lib_ignore} +extra_scripts = + ${hal_app:RemoteControlTarget.extra_scripts} + +; ***************************************************************************** +; Remote control application on simulation +; ***************************************************************************** +[env:RemoteControlSim] +extends = hal_app:RemoteControlSim, app:RemoteControl, static_check_configuration +build_flags = + ${hal_app:RemoteControlSim.build_flags} + ${app:RemoteControl.build_flags} +lib_deps = + ${hal_app:RemoteControlSim.lib_deps} + ${app:RemoteControl.lib_deps} +lib_ignore = + ${hal_app:RemoteControlSim.lib_ignore} + ${app:RemoteControl.lib_ignore} +extra_scripts = + ${hal_app:RemoteControlSim.extra_scripts} + +; ***************************************************************************** +; Sensor Fusion application on target +; ***************************************************************************** +[env:SensorFusionTarget] +extends = hal_app:SensorFusionTarget, app:SensorFusion, static_check_configuration +build_flags = + ${hal_app:SensorFusionTarget.build_flags} + ${app:SensorFusion.build_flags} +lib_deps = + ${hal_app:SensorFusionTarget.lib_deps} + ${app:SensorFusion.lib_deps} +lib_ignore = + ${hal_app:SensorFusionTarget.lib_ignore} + ${app:SensorFusion.lib_ignore} +extra_scripts = + ${hal_app:SensorFusionTarget.extra_scripts} + +; ***************************************************************************** +; Sensor Fusion application on simulation +; ***************************************************************************** +[env:SensorFusionSim] +extends = hal_app:SensorFusionSim, app:SensorFusion, static_check_configuration +build_flags = + ${hal_app:SensorFusionSim.build_flags} + ${app:SensorFusion.build_flags} +lib_deps = + ${hal_app:SensorFusionSim.lib_deps} + ${app:SensorFusion.lib_deps} +lib_ignore = + ${hal_app:SensorFusionSim.lib_ignore} + ${app:SensorFusion.lib_ignore} +extra_scripts = + ${hal_app:SensorFusionSim.extra_scripts} + +; ***************************************************************************** +; PC target environment for tests +; ***************************************************************************** +[env:TestSim] +extends = hal_app:TestSim, app:Test, static_check_configuration +build_flags = + ${hal_app:TestSim.build_flags} + ${app:Test.build_flags} +lib_deps = + ${hal_app:TestSim.lib_deps} + ${app:Test.lib_deps} +lib_ignore = + ${hal_app:TestSim.lib_ignore} + ${app:Test.lib_ignore} +extra_scripts = + ${hal_app:TestSim.extra_scripts} diff --git a/scripts/copy_sounds.py b/scripts/copy_sounds.py index 8b58d7ed..fdd359d6 100644 --- a/scripts/copy_sounds.py +++ b/scripts/copy_sounds.py @@ -1,71 +1,71 @@ -"""Copies the sounds to the executable local folder.""" - -# MIT License -# -# Copyright (c) 2022 - 2024 Andreas Merkle (web@blue-andi.de) -# -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in all -# copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -# SOFTWARE. - -################################################################################ -# Imports -################################################################################ -import shutil -import errno -import os - -Import("env") # pylint: disable=undefined-variable - -################################################################################ -# Variables -################################################################################ - -PIO_ENV_NAME = env["PIOENV"] # pylint: disable=undefined-variable -BUILD_DIR = env["PROJECT_BUILD_DIR"] + "/" + PIO_ENV_NAME # pylint: disable=undefined-variable -DST_PATH = BUILD_DIR + "/sounds" -SOUNDS_FOLDER = "./sounds" - -################################################################################ -# Classes -################################################################################ - -################################################################################ -# Functions -################################################################################ - -def copy_path_recursive(src, dst): - """Copy all files from the source path to the destination path. - - Args: - src (str): Source path - dst (str): Destination path - """ - try: - shutil.copytree(src, dst) - except OSError as exc: # python >2.5 - if exc.errno in (errno.ENOTDIR, errno.EINVAL): - shutil.copy(src, dst) - else: - raise - -################################################################################ -# Main -################################################################################ - -if os.path.exists(DST_PATH) is False: - copy_path_recursive(SOUNDS_FOLDER, DST_PATH) +"""Copies the sounds to the executable local folder.""" + +# MIT License +# +# Copyright (c) 2022 - 2024 Andreas Merkle (web@blue-andi.de) +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. + +################################################################################ +# Imports +################################################################################ +import shutil +import errno +import os + +Import("env") # pylint: disable=undefined-variable + +################################################################################ +# Variables +################################################################################ + +PIO_ENV_NAME = env["PIOENV"] # pylint: disable=undefined-variable +BUILD_DIR = env["PROJECT_BUILD_DIR"] + "/" + PIO_ENV_NAME # pylint: disable=undefined-variable +DST_PATH = BUILD_DIR + "/sounds" +SOUNDS_FOLDER = "./sounds" + +################################################################################ +# Classes +################################################################################ + +################################################################################ +# Functions +################################################################################ + +def copy_path_recursive(src, dst): + """Copy all files from the source path to the destination path. + + Args: + src (str): Source path + dst (str): Destination path + """ + try: + shutil.copytree(src, dst) + except OSError as exc: # python >2.5 + if exc.errno in (errno.ENOTDIR, errno.EINVAL): + shutil.copy(src, dst) + else: + raise + +################################################################################ +# Main +################################################################################ + +if os.path.exists(DST_PATH) is False: + copy_path_recursive(SOUNDS_FOLDER, DST_PATH) diff --git a/scripts/copy_webots_shared_libs.py b/scripts/copy_webots_shared_libs.py index bc32b2aa..7255d16c 100644 --- a/scripts/copy_webots_shared_libs.py +++ b/scripts/copy_webots_shared_libs.py @@ -1,91 +1,91 @@ -"""Copies the Webots shared libraries.""" - -# MIT License -# -# Copyright (c) 2022 - 2024 Andreas Merkle (web@blue-andi.de) -# -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in all -# copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -# SOFTWARE. - -################################################################################ -# Imports -################################################################################ -import shutil -import os -import platform -import sys - -Import("env") # pylint: disable=undefined-variable - -################################################################################ -# Variables -################################################################################ - -################################################################################ -# Variables -################################################################################ - -OS_PLATFORM_TYPE_WIN = "Windows" -OS_PLATFORM_TYPE_LINUX = "Linux" -OS_PLATFORM_TYPE_MACOS = "Darwin" -OS_PLATFORM_TYPE = platform.system() - -SHARED_FILES_LIST = [] - -if OS_PLATFORM_TYPE == OS_PLATFORM_TYPE_WIN: - - SHARED_FILES_LIST = [ - '/lib/controller/Controller.dll', - '/lib/controller/CppController.dll' - ] - -elif OS_PLATFORM_TYPE == OS_PLATFORM_TYPE_MACOS: - - SHARED_FILES_LIST = [ - '/lib/controller/libController.dylib', - '/lib/controller/libCppController.dylib' - ] - -elif OS_PLATFORM_TYPE == OS_PLATFORM_TYPE_LINUX: - - SHARED_FILES_LIST = [ - '/lib/controller/libController.so', - '/lib/controller/libCppController.so' - ] - -else: - print(f"OS type {OS_PLATFORM_TYPE} not supported.") - sys.exit(1) - -PIO_ENV_NAME = env["PIOENV"] # pylint: disable=undefined-variable - -################################################################################ -# Classes -################################################################################ - -################################################################################ -# Functions -################################################################################ - -################################################################################ -# Main -################################################################################ - -for filePath in SHARED_FILES_LIST: - fullPath = os.getenv('WEBOTS_HOME') + filePath - shutil.copy(fullPath, './.pio/build/' + PIO_ENV_NAME) +"""Copies the Webots shared libraries.""" + +# MIT License +# +# Copyright (c) 2022 - 2024 Andreas Merkle (web@blue-andi.de) +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. + +################################################################################ +# Imports +################################################################################ +import shutil +import os +import platform +import sys + +Import("env") # pylint: disable=undefined-variable + +################################################################################ +# Variables +################################################################################ + +################################################################################ +# Variables +################################################################################ + +OS_PLATFORM_TYPE_WIN = "Windows" +OS_PLATFORM_TYPE_LINUX = "Linux" +OS_PLATFORM_TYPE_MACOS = "Darwin" +OS_PLATFORM_TYPE = platform.system() + +SHARED_FILES_LIST = [] + +if OS_PLATFORM_TYPE == OS_PLATFORM_TYPE_WIN: + + SHARED_FILES_LIST = [ + '/lib/controller/Controller.dll', + '/lib/controller/CppController.dll' + ] + +elif OS_PLATFORM_TYPE == OS_PLATFORM_TYPE_MACOS: + + SHARED_FILES_LIST = [ + '/lib/controller/libController.dylib', + '/lib/controller/libCppController.dylib' + ] + +elif OS_PLATFORM_TYPE == OS_PLATFORM_TYPE_LINUX: + + SHARED_FILES_LIST = [ + '/lib/controller/libController.so', + '/lib/controller/libCppController.so' + ] + +else: + print(f"OS type {OS_PLATFORM_TYPE} not supported.") + sys.exit(1) + +PIO_ENV_NAME = env["PIOENV"] # pylint: disable=undefined-variable + +################################################################################ +# Classes +################################################################################ + +################################################################################ +# Functions +################################################################################ + +################################################################################ +# Main +################################################################################ + +for filePath in SHARED_FILES_LIST: + fullPath = os.getenv('WEBOTS_HOME') + filePath + shutil.copy(fullPath, './.pio/build/' + PIO_ENV_NAME) diff --git a/scripts/create_webots_library.py b/scripts/create_webots_library.py index 5e611031..7755237c 100644 --- a/scripts/create_webots_library.py +++ b/scripts/create_webots_library.py @@ -1,119 +1,119 @@ -"""Webots library creation""" - -# MIT License -# -# Copyright (c) 2022 - 2024 Andreas Merkle (web@blue-andi.de) -# -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in all -# copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -# SOFTWARE. - -################################################################################ -# Imports -################################################################################ -import sys -import shutil -import errno -import os -import json -import platform - -################################################################################ -# Variables -################################################################################ - -src_path_include_c = os.getenv('WEBOTS_HOME') + '/include/controller/c' -src_path_include_cpp = os.getenv('WEBOTS_HOME') + '/include/controller/cpp' -src_path_source = os.getenv('WEBOTS_HOME') + '/src/controller/cpp' -src_path_lib = os.getenv('WEBOTS_HOME') + '/lib/controller/libController' - -OS_PLATFORM_TYPE_WIN = "Windows" -OS_PLATFORM_TYPE_LINUX = "Linux" -OS_PLATFORM_TYPE_MACOS = "Darwin" -OS_PLATFORM_TYPE = platform.system() - -# Add correct file extension for the platform. -if OS_PLATFORM_TYPE == OS_PLATFORM_TYPE_WIN: - - src_path_lib += '.a' - -elif OS_PLATFORM_TYPE == OS_PLATFORM_TYPE_MACOS: - - src_path_lib += '.dylib' - -elif OS_PLATFORM_TYPE == OS_PLATFORM_TYPE_LINUX: - - src_path_lib += '.so' - -else: - print(f"OS type {OS_PLATFORM_TYPE} not supported.") - sys.exit(1) - -DST_PATH = './lib/Webots' - -library_json = { - "name": "Webots", - "version": "0.1.0", - "platforms": ["native"] -} - -################################################################################ -# Classes -################################################################################ - -################################################################################ -# Functions -################################################################################ - -def copy_path_recursive(src, dst): - """Copy all files from the source path to the destination path. - - Args: - src (str): Source path - dst (str): Destination path - """ - try: - shutil.copytree(src, dst) - except OSError as exc: # python >2.5 - if exc.errno in (errno.ENOTDIR, errno.EINVAL): - shutil.copy(src, dst) - else: raise - -def copy_single_file(src, dst): - """Copy a single source file to the destination path. - - Args: - src (str): Source file - dst (str): Destination path - """ - os.makedirs(os.path.dirname(dst), exist_ok=True) - shutil.copy(src, dst) - -################################################################################ -# Main -################################################################################ - -if os.path.exists(DST_PATH) is False: - print(f'Create WEBOTS library to {DST_PATH}.') - - copy_path_recursive(src_path_include_c, DST_PATH + '/include/c/') - copy_path_recursive(src_path_include_cpp, DST_PATH + '/include/cpp/') - copy_path_recursive(src_path_source, DST_PATH + '/src/') - copy_single_file(src_path_lib, DST_PATH + '/lib/') - - with open(DST_PATH + '/library.json', 'w', encoding="utf-8") as fd: - json.dump(library_json, fd, ensure_ascii=False, indent=4) +"""Webots library creation""" + +# MIT License +# +# Copyright (c) 2022 - 2024 Andreas Merkle (web@blue-andi.de) +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. + +################################################################################ +# Imports +################################################################################ +import sys +import shutil +import errno +import os +import json +import platform + +################################################################################ +# Variables +################################################################################ + +src_path_include_c = os.getenv('WEBOTS_HOME') + '/include/controller/c' +src_path_include_cpp = os.getenv('WEBOTS_HOME') + '/include/controller/cpp' +src_path_source = os.getenv('WEBOTS_HOME') + '/src/controller/cpp' +src_path_lib = os.getenv('WEBOTS_HOME') + '/lib/controller/libController' + +OS_PLATFORM_TYPE_WIN = "Windows" +OS_PLATFORM_TYPE_LINUX = "Linux" +OS_PLATFORM_TYPE_MACOS = "Darwin" +OS_PLATFORM_TYPE = platform.system() + +# Add correct file extension for the platform. +if OS_PLATFORM_TYPE == OS_PLATFORM_TYPE_WIN: + + src_path_lib += '.a' + +elif OS_PLATFORM_TYPE == OS_PLATFORM_TYPE_MACOS: + + src_path_lib += '.dylib' + +elif OS_PLATFORM_TYPE == OS_PLATFORM_TYPE_LINUX: + + src_path_lib += '.so' + +else: + print(f"OS type {OS_PLATFORM_TYPE} not supported.") + sys.exit(1) + +DST_PATH = './lib/Webots' + +library_json = { + "name": "Webots", + "version": "0.1.0", + "platforms": ["native"] +} + +################################################################################ +# Classes +################################################################################ + +################################################################################ +# Functions +################################################################################ + +def copy_path_recursive(src, dst): + """Copy all files from the source path to the destination path. + + Args: + src (str): Source path + dst (str): Destination path + """ + try: + shutil.copytree(src, dst) + except OSError as exc: # python >2.5 + if exc.errno in (errno.ENOTDIR, errno.EINVAL): + shutil.copy(src, dst) + else: raise + +def copy_single_file(src, dst): + """Copy a single source file to the destination path. + + Args: + src (str): Source file + dst (str): Destination path + """ + os.makedirs(os.path.dirname(dst), exist_ok=True) + shutil.copy(src, dst) + +################################################################################ +# Main +################################################################################ + +if os.path.exists(DST_PATH) is False: + print(f'Create WEBOTS library to {DST_PATH}.') + + copy_path_recursive(src_path_include_c, DST_PATH + '/include/c/') + copy_path_recursive(src_path_include_cpp, DST_PATH + '/include/cpp/') + copy_path_recursive(src_path_source, DST_PATH + '/src/') + copy_single_file(src_path_lib, DST_PATH + '/lib/') + + with open(DST_PATH + '/library.json', 'w', encoding="utf-8") as fd: + json.dump(library_json, fd, ensure_ascii=False, indent=4) diff --git a/scripts/package.py b/scripts/package.py index 0110b7bb..aa553641 100644 --- a/scripts/package.py +++ b/scripts/package.py @@ -1,115 +1,115 @@ -"""Pack the executable and its dependencies into a ZIP file.""" - -# MIT License -# -# Copyright (c) 2022 - 2024 Andreas Merkle (web@blue-andi.de) -# -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in all -# copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -# SOFTWARE. - -################################################################################ -# Imports -################################################################################ -import platform -import sys -from zipfile import ZipFile - -Import("env") # pylint: disable=undefined-variable - -################################################################################ -# Variables -################################################################################ - -OS_PLATFORM_TYPE_WIN = "Windows" -OS_PLATFORM_TYPE_LINUX = "Linux" -OS_PLATFORM_TYPE_MACOS = "Darwin" -OS_PLATFORM_TYPE = platform.system() - -PIO_ENV_NAME = env["PIOENV"] # pylint: disable=undefined-variable -BUILD_DIR = env["PROJECT_BUILD_DIR"] + "/" + PIO_ENV_NAME # pylint: disable=undefined-variable -OUT_PATH = BUILD_DIR + "/" + PIO_ENV_NAME + ".zip" - -FILE_NAME_LIST = [] - -if OS_PLATFORM_TYPE == OS_PLATFORM_TYPE_WIN: - - FILE_NAME_LIST = [ - "/Controller.dll", - "/CppController.dll", - "/program.exe", - "/sounds/4KHz.wav", - "/sounds/10KHz.wav", - "/sounds/440Hz.wav", - ] - -elif OS_PLATFORM_TYPE == OS_PLATFORM_TYPE_MACOS: - - FILE_NAME_LIST = [ - "/Controller.dylib", - "/CppController.dylib", - "/program", - "/sounds/4KHz.wav", - "/sounds/10KHz.wav", - "/sounds/440Hz.wav", - ] - -elif OS_PLATFORM_TYPE == OS_PLATFORM_TYPE_LINUX: - - FILE_NAME_LIST = [ - "/Controller.so", - "/CppController.so", - "/program", - "/sounds/4KHz.wav", - "/sounds/10KHz.wav", - "/sounds/440Hz.wav", - ] - -else: - print(f"OS type {OS_PLATFORM_TYPE} not supported.") - sys.exit(1) - -################################################################################ -# Classes -################################################################################ - -################################################################################ -# Functions -################################################################################ - -def zip_executable(source, target, env): # pylint: disable=unused-argument - """Create a zip file with the executable and all required libraries. - - Args: - source (Any): Source (not used) - target (Any): Target (not used) - env (Any): Environment (not used) - """ - - with ZipFile(OUT_PATH, 'w') as zip_object: - for file_name in FILE_NAME_LIST: - full_path = BUILD_DIR + file_name - zip_object.write(full_path, PIO_ENV_NAME + file_name) - - print("Packed executable found in: " + OUT_PATH) - -################################################################################ -# Main -################################################################################ - -# Create packed executable as a Post action. -env.AddPostAction(BUILD_DIR + "/program.exe", zip_executable) # pylint: disable=undefined-variable +"""Pack the executable and its dependencies into a ZIP file.""" + +# MIT License +# +# Copyright (c) 2022 - 2024 Andreas Merkle (web@blue-andi.de) +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. + +################################################################################ +# Imports +################################################################################ +import platform +import sys +from zipfile import ZipFile + +Import("env") # pylint: disable=undefined-variable + +################################################################################ +# Variables +################################################################################ + +OS_PLATFORM_TYPE_WIN = "Windows" +OS_PLATFORM_TYPE_LINUX = "Linux" +OS_PLATFORM_TYPE_MACOS = "Darwin" +OS_PLATFORM_TYPE = platform.system() + +PIO_ENV_NAME = env["PIOENV"] # pylint: disable=undefined-variable +BUILD_DIR = env["PROJECT_BUILD_DIR"] + "/" + PIO_ENV_NAME # pylint: disable=undefined-variable +OUT_PATH = BUILD_DIR + "/" + PIO_ENV_NAME + ".zip" + +FILE_NAME_LIST = [] + +if OS_PLATFORM_TYPE == OS_PLATFORM_TYPE_WIN: + + FILE_NAME_LIST = [ + "/Controller.dll", + "/CppController.dll", + "/program.exe", + "/sounds/4KHz.wav", + "/sounds/10KHz.wav", + "/sounds/440Hz.wav", + ] + +elif OS_PLATFORM_TYPE == OS_PLATFORM_TYPE_MACOS: + + FILE_NAME_LIST = [ + "/Controller.dylib", + "/CppController.dylib", + "/program", + "/sounds/4KHz.wav", + "/sounds/10KHz.wav", + "/sounds/440Hz.wav", + ] + +elif OS_PLATFORM_TYPE == OS_PLATFORM_TYPE_LINUX: + + FILE_NAME_LIST = [ + "/Controller.so", + "/CppController.so", + "/program", + "/sounds/4KHz.wav", + "/sounds/10KHz.wav", + "/sounds/440Hz.wav", + ] + +else: + print(f"OS type {OS_PLATFORM_TYPE} not supported.") + sys.exit(1) + +################################################################################ +# Classes +################################################################################ + +################################################################################ +# Functions +################################################################################ + +def zip_executable(source, target, env): # pylint: disable=unused-argument + """Create a zip file with the executable and all required libraries. + + Args: + source (Any): Source (not used) + target (Any): Target (not used) + env (Any): Environment (not used) + """ + + with ZipFile(OUT_PATH, 'w') as zip_object: + for file_name in FILE_NAME_LIST: + full_path = BUILD_DIR + file_name + zip_object.write(full_path, PIO_ENV_NAME + file_name) + + print("Packed executable found in: " + OUT_PATH) + +################################################################################ +# Main +################################################################################ + +# Create packed executable as a Post action. +env.AddPostAction(BUILD_DIR + "/program.exe", zip_executable) # pylint: disable=undefined-variable diff --git a/scripts/webots_launcher.py b/scripts/webots_launcher.py index 1e6b24a6..e62171a9 100644 --- a/scripts/webots_launcher.py +++ b/scripts/webots_launcher.py @@ -1,114 +1,114 @@ -"""Webots launcher""" - -# MIT License -# -# Copyright (c) 2022 - 2024 Andreas Merkle (web@blue-andi.de) -# -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in all -# copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -# SOFTWARE. - -################################################################################ -# Imports -################################################################################ -import os -import sys -import platform - -Import("env") # pylint: disable=undefined-variable - -################################################################################ -# Variables -################################################################################ -OS_PLATFORM_TYPE_WIN = "Windows" -OS_PLATFORM_TYPE_LINUX = "Linux" -OS_PLATFORM_TYPE_MACOS = "Darwin" -OS_PLATFORM_TYPE = platform.system() -ROBOT_NAME = env.GetProjectOption("webots_robot_name") # pylint: disable=undefined-variable -ROBOT_SERIAL_RX_CHANNEL = env.GetProjectOption("webots_robot_serial_rx_channel") # pylint: disable=undefined-variable -ROBOT_SERIAL_TX_CHANNEL = env.GetProjectOption("webots_robot_serial_tx_channel") # pylint: disable=undefined-variable -PROGRAM_PATH = "$BUILD_DIR/" -PROGRAM_OPTIONS = '' -PROGRAM_OPTIONS_ZUMO_COM_SYSTEM = '-c ' \ - + '--serialRxCh ' + ROBOT_SERIAL_RX_CHANNEL + ' ' \ - + '--serialTxCh ' + ROBOT_SERIAL_TX_CHANNEL + ' ' \ - + '-v' -WEBOTS_CONTROLLER_OPTIONS = '--robot-name=' + ROBOT_NAME + ' --stdout-redirect' - -if OS_PLATFORM_TYPE == OS_PLATFORM_TYPE_WIN: - - WEBOTS_HOME = os.getenv('WEBOTS_HOME').replace('\\', '/') - WEBOTS_CONTROLLER = '"' + WEBOTS_HOME + '/msys64/mingw64/bin/webots-controller.exe"' - PROGRAM_NAME = "${PROGNAME}.exe" - -elif OS_PLATFORM_TYPE == OS_PLATFORM_TYPE_LINUX: - - WEBOTS_CONTROLLER = "$WEBOTS_HOME/webots-controller" - PROGRAM_NAME = "${PROGNAME}" - -elif OS_PLATFORM_TYPE == OS_PLATFORM_TYPE_MACOS: - - WEBOTS_CONTROLLER = "$WEBOTS_HOME/Contents/MacOS/webots-controller" - PROGRAM_NAME = "${PROGNAME}" - -else: - print(f"OS type {OS_PLATFORM_TYPE} not supported.") - sys.exit(1) - -WEBOTS_LAUNCHER_ACTION = WEBOTS_CONTROLLER + ' '\ - + WEBOTS_CONTROLLER_OPTIONS + ' ' \ - + PROGRAM_PATH + PROGRAM_NAME + ' ' \ - + PROGRAM_OPTIONS - -WEBOTS_LAUNCHER_ZUMO_COM_SYSTEM_ACTION = WEBOTS_CONTROLLER + ' ' \ - + WEBOTS_CONTROLLER_OPTIONS + ' ' \ - + PROGRAM_PATH + PROGRAM_NAME + ' ' \ - + PROGRAM_OPTIONS_ZUMO_COM_SYSTEM - -################################################################################ -# Classes -################################################################################ - -################################################################################ -# Functions -################################################################################ - -################################################################################ -# Main -################################################################################ - -# pylint: disable=undefined-variable -env.AddCustomTarget( - name="webots_launcher", - dependencies=PROGRAM_PATH + PROGRAM_NAME, - actions=[ - WEBOTS_LAUNCHER_ACTION - ], - title="Launch alone", - description="Launch application with Webots launcher and without ZumoComSystem." -) - -# pylint: disable=undefined-variable -env.AddCustomTarget( - name="webots_launcher_zumo_com_system", - dependencies=PROGRAM_PATH + PROGRAM_NAME, - actions=[ - WEBOTS_LAUNCHER_ZUMO_COM_SYSTEM_ACTION - ], - title="Launch with ZumoComSystem", - description="Launch application with Webots launcher and with ZumoComSystem." -) +"""Webots launcher""" + +# MIT License +# +# Copyright (c) 2022 - 2024 Andreas Merkle (web@blue-andi.de) +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. + +################################################################################ +# Imports +################################################################################ +import os +import sys +import platform + +Import("env") # pylint: disable=undefined-variable + +################################################################################ +# Variables +################################################################################ +OS_PLATFORM_TYPE_WIN = "Windows" +OS_PLATFORM_TYPE_LINUX = "Linux" +OS_PLATFORM_TYPE_MACOS = "Darwin" +OS_PLATFORM_TYPE = platform.system() +ROBOT_NAME = env.GetProjectOption("webots_robot_name") # pylint: disable=undefined-variable +ROBOT_SERIAL_RX_CHANNEL = env.GetProjectOption("webots_robot_serial_rx_channel") # pylint: disable=undefined-variable +ROBOT_SERIAL_TX_CHANNEL = env.GetProjectOption("webots_robot_serial_tx_channel") # pylint: disable=undefined-variable +PROGRAM_PATH = "$BUILD_DIR/" +PROGRAM_OPTIONS = '' +PROGRAM_OPTIONS_ZUMO_COM_SYSTEM = '-c ' \ + + '--serialRxCh ' + ROBOT_SERIAL_RX_CHANNEL + ' ' \ + + '--serialTxCh ' + ROBOT_SERIAL_TX_CHANNEL + ' ' \ + + '-v' +WEBOTS_CONTROLLER_OPTIONS = '--robot-name=' + ROBOT_NAME + ' --stdout-redirect' + +if OS_PLATFORM_TYPE == OS_PLATFORM_TYPE_WIN: + + WEBOTS_HOME = os.getenv('WEBOTS_HOME').replace('\\', '/') + WEBOTS_CONTROLLER = '"' + WEBOTS_HOME + '/msys64/mingw64/bin/webots-controller.exe"' + PROGRAM_NAME = "${PROGNAME}.exe" + +elif OS_PLATFORM_TYPE == OS_PLATFORM_TYPE_LINUX: + + WEBOTS_CONTROLLER = "$WEBOTS_HOME/webots-controller" + PROGRAM_NAME = "${PROGNAME}" + +elif OS_PLATFORM_TYPE == OS_PLATFORM_TYPE_MACOS: + + WEBOTS_CONTROLLER = "$WEBOTS_HOME/Contents/MacOS/webots-controller" + PROGRAM_NAME = "${PROGNAME}" + +else: + print(f"OS type {OS_PLATFORM_TYPE} not supported.") + sys.exit(1) + +WEBOTS_LAUNCHER_ACTION = WEBOTS_CONTROLLER + ' '\ + + WEBOTS_CONTROLLER_OPTIONS + ' ' \ + + PROGRAM_PATH + PROGRAM_NAME + ' ' \ + + PROGRAM_OPTIONS + +WEBOTS_LAUNCHER_ZUMO_COM_SYSTEM_ACTION = WEBOTS_CONTROLLER + ' ' \ + + WEBOTS_CONTROLLER_OPTIONS + ' ' \ + + PROGRAM_PATH + PROGRAM_NAME + ' ' \ + + PROGRAM_OPTIONS_ZUMO_COM_SYSTEM + +################################################################################ +# Classes +################################################################################ + +################################################################################ +# Functions +################################################################################ + +################################################################################ +# Main +################################################################################ + +# pylint: disable=undefined-variable +env.AddCustomTarget( + name="webots_launcher", + dependencies=PROGRAM_PATH + PROGRAM_NAME, + actions=[ + WEBOTS_LAUNCHER_ACTION + ], + title="Launch alone", + description="Launch application with Webots launcher and without ZumoComSystem." +) + +# pylint: disable=undefined-variable +env.AddCustomTarget( + name="webots_launcher_zumo_com_system", + dependencies=PROGRAM_PATH + PROGRAM_NAME, + actions=[ + WEBOTS_LAUNCHER_ZUMO_COM_SYSTEM_ACTION + ], + title="Launch with ZumoComSystem", + description="Launch application with Webots launcher and with ZumoComSystem." +) diff --git a/src/main.cpp b/src/main.cpp index c79ea16d..292b73ec 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,80 +1,80 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Main entry point - * @author Andreas Merkle - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Variables - *****************************************************************************/ - -/** The main application. */ -static App gApplication; - -/****************************************************************************** - * External functions - *****************************************************************************/ - -/** - * Initialize the system. - * This function is called once during startup. - */ -void setup() // cppcheck-suppress unusedFunction -{ - gApplication.setup(); -} - -/** - * Main program loop. - * This function is called cyclic. - */ -void loop() // cppcheck-suppress unusedFunction -{ - gApplication.loop(); -} - -/****************************************************************************** - * Local functions - *****************************************************************************/ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Main entry point + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Variables + *****************************************************************************/ + +/** The main application. */ +static App gApplication; + +/****************************************************************************** + * External functions + *****************************************************************************/ + +/** + * Initialize the system. + * This function is called once during startup. + */ +void setup() // cppcheck-suppress unusedFunction +{ + gApplication.setup(); +} + +/** + * Main program loop. + * This function is called cyclic. + */ +void loop() // cppcheck-suppress unusedFunction +{ + gApplication.loop(); +} + +/****************************************************************************** + * Local functions + *****************************************************************************/ diff --git a/start_platoon.bat b/start_platoon.bat index bda9084c..7d7153bf 100644 --- a/start_platoon.bat +++ b/start_platoon.bat @@ -1,100 +1,100 @@ -@echo off -setlocal - -rem Change working directory to the directory of the batch script. -cd /d %~dp0 - -set WEBOTS_CONTROLLER="%WEBOTS_HOME%\msys64\mingw64\bin\webots-controller.exe" -set PROGRAM_NAME=program.exe - -set CONVOY_LEADER_PIO_ENV_NAME=ConvoyLeaderSim -set CONVOY_LEADER_PIO_PATH=.pio\build\%CONVOY_LEADER_PIO_ENV_NAME% -set CONVOY_LEADER_ROBOT_NAME=leader -set CONVOY_LEADER_RX_CHANNEL=1 -set CONVOY_LEADER_TX_CHANNEL=2 - -set CONVOY_FOLLOWER_PIO_ENV_NAME=ConvoyFollowerSim -set CONVOY_FOLLOWER_PIO_PATH=.pio\build\%CONVOY_FOLLOWER_PIO_ENV_NAME% -set CONVOY_FOLLOWER_ROBOT_NAME=follower_ - -set TEMP_PLATOON_PATH=tmp -set TEMP_PLATOON_LEADER_PATH=%TEMP_PLATOON_PATH%\%CONVOY_LEADER_PIO_ENV_NAME% -set TEMP_PLATOON_LEADER_NEW_PATH=%TEMP_PLATOON_PATH%\%CONVOY_LEADER_ROBOT_NAME% -set TEMP_PLATOON_FOLLOWER_PATH=%TEMP_PLATOON_PATH%\%CONVOY_FOLLOWER_PIO_ENV_NAME% -set TEMP_PLATOON_FOLLOWER_NEW_PATH=%TEMP_PLATOON_PATH%\%CONVOY_FOLLOWER_ROBOT_NAME% - -rem Compile Convoy Leader -%USERPROFILE%\.platformio\penv\Scripts\pio.exe run --environment %CONVOY_LEADER_PIO_ENV_NAME% - -rem Compile Convoy Follower -%USERPROFILE%\.platformio\penv\Scripts\pio.exe run --environment %CONVOY_FOLLOWER_PIO_ENV_NAME% - -rem If temporary folder doesn't exist, it will be created. -if not exist %TEMP_PLATOON_PATH%\ ( - md %TEMP_PLATOON_PATH% -) - -rem Unzip leader to temporary folder -if exist %TEMP_PLATOON_LEADER_PATH%\ ( - rmdir /s /q %TEMP_PLATOON_LEADER_PATH%\ -) -Call :UnZipFile "%~dp0%TEMP_PLATOON_PATH%\" "%~dp0%CONVOY_LEADER_PIO_PATH%\%CONVOY_LEADER_PIO_ENV_NAME%.zip" - -rem Unzip leader to temporary folder -if exist %TEMP_PLATOON_FOLLOWER_PATH%\ ( - rmdir /s /q %TEMP_PLATOON_FOLLOWER_PATH%\ -) -Call :UnZipFile "%~dp0%TEMP_PLATOON_PATH%\" "%~dp0%CONVOY_FOLLOWER_PIO_PATH%\%CONVOY_FOLLOWER_PIO_ENV_NAME%.zip" - -rem Copy leader folder according to a new folder with the robot's name. -rem The copy ensures that a existing settings.json will be kept. -copy /y %TEMP_PLATOON_LEADER_PATH% %TEMP_PLATOON_LEADER_NEW_PATH% - -rem Copy follower folder according to a new folder with the robot's name. -rem The copy ensures that a existing settings.json will be kept. -for /L %%i in (1, 1, 2) do ( - if exist %TEMP_PLATOON_FOLLOWER_PATH%\ ( - rmdir /s /q %TEMP_PLATOON_FOLLOWER_PATH%\ - ) - copy /y %TEMP_PLATOON_FOLLOWER_PATH% %TEMP_PLATOON_FOLLOWER_NEW_PATH%%%i -) - -rem Delete extraced files from .zip -rmdir /s /q %TEMP_PLATOON_LEADER_PATH% -rmdir /s /q %TEMP_PLATOON_FOLLOWER_PATH% - -rem Start the convoy leader -echo Start convoy leader. -start "Convoy Leader" ""%WEBOTS_CONTROLLER%"" --robot-name=%CONVOY_LEADER_ROBOT_NAME% --stdout-redirect %TEMP_PLATOON_LEADER_NEW_PATH%\%PROGRAM_NAME% -n %CONVOY_LEADER_ROBOT_NAME% -c --serialRxCh=%CONVOY_LEADER_RX_CHANNEL% --serialTxCh=%CONVOY_LEADER_TX_CHANNEL% -v" - -rem Start the followers -for /L %%i in (1, 1, 2) do ( - echo Start convoy follower %%i. - call :StartFollower %%i -) - -exit /b - -:StartFollower -set INSTANCE=%1 -set ROBOT_NAME=%CONVOY_FOLLOWER_ROBOT_NAME%%instance% -set /a SERIAL_RX_CHANNEL=(INSTANCE * 2) + 1 -set /a SERIAL_TX_CHANNEL=(INSTANCE * 2) + 2 -echo Start convoy follower %INSTANCE%. -start "Convoy Follower %INSTANCE%" ""%WEBOTS_CONTROLLER%"" --robot-name=%ROBOT_NAME% --stdout-redirect %TEMP_PLATOON_FOLLOWER_NEW_PATH%%INSTANCE%\%PROGRAM_NAME% -n %ROBOT_NAME% -c --serialRxCh=%SERIAL_RX_CHANNEL% --serialTxCh=%SERIAL_TX_CHANNEL% -v" -exit /b - -:UnZipFile -SET vbs="%TEMP%\_.vbs" -IF EXIST %vbs% DEL /f /q %vbs% ->%vbs% ECHO Set fso = CreateObject("Scripting.FileSystemObject") ->>%vbs% ECHO If NOT fso.FolderExists(%1) Then ->>%vbs% ECHO fso.CreateFolder(%1) ->>%vbs% ECHO End If ->>%vbs% ECHO set objShell = CreateObject("Shell.Application") ->>%vbs% ECHO set FilesInZip=objShell.NameSpace(%2).items ->>%vbs% ECHO objShell.NameSpace(%1).CopyHere(FilesInZip) ->>%vbs% ECHO Set fso = Nothing ->>%vbs% ECHO Set objShell = Nothing -cscript //nologo %vbs% -IF EXIST %vbs% DEL /f /q %vbs% +@echo off +setlocal + +rem Change working directory to the directory of the batch script. +cd /d %~dp0 + +set WEBOTS_CONTROLLER="%WEBOTS_HOME%\msys64\mingw64\bin\webots-controller.exe" +set PROGRAM_NAME=program.exe + +set CONVOY_LEADER_PIO_ENV_NAME=ConvoyLeaderSim +set CONVOY_LEADER_PIO_PATH=.pio\build\%CONVOY_LEADER_PIO_ENV_NAME% +set CONVOY_LEADER_ROBOT_NAME=leader +set CONVOY_LEADER_RX_CHANNEL=1 +set CONVOY_LEADER_TX_CHANNEL=2 + +set CONVOY_FOLLOWER_PIO_ENV_NAME=ConvoyFollowerSim +set CONVOY_FOLLOWER_PIO_PATH=.pio\build\%CONVOY_FOLLOWER_PIO_ENV_NAME% +set CONVOY_FOLLOWER_ROBOT_NAME=follower_ + +set TEMP_PLATOON_PATH=tmp +set TEMP_PLATOON_LEADER_PATH=%TEMP_PLATOON_PATH%\%CONVOY_LEADER_PIO_ENV_NAME% +set TEMP_PLATOON_LEADER_NEW_PATH=%TEMP_PLATOON_PATH%\%CONVOY_LEADER_ROBOT_NAME% +set TEMP_PLATOON_FOLLOWER_PATH=%TEMP_PLATOON_PATH%\%CONVOY_FOLLOWER_PIO_ENV_NAME% +set TEMP_PLATOON_FOLLOWER_NEW_PATH=%TEMP_PLATOON_PATH%\%CONVOY_FOLLOWER_ROBOT_NAME% + +rem Compile Convoy Leader +%USERPROFILE%\.platformio\penv\Scripts\pio.exe run --environment %CONVOY_LEADER_PIO_ENV_NAME% + +rem Compile Convoy Follower +%USERPROFILE%\.platformio\penv\Scripts\pio.exe run --environment %CONVOY_FOLLOWER_PIO_ENV_NAME% + +rem If temporary folder doesn't exist, it will be created. +if not exist %TEMP_PLATOON_PATH%\ ( + md %TEMP_PLATOON_PATH% +) + +rem Unzip leader to temporary folder +if exist %TEMP_PLATOON_LEADER_PATH%\ ( + rmdir /s /q %TEMP_PLATOON_LEADER_PATH%\ +) +Call :UnZipFile "%~dp0%TEMP_PLATOON_PATH%\" "%~dp0%CONVOY_LEADER_PIO_PATH%\%CONVOY_LEADER_PIO_ENV_NAME%.zip" + +rem Unzip leader to temporary folder +if exist %TEMP_PLATOON_FOLLOWER_PATH%\ ( + rmdir /s /q %TEMP_PLATOON_FOLLOWER_PATH%\ +) +Call :UnZipFile "%~dp0%TEMP_PLATOON_PATH%\" "%~dp0%CONVOY_FOLLOWER_PIO_PATH%\%CONVOY_FOLLOWER_PIO_ENV_NAME%.zip" + +rem Copy leader folder according to a new folder with the robot's name. +rem The copy ensures that a existing settings.json will be kept. +copy /y %TEMP_PLATOON_LEADER_PATH% %TEMP_PLATOON_LEADER_NEW_PATH% + +rem Copy follower folder according to a new folder with the robot's name. +rem The copy ensures that a existing settings.json will be kept. +for /L %%i in (1, 1, 2) do ( + if exist %TEMP_PLATOON_FOLLOWER_PATH%\ ( + rmdir /s /q %TEMP_PLATOON_FOLLOWER_PATH%\ + ) + copy /y %TEMP_PLATOON_FOLLOWER_PATH% %TEMP_PLATOON_FOLLOWER_NEW_PATH%%%i +) + +rem Delete extraced files from .zip +rmdir /s /q %TEMP_PLATOON_LEADER_PATH% +rmdir /s /q %TEMP_PLATOON_FOLLOWER_PATH% + +rem Start the convoy leader +echo Start convoy leader. +start "Convoy Leader" ""%WEBOTS_CONTROLLER%"" --robot-name=%CONVOY_LEADER_ROBOT_NAME% --stdout-redirect %TEMP_PLATOON_LEADER_NEW_PATH%\%PROGRAM_NAME% -n %CONVOY_LEADER_ROBOT_NAME% -c --serialRxCh=%CONVOY_LEADER_RX_CHANNEL% --serialTxCh=%CONVOY_LEADER_TX_CHANNEL% -v" + +rem Start the followers +for /L %%i in (1, 1, 2) do ( + echo Start convoy follower %%i. + call :StartFollower %%i +) + +exit /b + +:StartFollower +set INSTANCE=%1 +set ROBOT_NAME=%CONVOY_FOLLOWER_ROBOT_NAME%%instance% +set /a SERIAL_RX_CHANNEL=(INSTANCE * 2) + 1 +set /a SERIAL_TX_CHANNEL=(INSTANCE * 2) + 2 +echo Start convoy follower %INSTANCE%. +start "Convoy Follower %INSTANCE%" ""%WEBOTS_CONTROLLER%"" --robot-name=%ROBOT_NAME% --stdout-redirect %TEMP_PLATOON_FOLLOWER_NEW_PATH%%INSTANCE%\%PROGRAM_NAME% -n %ROBOT_NAME% -c --serialRxCh=%SERIAL_RX_CHANNEL% --serialTxCh=%SERIAL_TX_CHANNEL% -v" +exit /b + +:UnZipFile +SET vbs="%TEMP%\_.vbs" +IF EXIST %vbs% DEL /f /q %vbs% +>%vbs% ECHO Set fso = CreateObject("Scripting.FileSystemObject") +>>%vbs% ECHO If NOT fso.FolderExists(%1) Then +>>%vbs% ECHO fso.CreateFolder(%1) +>>%vbs% ECHO End If +>>%vbs% ECHO set objShell = CreateObject("Shell.Application") +>>%vbs% ECHO set FilesInZip=objShell.NameSpace(%2).items +>>%vbs% ECHO objShell.NameSpace(%1).CopyHere(FilesInZip) +>>%vbs% ECHO Set fso = Nothing +>>%vbs% ECHO Set objShell = Nothing +cscript //nologo %vbs% +IF EXIST %vbs% DEL /f /q %vbs% diff --git a/test/test_Odometry/test_Odometry.cpp b/test/test_Odometry/test_Odometry.cpp index 33d6a4e2..c5e09f6a 100644 --- a/test/test_Odometry/test_Odometry.cpp +++ b/test/test_Odometry/test_Odometry.cpp @@ -1,326 +1,326 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/** - * @author Andreas Merkle - * @brief This module contains the program entry point for the tests. - */ - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ - -#include -#include -#include -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -static void testOrientation(); -static void testPosition(); -static float calcStepsToAngle(float angle, bool turnInPlace); -static float calcStepsToDistance(float distance); -static void testTurnInPlace(float angle); -static void testTurn(float angle); -static void testDriveDistance(float distance); - -/****************************************************************************** - * 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(testOrientation); - RUN_TEST(testPosition); - - 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 - *****************************************************************************/ - -static void testOrientation() -{ - testTurnInPlace(0); - testTurnInPlace(M_PI / 2.0f); - testTurnInPlace(M_PI); - testTurnInPlace(M_PI * 3.0f / 2.0f); - testTurnInPlace(M_PI * 2.0f); - testTurnInPlace(-M_PI / 2.0f); - testTurnInPlace(-M_PI); - testTurnInPlace(-M_PI * 3.0f / 2.0f); - testTurnInPlace(-M_PI * 2.0f); - - testTurn(0); - testTurn(M_PI / 2.0f); - testTurn(M_PI); - testTurn(M_PI * 3.0f / 2.0f); - testTurn(M_PI * 2.0f); - testTurn(-M_PI / 2.0f); - testTurn(-M_PI); - testTurn(-M_PI * 3.0f / 2.0f); - testTurn(-M_PI * 2.0f); -} - -static void testPosition() -{ - testDriveDistance(0); - testDriveDistance(10); - testDriveDistance(200); - testDriveDistance(1000); - testDriveDistance(static_cast(INT16_MAX / 2) * 1000.0f / - static_cast(RobotConstants::ENCODER_STEPS_PER_M)); - testDriveDistance(-10); - testDriveDistance(-200); - testDriveDistance(-1000); - testDriveDistance(static_cast(INT16_MIN / 2) * 1000.0f / - static_cast(RobotConstants::ENCODER_STEPS_PER_M)); -} - -static float calcStepsToAngle(float angle, bool turnInPlace) -{ - float wheelBase = static_cast(RobotConstants::WHEEL_BASE); /* [mm] */ - float diameter = (false == turnInPlace) ? 2.0f * wheelBase : wheelBase; /* [mm] */ - float stepsPerMM = static_cast(RobotConstants::ENCODER_STEPS_PER_M) / 1000.0f; /* [steps/mm] */ - float circumference = M_PI * diameter; /* [mm] */ - - return circumference * stepsPerMM * angle / (2 * M_PI); -} - -static float calcStepsToDistance(float distance) -{ - float stepsPerMM = static_cast(RobotConstants::ENCODER_STEPS_PER_M) / 1000.0f; /* [steps/mm] */ - - return stepsPerMM * distance; -} - -static void testTurnInPlace(float angle) -{ - IEncodersTest& encodersTest = Board::getInstance().getEncodersTest(); - Odometry& odometry = Odometry::getInstance(); - float stepsToAngle = calcStepsToAngle(angle, true); - int16_t stepsLeft = -static_cast(stepsToAngle); - int16_t stepsRight = static_cast(stepsToAngle); - float angleEpsilon = M_PI * 1.0f / 180.0f; /* 1° epsilon in rad */ - int32_t posX = 0; - int32_t posY = 0; - - TEST_ASSERT(static_cast(INT16_MAX) >= stepsToAngle); - TEST_ASSERT(static_cast(INT16_MIN) <= stepsToAngle); - - encodersTest.setCountsLeft(0); - encodersTest.setCountsRight(0); - odometry.clearPosition(); - odometry.clearMileage(); - odometry.setOrientation(0); - TEST_ASSERT_EQUAL_INT32(0, odometry.getOrientation()); - - encodersTest.setCountsLeft(stepsLeft); - encodersTest.setCountsRight(stepsRight); - odometry.process(); - - TEST_ASSERT_EQUAL_UINT32(0, odometry.getMileageCenter()); - odometry.getPosition(posX, posY); - TEST_ASSERT_EQUAL_INT32(0, posX); - TEST_ASSERT_EQUAL_INT32(0, posY); - - if (0.001 > fabs(angle)) - { - TEST_ASSERT_TRUE(odometry.isStandStill()); - } - else - { - TEST_ASSERT_FALSE(odometry.isStandStill()); - } - - TEST_ASSERT_INT32_WITHIN(static_cast(angleEpsilon * 1000.0f), static_cast(angle * 1000.0f), - odometry.getOrientation()); -} - -static void testTurn(float angle) -{ - IEncodersTest& encodersTest = Board::getInstance().getEncodersTest(); - Odometry& odometry = Odometry::getInstance(); - float stepsToAngle = calcStepsToAngle(angle, false); - int16_t stepsLeft = (0 <= angle) ? 0 : static_cast(stepsToAngle); - int16_t stepsRight = (0 <= angle) ? static_cast(stepsToAngle) : 0; - float angleEpsilon = M_PI * 1.0f / 180.0f; /* 1° epsilon in rad */ - - TEST_ASSERT(static_cast(INT16_MAX) >= stepsToAngle); - TEST_ASSERT(static_cast(INT16_MIN) <= stepsToAngle); - - encodersTest.setCountsLeft(0); - encodersTest.setCountsRight(0); - odometry.clearPosition(); - odometry.clearMileage(); - odometry.setOrientation(0); - TEST_ASSERT_EQUAL_INT32(0, odometry.getOrientation()); - - encodersTest.setCountsLeft(stepsLeft); - encodersTest.setCountsRight(stepsRight); - odometry.process(); - - /* Standstill detection has debouncing. */ - delay(10U); - odometry.process(); - - if (0.001 > fabs(angle)) - { - TEST_ASSERT_TRUE(odometry.isStandStill()); - } - else - { - TEST_ASSERT_FALSE(odometry.isStandStill()); - } - - TEST_ASSERT_INT32_WITHIN(static_cast(angleEpsilon * 1000.0f), static_cast(fabs(angle) * 1000.0f), - odometry.getOrientation()); -} - -static void testDriveDistance(float distance) -{ - IEncodersTest& encodersTest = Board::getInstance().getEncodersTest(); - Odometry& odometry = Odometry::getInstance(); - int32_t posX = 0; - int32_t posY = 0; - float stepsToDistance = calcStepsToDistance(distance); - - encodersTest.setCountsLeft(0); - encodersTest.setCountsRight(0); - odometry.clearPosition(); - odometry.clearMileage(); - odometry.setOrientation(0); - odometry.getPosition(posX, posY); - TEST_ASSERT_EQUAL_INT32(0, odometry.getOrientation()); - TEST_ASSERT_EQUAL_INT32(0, posX); - TEST_ASSERT_EQUAL_INT32(0, posY); - - encodersTest.setCountsLeft(stepsToDistance); - encodersTest.setCountsRight(stepsToDistance); - odometry.process(); - - /* Standstill detection has debouncing. */ - delay(10U); - odometry.process(); - - if (0.001f > fabs(distance)) - { - TEST_ASSERT_TRUE(odometry.isStandStill()); - } - else - { - TEST_ASSERT_FALSE(odometry.isStandStill()); - } - - if (0.0F <= distance) - { - distance += 0.5F; - } - else - { - distance -= 0.5F; - } - - TEST_ASSERT_EQUAL_UINT32(static_cast(fabs(distance)), odometry.getMileageCenter()); - - odometry.getPosition(posX, posY); - TEST_ASSERT_EQUAL_INT32(static_cast(distance), posX); - TEST_ASSERT_EQUAL_INT32(0, posY); -} +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/** + * @author Andreas Merkle + * @brief This module contains the program entry point for the tests. + */ + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ + +#include +#include +#include +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +static void testOrientation(); +static void testPosition(); +static float calcStepsToAngle(float angle, bool turnInPlace); +static float calcStepsToDistance(float distance); +static void testTurnInPlace(float angle); +static void testTurn(float angle); +static void testDriveDistance(float distance); + +/****************************************************************************** + * 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(testOrientation); + RUN_TEST(testPosition); + + 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 + *****************************************************************************/ + +static void testOrientation() +{ + testTurnInPlace(0); + testTurnInPlace(M_PI / 2.0f); + testTurnInPlace(M_PI); + testTurnInPlace(M_PI * 3.0f / 2.0f); + testTurnInPlace(M_PI * 2.0f); + testTurnInPlace(-M_PI / 2.0f); + testTurnInPlace(-M_PI); + testTurnInPlace(-M_PI * 3.0f / 2.0f); + testTurnInPlace(-M_PI * 2.0f); + + testTurn(0); + testTurn(M_PI / 2.0f); + testTurn(M_PI); + testTurn(M_PI * 3.0f / 2.0f); + testTurn(M_PI * 2.0f); + testTurn(-M_PI / 2.0f); + testTurn(-M_PI); + testTurn(-M_PI * 3.0f / 2.0f); + testTurn(-M_PI * 2.0f); +} + +static void testPosition() +{ + testDriveDistance(0); + testDriveDistance(10); + testDriveDistance(200); + testDriveDistance(1000); + testDriveDistance(static_cast(INT16_MAX / 2) * 1000.0f / + static_cast(RobotConstants::ENCODER_STEPS_PER_M)); + testDriveDistance(-10); + testDriveDistance(-200); + testDriveDistance(-1000); + testDriveDistance(static_cast(INT16_MIN / 2) * 1000.0f / + static_cast(RobotConstants::ENCODER_STEPS_PER_M)); +} + +static float calcStepsToAngle(float angle, bool turnInPlace) +{ + float wheelBase = static_cast(RobotConstants::WHEEL_BASE); /* [mm] */ + float diameter = (false == turnInPlace) ? 2.0f * wheelBase : wheelBase; /* [mm] */ + float stepsPerMM = static_cast(RobotConstants::ENCODER_STEPS_PER_M) / 1000.0f; /* [steps/mm] */ + float circumference = M_PI * diameter; /* [mm] */ + + return circumference * stepsPerMM * angle / (2 * M_PI); +} + +static float calcStepsToDistance(float distance) +{ + float stepsPerMM = static_cast(RobotConstants::ENCODER_STEPS_PER_M) / 1000.0f; /* [steps/mm] */ + + return stepsPerMM * distance; +} + +static void testTurnInPlace(float angle) +{ + IEncodersTest& encodersTest = Board::getInstance().getEncodersTest(); + Odometry& odometry = Odometry::getInstance(); + float stepsToAngle = calcStepsToAngle(angle, true); + int16_t stepsLeft = -static_cast(stepsToAngle); + int16_t stepsRight = static_cast(stepsToAngle); + float angleEpsilon = M_PI * 1.0f / 180.0f; /* 1° epsilon in rad */ + int32_t posX = 0; + int32_t posY = 0; + + TEST_ASSERT(static_cast(INT16_MAX) >= stepsToAngle); + TEST_ASSERT(static_cast(INT16_MIN) <= stepsToAngle); + + encodersTest.setCountsLeft(0); + encodersTest.setCountsRight(0); + odometry.clearPosition(); + odometry.clearMileage(); + odometry.setOrientation(0); + TEST_ASSERT_EQUAL_INT32(0, odometry.getOrientation()); + + encodersTest.setCountsLeft(stepsLeft); + encodersTest.setCountsRight(stepsRight); + odometry.process(); + + TEST_ASSERT_EQUAL_UINT32(0, odometry.getMileageCenter()); + odometry.getPosition(posX, posY); + TEST_ASSERT_EQUAL_INT32(0, posX); + TEST_ASSERT_EQUAL_INT32(0, posY); + + if (0.001 > fabs(angle)) + { + TEST_ASSERT_TRUE(odometry.isStandStill()); + } + else + { + TEST_ASSERT_FALSE(odometry.isStandStill()); + } + + TEST_ASSERT_INT32_WITHIN(static_cast(angleEpsilon * 1000.0f), static_cast(angle * 1000.0f), + odometry.getOrientation()); +} + +static void testTurn(float angle) +{ + IEncodersTest& encodersTest = Board::getInstance().getEncodersTest(); + Odometry& odometry = Odometry::getInstance(); + float stepsToAngle = calcStepsToAngle(angle, false); + int16_t stepsLeft = (0 <= angle) ? 0 : static_cast(stepsToAngle); + int16_t stepsRight = (0 <= angle) ? static_cast(stepsToAngle) : 0; + float angleEpsilon = M_PI * 1.0f / 180.0f; /* 1° epsilon in rad */ + + TEST_ASSERT(static_cast(INT16_MAX) >= stepsToAngle); + TEST_ASSERT(static_cast(INT16_MIN) <= stepsToAngle); + + encodersTest.setCountsLeft(0); + encodersTest.setCountsRight(0); + odometry.clearPosition(); + odometry.clearMileage(); + odometry.setOrientation(0); + TEST_ASSERT_EQUAL_INT32(0, odometry.getOrientation()); + + encodersTest.setCountsLeft(stepsLeft); + encodersTest.setCountsRight(stepsRight); + odometry.process(); + + /* Standstill detection has debouncing. */ + delay(10U); + odometry.process(); + + if (0.001 > fabs(angle)) + { + TEST_ASSERT_TRUE(odometry.isStandStill()); + } + else + { + TEST_ASSERT_FALSE(odometry.isStandStill()); + } + + TEST_ASSERT_INT32_WITHIN(static_cast(angleEpsilon * 1000.0f), static_cast(fabs(angle) * 1000.0f), + odometry.getOrientation()); +} + +static void testDriveDistance(float distance) +{ + IEncodersTest& encodersTest = Board::getInstance().getEncodersTest(); + Odometry& odometry = Odometry::getInstance(); + int32_t posX = 0; + int32_t posY = 0; + float stepsToDistance = calcStepsToDistance(distance); + + encodersTest.setCountsLeft(0); + encodersTest.setCountsRight(0); + odometry.clearPosition(); + odometry.clearMileage(); + odometry.setOrientation(0); + odometry.getPosition(posX, posY); + TEST_ASSERT_EQUAL_INT32(0, odometry.getOrientation()); + TEST_ASSERT_EQUAL_INT32(0, posX); + TEST_ASSERT_EQUAL_INT32(0, posY); + + encodersTest.setCountsLeft(stepsToDistance); + encodersTest.setCountsRight(stepsToDistance); + odometry.process(); + + /* Standstill detection has debouncing. */ + delay(10U); + odometry.process(); + + if (0.001f > fabs(distance)) + { + TEST_ASSERT_TRUE(odometry.isStandStill()); + } + else + { + TEST_ASSERT_FALSE(odometry.isStandStill()); + } + + if (0.0F <= distance) + { + distance += 0.5F; + } + else + { + distance -= 0.5F; + } + + TEST_ASSERT_EQUAL_UINT32(static_cast(fabs(distance)), odometry.getMileageCenter()); + + odometry.getPosition(posX, posY); + TEST_ASSERT_EQUAL_INT32(static_cast(distance), posX); + TEST_ASSERT_EQUAL_INT32(0, posY); +} diff --git a/test/test_PIDController/test_PIDController.cpp b/test/test_PIDController/test_PIDController.cpp index 4e9a5e83..67279507 100644 --- a/test/test_PIDController/test_PIDController.cpp +++ b/test/test_PIDController/test_PIDController.cpp @@ -1,183 +1,183 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/** - * @author Andreas Merkle - * @brief This module contains the PID Controller tests. - */ - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -static void testPIDController(); - -/****************************************************************************** - * 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(testPIDController); - - 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 PICController class. - */ -static void testPIDController() -{ - PIDController pidCtrl; - uint8_t index = 0; - int16_t output = 0; - const uint32_t TEST_SAMPLE_TIME = 0; /* Every PID calculate() call shall be processed */ - - /* Sample time for all tests shall be set once. */ - pidCtrl.setSampleTime(TEST_SAMPLE_TIME); - - /* Kp = 1, Ki = 0, Kd = 0 */ - pidCtrl.setPFactor(1, 1); - pidCtrl.setIFactor(0, 1); - pidCtrl.setDFactor(0, 1); - pidCtrl.clear(); - - /* Output must follow error */ - output = 0; - for (index = 0; index < 10; ++index) - { - output = 0 - index; - TEST_ASSERT_EQUAL_INT16(output, pidCtrl.calculate(0, index)); - } - - /* Kp = 0, Ki = 1, Kd = 0 */ - pidCtrl.setPFactor(0, 1); - pidCtrl.setIFactor(1, 1); - pidCtrl.setDFactor(0, 1); - pidCtrl.clear(); - - /* Output must increase per error */ - output = 0; - for (index = 0; index < 10; ++index) - { - output += (0 - index); - TEST_ASSERT_EQUAL_INT16(output, pidCtrl.calculate(0, index)); - } - - /* Kp = 0, Ki = 0, Kd = 1 */ - pidCtrl.setPFactor(0, 1); - pidCtrl.setIFactor(0, 1); - pidCtrl.setDFactor(1, 1); - pidCtrl.clear(); - - /* Output must be equal to error deviation from previous error */ - output = 0; - for (index = 1; index < 10; ++index) - { - output = -1; - TEST_ASSERT_EQUAL_INT16(output, pidCtrl.calculate(0, index)); - } -} +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/** + * @author Andreas Merkle + * @brief This module contains the PID Controller tests. + */ + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +static void testPIDController(); + +/****************************************************************************** + * 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(testPIDController); + + 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 PICController class. + */ +static void testPIDController() +{ + PIDController pidCtrl; + uint8_t index = 0; + int16_t output = 0; + const uint32_t TEST_SAMPLE_TIME = 0; /* Every PID calculate() call shall be processed */ + + /* Sample time for all tests shall be set once. */ + pidCtrl.setSampleTime(TEST_SAMPLE_TIME); + + /* Kp = 1, Ki = 0, Kd = 0 */ + pidCtrl.setPFactor(1, 1); + pidCtrl.setIFactor(0, 1); + pidCtrl.setDFactor(0, 1); + pidCtrl.clear(); + + /* Output must follow error */ + output = 0; + for (index = 0; index < 10; ++index) + { + output = 0 - index; + TEST_ASSERT_EQUAL_INT16(output, pidCtrl.calculate(0, index)); + } + + /* Kp = 0, Ki = 1, Kd = 0 */ + pidCtrl.setPFactor(0, 1); + pidCtrl.setIFactor(1, 1); + pidCtrl.setDFactor(0, 1); + pidCtrl.clear(); + + /* Output must increase per error */ + output = 0; + for (index = 0; index < 10; ++index) + { + output += (0 - index); + TEST_ASSERT_EQUAL_INT16(output, pidCtrl.calculate(0, index)); + } + + /* Kp = 0, Ki = 0, Kd = 1 */ + pidCtrl.setPFactor(0, 1); + pidCtrl.setIFactor(0, 1); + pidCtrl.setDFactor(1, 1); + pidCtrl.clear(); + + /* Output must be equal to error deviation from previous error */ + output = 0; + for (index = 1; index < 10; ++index) + { + output = -1; + TEST_ASSERT_EQUAL_INT16(output, pidCtrl.calculate(0, index)); + } +} diff --git a/test/test_RelativeEncoders/test_RelativeEncoders.cpp b/test/test_RelativeEncoders/test_RelativeEncoders.cpp index 7478569c..4c26972e 100644 --- a/test/test_RelativeEncoders/test_RelativeEncoders.cpp +++ b/test/test_RelativeEncoders/test_RelativeEncoders.cpp @@ -1,181 +1,181 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/** - * @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 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/** + * @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/test/test_SimpleTimer/test_SimpleTimer.cpp b/test/test_SimpleTimer/test_SimpleTimer.cpp index cf289b08..1658efd6 100644 --- a/test/test_SimpleTimer/test_SimpleTimer.cpp +++ b/test/test_SimpleTimer/test_SimpleTimer.cpp @@ -1,168 +1,168 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/** - * @author Andreas Merkle - * @brief This module contains the SimpleTimer tests. - */ - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ - -#include -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -static void testSimpleTimer(); - -/****************************************************************************** - * 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(testSimpleTimer); - - 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 SimpleTimer class. - */ -static void testSimpleTimer() -{ - const uint32_t WAIT_TIME = 100; - const uint32_t DELTA_MIN = 1; - SimpleTimer testTimer; - - /* If timer is not running, it shall not signal timeout. */ - TEST_ASSERT_FALSE(testTimer.isTimeout()); - - /* Start timer with 0. It must signal timeout immediately. */ - testTimer.start(0); - TEST_ASSERT_TRUE(testTimer.isTimeout()); - - /* Stop timer. It must not signal timeout. */ - testTimer.stop(); - TEST_ASSERT_FALSE(testTimer.isTimeout()); - - /* Start timer with 100 ms. It must not signal timeout. */ - testTimer.start(WAIT_TIME); - TEST_ASSERT_FALSE(testTimer.isTimeout()); - - /* Test timeout signalling. */ - delay(WAIT_TIME + DELTA_MIN); - TEST_ASSERT_TRUE(testTimer.isTimeout()); - - /* Restart timer. It must not signal timeout. */ - testTimer.restart(); - TEST_ASSERT_FALSE(testTimer.isTimeout()); - - /* Test timeout signalling after a restart. */ - delay(WAIT_TIME + DELTA_MIN); - TEST_ASSERT_TRUE(testTimer.isTimeout()); - - /* Verify timer duration till now. */ - TEST_ASSERT_GREATER_OR_EQUAL(WAIT_TIME + DELTA_MIN, testTimer.getCurrentDuration()); -} +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/** + * @author Andreas Merkle + * @brief This module contains the SimpleTimer tests. + */ + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ + +#include +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +static void testSimpleTimer(); + +/****************************************************************************** + * 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(testSimpleTimer); + + 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 SimpleTimer class. + */ +static void testSimpleTimer() +{ + const uint32_t WAIT_TIME = 100; + const uint32_t DELTA_MIN = 1; + SimpleTimer testTimer; + + /* If timer is not running, it shall not signal timeout. */ + TEST_ASSERT_FALSE(testTimer.isTimeout()); + + /* Start timer with 0. It must signal timeout immediately. */ + testTimer.start(0); + TEST_ASSERT_TRUE(testTimer.isTimeout()); + + /* Stop timer. It must not signal timeout. */ + testTimer.stop(); + TEST_ASSERT_FALSE(testTimer.isTimeout()); + + /* Start timer with 100 ms. It must not signal timeout. */ + testTimer.start(WAIT_TIME); + TEST_ASSERT_FALSE(testTimer.isTimeout()); + + /* Test timeout signalling. */ + delay(WAIT_TIME + DELTA_MIN); + TEST_ASSERT_TRUE(testTimer.isTimeout()); + + /* Restart timer. It must not signal timeout. */ + testTimer.restart(); + TEST_ASSERT_FALSE(testTimer.isTimeout()); + + /* Test timeout signalling after a restart. */ + delay(WAIT_TIME + DELTA_MIN); + TEST_ASSERT_TRUE(testTimer.isTimeout()); + + /* Verify timer duration till now. */ + TEST_ASSERT_GREATER_OR_EQUAL(WAIT_TIME + DELTA_MIN, testTimer.getCurrentDuration()); +} diff --git a/test/test_Util/test_Util.cpp b/test/test_Util/test_Util.cpp index d2cabe54..9d4339ef 100644 --- a/test/test_Util/test_Util.cpp +++ b/test/test_Util/test_Util.cpp @@ -1,190 +1,190 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/** - * @author Andreas Merkle - * @brief This module contains the Util tests. - */ - -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ - -#include -#include - -/****************************************************************************** - * Compiler Switches - *****************************************************************************/ - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -static void testUtil(); - -/****************************************************************************** - * 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(testUtil); - - 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 utilities. - */ -static void testUtil() -{ - uint32_t testVectorUInt[] = {0, UINT32_MAX / 2, UINT32_MAX}; - const char* expectedResultUInt[] = {"0", "2147483647", "4294967295"}; - int32_t testVectorInt[] = {INT32_MIN, INT32_MIN / 2, 0, INT32_MAX / 2, INT32_MAX}; - const char* expectedResultInt[] = {"-2147483647", "-1073741824", "0", "1073741823", "2147483647"}; - uint8_t idx = 0; - const size_t RESULT_STD_BUFFER_SIZE = 12; - const size_t RESULT_LIMIT_BUFFER_SIZE = 4; - - /* Standard use case */ - while (sizeof(testVectorUInt) / sizeof(testVectorUInt[0]) > idx) - { - char result[RESULT_STD_BUFFER_SIZE]; - - Util::uintToStr(result, sizeof(result), testVectorUInt[idx]); - - TEST_ASSERT_EQUAL_STRING(expectedResultUInt[idx], result); - - ++idx; - } - - /* With limited result buffer. */ - while (sizeof(testVectorUInt) / sizeof(testVectorUInt[0]) > idx) - { - char result[RESULT_LIMIT_BUFFER_SIZE]; - - Util::uintToStr(result, sizeof(result), testVectorUInt[idx]); - - TEST_ASSERT_EQUAL_STRING_ARRAY(expectedResultUInt[idx], result, sizeof(result) - 1); - - ++idx; - } - - /* Standard use case */ - while (sizeof(testVectorInt) / sizeof(testVectorInt[0]) > idx) - { - char result[RESULT_STD_BUFFER_SIZE]; - - Util::intToStr(result, sizeof(result), testVectorInt[idx]); - - TEST_ASSERT_EQUAL_STRING(expectedResultInt[idx], result); - - ++idx; - } - - /* With limited result buffer. */ - while (sizeof(testVectorInt) / sizeof(testVectorInt[0]) > idx) - { - char result[RESULT_LIMIT_BUFFER_SIZE]; - - Util::intToStr(result, sizeof(result), testVectorInt[idx]); - - TEST_ASSERT_EQUAL_STRING_ARRAY(expectedResultInt[idx], result, sizeof(result) - 1); - - ++idx; - } -} +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/** + * @author Andreas Merkle + * @brief This module contains the Util tests. + */ + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ + +#include +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +static void testUtil(); + +/****************************************************************************** + * 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(testUtil); + + 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 utilities. + */ +static void testUtil() +{ + uint32_t testVectorUInt[] = {0, UINT32_MAX / 2, UINT32_MAX}; + const char* expectedResultUInt[] = {"0", "2147483647", "4294967295"}; + int32_t testVectorInt[] = {INT32_MIN, INT32_MIN / 2, 0, INT32_MAX / 2, INT32_MAX}; + const char* expectedResultInt[] = {"-2147483647", "-1073741824", "0", "1073741823", "2147483647"}; + uint8_t idx = 0; + const size_t RESULT_STD_BUFFER_SIZE = 12; + const size_t RESULT_LIMIT_BUFFER_SIZE = 4; + + /* Standard use case */ + while (sizeof(testVectorUInt) / sizeof(testVectorUInt[0]) > idx) + { + char result[RESULT_STD_BUFFER_SIZE]; + + Util::uintToStr(result, sizeof(result), testVectorUInt[idx]); + + TEST_ASSERT_EQUAL_STRING(expectedResultUInt[idx], result); + + ++idx; + } + + /* With limited result buffer. */ + while (sizeof(testVectorUInt) / sizeof(testVectorUInt[0]) > idx) + { + char result[RESULT_LIMIT_BUFFER_SIZE]; + + Util::uintToStr(result, sizeof(result), testVectorUInt[idx]); + + TEST_ASSERT_EQUAL_STRING_ARRAY(expectedResultUInt[idx], result, sizeof(result) - 1); + + ++idx; + } + + /* Standard use case */ + while (sizeof(testVectorInt) / sizeof(testVectorInt[0]) > idx) + { + char result[RESULT_STD_BUFFER_SIZE]; + + Util::intToStr(result, sizeof(result), testVectorInt[idx]); + + TEST_ASSERT_EQUAL_STRING(expectedResultInt[idx], result); + + ++idx; + } + + /* With limited result buffer. */ + while (sizeof(testVectorInt) / sizeof(testVectorInt[0]) > idx) + { + char result[RESULT_LIMIT_BUFFER_SIZE]; + + Util::intToStr(result, sizeof(result), testVectorInt[idx]); + + TEST_ASSERT_EQUAL_STRING_ARRAY(expectedResultInt[idx], result, sizeof(result) - 1); + + ++idx; + } +} diff --git a/webots/controllers/Obstacle/Obstacle.py b/webots/controllers/Obstacle/Obstacle.py index 5daf3c9d..9b7ec60a 100644 --- a/webots/controllers/Obstacle/Obstacle.py +++ b/webots/controllers/Obstacle/Obstacle.py @@ -1,53 +1,53 @@ -""" Controller for Obstacle proto.""" - -import sys -from controller import Robot - -# Debounce time of collision in seconds. -DEBOUNCE_TIME = 1 -TOUCH_SENSOR_NAME = 'touch' - - -def main_loop(): - """Main loop: - - Perform simulation steps until Webots is stopping the controller- - - Returns: - number: Status - """ - status = 0 - - # Create robot instance. - obstacle = Robot() - - # Get touch sensor device to detect collision. - # Touch sensor also triggered when clicking the obstacle in the Webots world. - touch_sensor = obstacle.getDevice(TOUCH_SENSOR_NAME) - - if touch_sensor is None: - print(f'Touch sensor "{TOUCH_SENSOR_NAME}" not found!') - status = -1 - else: - # Get basic time step of the current world. - timestep = int(obstacle.getBasicTimeStep()) - - touch_sensor.enable(timestep) # Enable touch sensor - last_collision_time = obstacle.getTime() # Get current time for debouncing - - print('Obstacle ready!') - - while -1 != obstacle.step(timestep): # Main loop - - if 0 < touch_sensor.getValue(): # If collision detected - if (obstacle.getTime() - last_collision_time) > DEBOUNCE_TIME: # Debounce collision - - # Update last collision time - last_collision_time = obstacle.getTime() - - # On (debounced) collision detected: - print('Collision Detected!') - - return status - - -sys.exit(main_loop()) +""" Controller for Obstacle proto.""" + +import sys +from controller import Robot + +# Debounce time of collision in seconds. +DEBOUNCE_TIME = 1 +TOUCH_SENSOR_NAME = 'touch' + + +def main_loop(): + """Main loop: + - Perform simulation steps until Webots is stopping the controller- + + Returns: + number: Status + """ + status = 0 + + # Create robot instance. + obstacle = Robot() + + # Get touch sensor device to detect collision. + # Touch sensor also triggered when clicking the obstacle in the Webots world. + touch_sensor = obstacle.getDevice(TOUCH_SENSOR_NAME) + + if touch_sensor is None: + print(f'Touch sensor "{TOUCH_SENSOR_NAME}" not found!') + status = -1 + else: + # Get basic time step of the current world. + timestep = int(obstacle.getBasicTimeStep()) + + touch_sensor.enable(timestep) # Enable touch sensor + last_collision_time = obstacle.getTime() # Get current time for debouncing + + print('Obstacle ready!') + + while -1 != obstacle.step(timestep): # Main loop + + if 0 < touch_sensor.getValue(): # If collision detected + if (obstacle.getTime() - last_collision_time) > DEBOUNCE_TIME: # Debounce collision + + # Update last collision time + last_collision_time = obstacle.getTime() + + # On (debounced) collision detected: + print('Collision Detected!') + + return status + + +sys.exit(main_loop()) diff --git a/webots/controllers/PlatoonSupervisor/PlatoonSupervisor.py b/webots/controllers/PlatoonSupervisor/PlatoonSupervisor.py index 2a2a0424..c74a2daf 100644 --- a/webots/controllers/PlatoonSupervisor/PlatoonSupervisor.py +++ b/webots/controllers/PlatoonSupervisor/PlatoonSupervisor.py @@ -1,85 +1,85 @@ -""" Platoon Supervisor controller for Webots. Moves the obstacle up and down when the space key is pressed. """ - -import sys -from controller import Supervisor - -DEBOUNCE_TIME = 0.2 # Debounce time for the spacebar in seconds -OBSTACLE_NAME = "OBSTACLE" # Name of the obstacle node -TRANSLATION_FIELD = "translation" # Field name for the translation - -RAISED_HEIGHT = 0.4 # Active height in meters -LOWERED_HEIGHT = 0.06 # Rest height in meters - -# Create the Supervisor instance. -supervisor = Supervisor() - -# Get the keyboard device -keyboard = supervisor.getKeyboard() - - -def main_loop(): - """Main loop: - - Perform simulation steps until Webots is stopping the controller- - - Returns: - number: Status - """ - - # Get the obstacle node - obstacle_node = supervisor.getFromDef(OBSTACLE_NAME) - - if obstacle_node is None: - - print(f"Robot DEF {OBSTACLE_NAME} not found.") - status = -1 - - else: - # Get the translation field of the obstacle node - translation_field = obstacle_node.getField(TRANSLATION_FIELD) - - if translation_field is None: - print( - f"Translation field {TRANSLATION_FIELD} of {OBSTACLE_NAME} not found.") - status = -1 - - else: - # Get the time step of the current world. - timestep = int(supervisor.getBasicTimeStep()) - - # Enable the keyboard device - keyboard.enable(timestep) - - # Get current time for debouncing - last_time = supervisor.getTime() - - # Set the initial state to raised obstacle - is_obstacle_lowered = False - curr_pos = translation_field.getSFVec3f() - translation_field.setSFVec3f( - [curr_pos[0], curr_pos[1], RAISED_HEIGHT]) - - while supervisor.step(timestep) != -1: - if keyboard.getKey() == ord(' '): # If space key is pressed - if (supervisor.getTime() - last_time) > DEBOUNCE_TIME: # Debounce space key - - # Update last time - last_time = supervisor.getTime() - - # Get the current position of the obstacle - curr_pos = translation_field.getSFVec3f() - - # Move the obstacle - if is_obstacle_lowered is True: # Raise the obstacle - translation_field.setSFVec3f( - [curr_pos[0], curr_pos[1], RAISED_HEIGHT]) - else: # Lower the obstacle - translation_field.setSFVec3f( - [curr_pos[0], curr_pos[1], LOWERED_HEIGHT]) - - # Toggle the obstacle state - is_obstacle_lowered = not is_obstacle_lowered - - return status - - -sys.exit(main_loop()) +""" Platoon Supervisor controller for Webots. Moves the obstacle up and down when the space key is pressed. """ + +import sys +from controller import Supervisor + +DEBOUNCE_TIME = 0.2 # Debounce time for the spacebar in seconds +OBSTACLE_NAME = "OBSTACLE" # Name of the obstacle node +TRANSLATION_FIELD = "translation" # Field name for the translation + +RAISED_HEIGHT = 0.4 # Active height in meters +LOWERED_HEIGHT = 0.06 # Rest height in meters + +# Create the Supervisor instance. +supervisor = Supervisor() + +# Get the keyboard device +keyboard = supervisor.getKeyboard() + + +def main_loop(): + """Main loop: + - Perform simulation steps until Webots is stopping the controller- + + Returns: + number: Status + """ + + # Get the obstacle node + obstacle_node = supervisor.getFromDef(OBSTACLE_NAME) + + if obstacle_node is None: + + print(f"Robot DEF {OBSTACLE_NAME} not found.") + status = -1 + + else: + # Get the translation field of the obstacle node + translation_field = obstacle_node.getField(TRANSLATION_FIELD) + + if translation_field is None: + print( + f"Translation field {TRANSLATION_FIELD} of {OBSTACLE_NAME} not found.") + status = -1 + + else: + # Get the time step of the current world. + timestep = int(supervisor.getBasicTimeStep()) + + # Enable the keyboard device + keyboard.enable(timestep) + + # Get current time for debouncing + last_time = supervisor.getTime() + + # Set the initial state to raised obstacle + is_obstacle_lowered = False + curr_pos = translation_field.getSFVec3f() + translation_field.setSFVec3f( + [curr_pos[0], curr_pos[1], RAISED_HEIGHT]) + + while supervisor.step(timestep) != -1: + if keyboard.getKey() == ord(' '): # If space key is pressed + if (supervisor.getTime() - last_time) > DEBOUNCE_TIME: # Debounce space key + + # Update last time + last_time = supervisor.getTime() + + # Get the current position of the obstacle + curr_pos = translation_field.getSFVec3f() + + # Move the obstacle + if is_obstacle_lowered is True: # Raise the obstacle + translation_field.setSFVec3f( + [curr_pos[0], curr_pos[1], RAISED_HEIGHT]) + else: # Lower the obstacle + translation_field.setSFVec3f( + [curr_pos[0], curr_pos[1], LOWERED_HEIGHT]) + + # Toggle the obstacle state + is_obstacle_lowered = not is_obstacle_lowered + + return status + + +sys.exit(main_loop()) diff --git a/webots/worlds/HeadingCalculation.wbt b/webots/worlds/HeadingCalculation.wbt index 0795f42d..2281098e 100644 --- a/webots/worlds/HeadingCalculation.wbt +++ b/webots/worlds/HeadingCalculation.wbt @@ -1,40 +1,40 @@ -#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 "rubber" - material2 "cardboard" - coulombFriction [ - 0.65 - ] - } - ] -} -Viewpoint { - orientation -0.5773502691896258 0.5773502691896258 0.5773502691896258 2.0944 - position 0.10671498723053877 -0.025801636297686177 4.366758398391875 -} -TexturedBackground { -} -TexturedBackgroundLight { -} -HeadingCalculationTrack { - contactMaterial "cardboard" -} -Zumo32U4 { - translation 0 0 0.0139993 - rotation 6.60216e-09 2.69589e-07 -1 -1.5286253071795866 - name "zumo_0" - contactMaterial "rubber" -} +#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 "rubber" + material2 "cardboard" + coulombFriction [ + 0.65 + ] + } + ] +} +Viewpoint { + orientation -0.5773502691896258 0.5773502691896258 0.5773502691896258 2.0944 + position 0.10671498723053877 -0.025801636297686177 4.366758398391875 +} +TexturedBackground { +} +TexturedBackgroundLight { +} +HeadingCalculationTrack { + contactMaterial "cardboard" +} +Zumo32U4 { + translation 0 0 0.0139993 + rotation 6.60216e-09 2.69589e-07 -1 -1.5286253071795866 + name "zumo_0" + contactMaterial "rubber" +} diff --git a/webots/worlds/zumo_with_com_system/SensorFusionTrack.wbt b/webots/worlds/zumo_with_com_system/SensorFusionTrack.wbt index 4fe00402..638f43e7 100644 --- a/webots/worlds/zumo_with_com_system/SensorFusionTrack.wbt +++ b/webots/worlds/zumo_with_com_system/SensorFusionTrack.wbt @@ -1,52 +1,52 @@ -#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/ZumoComSystem.proto" -EXTERNPROTO "../../protos/SensorFusionTrack.proto" -IMPORTABLE EXTERNPROTO "../../protos/Supervisor.proto" - -WorldInfo { - info [ - "Example uses of Track nodes, as caterpillar tracks on a robot, or as a conveyor belt." - ] - title "Track" - basicTimeStep 8 - contactProperties [ - ContactProperties { - material1 "rubber" - material2 "cardboard" - coulombFriction [ - 0.65 - ] - } - ] -} -Viewpoint { - orientation 0.3787583266277594 0.09714544028170935 -0.9203830145339561 2.677946076660944 - position 1.2569587324159737 0.7945022889765716 1.5111964909293643 -} -TexturedBackground { -} -TexturedBackgroundLight { -} -SensorFusionTrack { - contactMaterial "cardboard" -} -DEF ROBOT Zumo32U4 { - translation -0.24713614078815466 -0.04863962992854465 0.013994298332013683 - rotation -1.0564747468923541e-06 8.746699709178704e-07 0.9999999999990595 1.5880805820884731 - name "Zumo" - contactMaterial "rubber" -} -ZumoComSystem { - translation 0 0 0 - rotation 0 0 1 0 - name "ZumoComSystem" -} -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/ZumoComSystem.proto" +EXTERNPROTO "../../protos/SensorFusionTrack.proto" +IMPORTABLE EXTERNPROTO "../../protos/Supervisor.proto" + +WorldInfo { + info [ + "Example uses of Track nodes, as caterpillar tracks on a robot, or as a conveyor belt." + ] + title "Track" + basicTimeStep 8 + contactProperties [ + ContactProperties { + material1 "rubber" + material2 "cardboard" + coulombFriction [ + 0.65 + ] + } + ] +} +Viewpoint { + orientation 0.3787583266277594 0.09714544028170935 -0.9203830145339561 2.677946076660944 + position 1.2569587324159737 0.7945022889765716 1.5111964909293643 +} +TexturedBackground { +} +TexturedBackgroundLight { +} +SensorFusionTrack { + contactMaterial "cardboard" +} +DEF ROBOT Zumo32U4 { + translation -0.24713614078815466 -0.04863962992854465 0.013994298332013683 + rotation -1.0564747468923541e-06 8.746699709178704e-07 0.9999999999990595 1.5880805820884731 + name "Zumo" + contactMaterial "rubber" +} +ZumoComSystem { + translation 0 0 0 + rotation 0 0 1 0 + name "ZumoComSystem" +} +Supervisor { + name "Supervisor" + controller "Supervisor" + supervisor TRUE +}