diff --git a/lib/APPSensorFusion/App.cpp b/lib/APPSensorFusion/App.cpp index 441a7b3d..ce7b0888 100644 --- a/lib/APPSensorFusion/App.cpp +++ b/lib/APPSensorFusion/App.cpp @@ -76,6 +76,9 @@ void App::setup() /* Providing Sensor data */ m_smpChannelIdSensorData = m_smpServer.createChannel(SENSORDATA_CHANNEL_NAME, SENSORDATA_CHANNEL_DLC); + + /* Sending End Line Detection signal */ + m_smpChannelIdEndLine = m_smpServer.createChannel(ENDLINE_CHANNEL_NAME, ENDLINE_CHANNEL_DLC); } void App::loop() @@ -107,6 +110,12 @@ void App::loop() { m_sendSensorsDataInterval.restart(); sendSensorData(); + + /* Send End line detection signal if the application is currently in the Driving state. */ + if (&DrivingState::getInstance() == m_systemStateMachine.getState()) + { + sendEndLineDetectionSignal(); + } } m_systemStateMachine.process(); } @@ -159,6 +168,21 @@ void App::sendSensorData() const (void)m_smpServer.sendData(m_smpChannelIdSensorData, reinterpret_cast(&payload), sizeof(payload)); } +void App::sendEndLineDetectionSignal() +{ + DrivingState::LineStatus lineStatus = DrivingState::getInstance().getLineStatus(); + + /* Send only if a new End Line has been detected. */ + if ((DrivingState::LINE_STATUS_FIND_END_LINE == m_lastLineDetectionStatus) && + (DrivingState::LINE_STATUS_END_LINE_DETECTED == lineStatus)) + { + EndLineFlag payload = {.isEndLineDetected = true}; + (void)m_smpServer.sendData(m_smpChannelIdEndLine, reinterpret_cast(&payload), sizeof(payload)); + } + + m_lastLineDetectionStatus = lineStatus; +} + /****************************************************************************** * External Functions *****************************************************************************/ diff --git a/lib/APPSensorFusion/App.h b/lib/APPSensorFusion/App.h index f4981b35..10fdaab7 100644 --- a/lib/APPSensorFusion/App.h +++ b/lib/APPSensorFusion/App.h @@ -48,6 +48,7 @@ #include #include #include "SerialMuxChannels.h" +#include "DrivingState.h" /****************************************************************************** * Macros @@ -66,9 +67,11 @@ class App */ App() : m_smpChannelIdSensorData(0U), + m_smpChannelIdEndLine(0U), m_systemStateMachine(), m_controlInterval(), m_sendSensorsDataInterval(), + m_lastLineDetectionStatus(DrivingState::LINE_STATUS_FIND_START_LINE), m_smpServer(Serial) { } @@ -100,9 +103,12 @@ class App /** Baudrate for Serial Communication */ static const uint32_t SERIAL_BAUDRATE = 115200U; - /** Channel id sending sensor data used for sensor fusion. */ + /** Channel id for sending sensor data used for sensor fusion. */ uint8_t m_smpChannelIdSensorData; + /** Channel id for sending End Line Detection signal. */ + uint8_t m_smpChannelIdEndLine; + /** The system state machine. */ StateMachine m_systemStateMachine; @@ -112,6 +118,9 @@ class App /** Timer used for sending data periodically. */ SimpleTimer m_sendSensorsDataInterval; + /** End Line Status of the previous iteration */ + DrivingState::LineStatus m_lastLineDetectionStatus; + /** * SerialMuxProt Server Instance * @@ -124,6 +133,12 @@ class App */ void sendSensorData() const; + /** + * Send the End Line Detection Flag as a EndLineFlag struct via SerialMuxProt. + * The Signal will only be sent if the a new End Line has been detected. + */ + void sendEndLineDetectionSignal(); + /* Not allowed. */ App(const App& app); /**< Copy construction of an instance. */ App& operator=(const App& app); /**< Assignment of an instance. */ diff --git a/lib/APPSensorFusion/DrivingState.cpp b/lib/APPSensorFusion/DrivingState.cpp index a3db9d40..cf2b4388 100644 --- a/lib/APPSensorFusion/DrivingState.cpp +++ b/lib/APPSensorFusion/DrivingState.cpp @@ -182,25 +182,12 @@ void DrivingState::processOnTrack(int16_t position, const uint16_t* lineSensorVa m_lineStatus = LINE_STATUS_START_LINE_DETECTED; Sound::playBeep(); - - /* Measure the lap time and use as start point the detected start line. */ - m_lapTime.start(0); } /* End line detected */ else if (LINE_STATUS_FIND_END_LINE == m_lineStatus) { - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); - - /* Stop motors immediately. Don't move this to a later position, - * as this would extend the driven length. - */ - diffDrive.setLinearSpeed(0, 0); - - Sound::playBeep(); - m_trackStatus = TRACK_STATUS_FINISHED; - - /* Calculate lap time and show it*/ - ReadyState::getInstance().setLapTime(m_lapTime.getCurrentDuration()); + m_lineStatus = LINE_STATUS_END_LINE_DETECTED; + /* Don't switch to the Ready State and do nothing. */ } else { @@ -213,7 +200,8 @@ void DrivingState::processOnTrack(int16_t position, const uint16_t* lineSensorVa } 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) diff --git a/lib/APPSensorFusion/DrivingState.h b/lib/APPSensorFusion/DrivingState.h index 1b0d831a..c15f898a 100644 --- a/lib/APPSensorFusion/DrivingState.h +++ b/lib/APPSensorFusion/DrivingState.h @@ -92,8 +92,6 @@ class DrivingState : public IState */ void exit() final; -protected: -private: /** * The line detection status. */ @@ -101,9 +99,22 @@ class DrivingState : public IState { 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_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. */ @@ -124,7 +135,6 @@ class DrivingState : public IState 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! */ @@ -138,7 +148,6 @@ class DrivingState : public IState */ DrivingState() : m_observationTimer(), - m_lapTime(), m_pidProcessTime(), m_pidCtrl(), m_topSpeed(0), diff --git a/lib/APPSensorFusion/SerialMuxChannels.h b/lib/APPSensorFusion/SerialMuxChannels.h index bfe7d957..43fca858 100644 --- a/lib/APPSensorFusion/SerialMuxChannels.h +++ b/lib/APPSensorFusion/SerialMuxChannels.h @@ -49,6 +49,12 @@ /** DLC of Sensordata Channel */ #define SENSORDATA_CHANNEL_DLC (sizeof(SensorData)) +/** Name of Channel to End Line Detected Signal. */ +#define ENDLINE_CHANNEL_NAME "END_LINE" + +/** DLC of End Line Channel */ +#define ENDLINE_CHANNEL_DLC (sizeof(EndLineFlag)) + /** Maximum number of SerialMuxProt Channels. */ #define MAX_CHANNELS (10U) @@ -99,6 +105,13 @@ typedef struct _SensorData int16_t turnRate; } __attribute__((packed)) SensorData; +/** Struct of the End Line Detection payload. */ +typedef struct _EndLineFlag +{ + /** Indicates if the End Line has been detected. */ + bool isEndLineDetected; +} __attribute__((packed)) EndLineFlag; + /****************************************************************************** * Functions *****************************************************************************/ diff --git a/webots/protos/track.png b/webots/protos/track.png index dc4357c6..4222d330 100644 Binary files a/webots/protos/track.png and b/webots/protos/track.png differ