From 35f1a7751f6e73c24d1bcb1f8ec17c1251745dac Mon Sep 17 00:00:00 2001
From: jkerpe <juliane-98@web.de>
Date: Tue, 21 Nov 2023 11:28:18 +0100
Subject: [PATCH 1/2] Send End Line Status via SerialMuxProt

---
 lib/APPSensorFusion/App.cpp             |  25 ++++++++++++++++++++++++
 lib/APPSensorFusion/App.h               |  17 +++++++++++++++-
 lib/APPSensorFusion/DrivingState.cpp    |  20 ++++---------------
 lib/APPSensorFusion/DrivingState.h      |  19 +++++++++++++-----
 lib/APPSensorFusion/SerialMuxChannels.h |  13 ++++++++++++
 webots/protos/track.png                 | Bin 22044 -> 22312 bytes
 6 files changed, 72 insertions(+), 22 deletions(-)

diff --git a/lib/APPSensorFusion/App.cpp b/lib/APPSensorFusion/App.cpp
index 441a7b3d..5bf9ce12 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,22 @@ void App::sendSensorData() const
     (void)m_smpServer.sendData(m_smpChannelIdSensorData, reinterpret_cast<uint8_t*>(&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;
+        payload.isEndLineDetected = true;
+        (void)m_smpServer.sendData(m_smpChannelIdEndLine, reinterpret_cast<uint8_t*>(&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 <Arduino.h>
 #include <SerialMuxProtServer.hpp>
 #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<int16_t> 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 dc4357c6227229eedd906fffdd9e2171a15b973e..4222d330c84c02cf05d4da0a283686b1597b5098 100644
GIT binary patch
literal 22312
zcmeHPeNa@_6+a8Bt{=c^f|vlZYD_buK`b?5im;N+tTt*onY6{OA8Q%$4}{&gV+xA!
z5;C)NR7i$YmT^bO$Hd7D@}Y^aBkD6K+K~cIT4I61lFIHhlCUWS*$8aUy_Xj>e|0n&
zY;*1my!+0(_q_AYJ@=g7IgiI5e!1BgGVAeK01&caz2PMQ4R$r4nU053Cq9kCgUa@j
zF$JnxqW*~|>fP%$tplhno;|vA8lKOzuHRw<2))F9RDWC+8VT_0a~llnUe4a$-%&dk
zv>@yIFT~G!;p*7xEsJeCf^iaSBOUwMeAbfE#SuUP<ua%jC`6sC^gzYL;!VRO<4rHA
zly1-*rKdG3Jz|NAM!F;;!i`M3huj8`zXYH_0Tdw#Z6HP>XaEEa2pZr{AZS3)0L_4)
z0YL*QM6l`!K?8yYXa-a@AZP#t4G0>rm_Stnsv4jf5HuiYfM!6@fS>_E0~QylYCzC{
zss?N)5HuiYfM!6@fS>_E1LhD38W1#iSfGLR#@Uja^{)WD9vld?I?rrc)6n+Skv*$s
zmqqlx-!=~xq|6P{>6z8?rV3Je8m-dPOG*!d)Jj*YQ+g77``4Ii_NS!05lak+q)Rd)
zETd0s35*N0d&q47`AbY0DNs;^qzH+TVG1H-+|fh(=0JmMWz(}A>89kP<;grk4K#-+
zk?P0mrG;*U9-MFEmrbHQESpt`SDt>jwn(Z@e3t`n>k2%SfAU&#*Oetg;Lp&zW<O>h
zuL&!*aQXMl1+YG4AGa<q-pdKO2=1+#83EsK7qU;h@GUPu%`!Z9C3bM8%`X7Ru~DOq
z2fi+x!3M;YuefD|Y)4~$cCu$FqsX~k_^WX^$^6mMi@*2GXEZ2AM!q-KhCY?K&Kfz$
z4N5YhIQ5<J_P5SNXBYW7H@Sz=$_k9xF`gxS;)O?0q7q|RWM;f|xeZS+feJ$aUA6zF
zbbUCede?D|rgZ@B@~_su{Lx9-`_tWARL{_^F%9Hf-_&?*1BtmeJ)9qOmmr5tp@H58
zgCN-Ep=dMg0c+l3PLTYEFMk*U$u6(9ZGMo~yq$lD)_f6#PQ0{ek<;#H-^fAS47^Kn
z*EdIN8Vw2MChy67@e!MKI|_9Em65Q66YQSBaiM{oiziO2Y7_pfavTSDV%1pHXi?Fx
z{QhHu{KlKI036N&y`%Nypuc^HKZJB!heXt_qf~^(3L`2vdCjOjc91V>#q%i2ho<f?
z_9bWz)-~JeI4f`#h$H@H=<<<&4JZ5JC$9ZWaKcMEJx+ei5(qYRq0rc@dE$s&=74;&
zErG^{=6rTZV#?omY3T3r=}NIQ1I!k}y2qzgat(o(FFzlPsh|AB*3OXr%lkP)-*FTo
z_a7Y=cfk-F0B$D|M7!6i$m-DxS!PRnIJ+p_#yiP@JJZL_+xXyw7_Jr8zb~lJw70fM
zRrq*X<$IZDg3O0L=7PJoqQOoy%FU)C^5_)x|G*gdaNqBugMAVPxM$nruMK@%{q@z>
za}k~44c%{LaQ_2$Y{HgNp!+hgNvq~AS^t*>Y0;Ud!@*P;&7aZx`Q(>(xWPozOG=MY
zK@2%MMH}CrHQcip@~v&hN)P0u=f${Qb@txj3!f0VTDj|ku11<G^$Hk{tZcvjj7e2<
zS;S3iEIN15zQ3o^5z8O8#z_9+M{2X=FD4d#3ewR3VXsF1g8aqlaKabN3LX?j-IIt4
zv--bd$^C_PRa2_5v!y2d(ew8B!NKy9o}aVFm0k7U=DPR$f@nN4t#XMpP&u3H&x^^l
zLq4_qmc|@&SAH#Y`yEs|(u7%eQfe5#*s=l31TzEGkv9$vsTsw+cBYsmN(gdT)!6jA
ziU5wnjI*!p-h!11Y~7`p$M49#6XDr4&T}j0F=Z~#c1m1*-(*=-jk#H6x2pmijAvW!
zS^SMxp9Z<LVT=pYrc-7!iRJgPaFN^hrKnJZqQU}C1W9+7MrutxU+i12SzFhVfprcx
z*J%eZ?_CUz;zlv;gW8d6vGTv8rhl@vIfspqp2TFpJs%u}6C+}gaZe#swD$6ns2-m{
ziZMW2pD%`uuXquc>p25h7kZYlb2+N%^HnP#&o;30os;VB5*PnxL~#+w)n`8traHH6
z&cGGHO3p=v?wO~ObJmRVXLqpj)CDB?6DA0srH=kJLRz+P&5gQqaz@z+b_e#qnd<-a
zp5=CDx;xeLBqzlnr>_3%1x?j^R@bZ;#L^ye>cNGoKUQEIN%U}0As>yvy{I)RC9h`M
z#mr`r^<*tV;?4t6zZh>ms_rQ>aej#@_#~eWCsnG4dslMZn`3vh7lSX(e%P1C3W3Pq
zk47qObA>=ssgawll!JFnctZ=@uPqTNFRo?iQni$p^EOw%y8x$^&JC8663=r3ufYnI
zd_o&6A51*O3DzSxqE4rgcH|hu6jo9bO*3*TkDDN#Zr-R+r}mY)D85=Uu=slBZ!JZA
zUa@NiTB&ZX6}l1Y^bpp~b&3Uj$NdSiPOn^^S_9K5Vvv*%T$UqUk`a*+p+h0(2ISSJ
z<nIi~1(OT@uUxPk__9NVAbL(?h5t7Pqb+)#A>F-gC*RRczBSTI=-ot{e4Be81AX1E
Y8h&zC=P2%fgK^vN{ANQ{iuv{b0N`a2@Bjb+

delta 1726
zcmY*aYfuwc6uz541O?=w2IY|eDxw(-iUzedp+1m`p-!vd02&4<c8HLSRq>H#@s&<>
zK!+(PBB;|Ae;|ZLBPb&)2n;fSEp5T@T408;MaNhK5z?gR-lac!e(di3?m6dv=W);8
zsiaCOsLQb)4G4KOPzzUlN+YzVx;o){SkT!Yo3$(G+i7-3+1kjc1^rGM|Iq5Hju-5n
zZuP<1(UdR~wTf~zv68Diz4XTZ_D*Dt?22rUXG9<Wg0R#SB1D%<t%k9wgM19IG9E&K
z&Cf;Vp>cN%&^w(F>ihndyss#J0|rnB2F^c`e^eO1kpQq(KVwCuZRG)Y&D3C^ffY?Q
zg$5EJXa8uu9vO>=<gfCNx?)HgC_~6-`Gdbb$-B;E#t=%}0=6rY+*TZpHeQ+#V2JSs
zHkHTPq`#W^5321LLQ{D_s>v-dRbCN80Q)k8R9EgdZtr#LE4okIGuJ@pz}>}C%YL@H
z;&ZH~N5IPV(sJweVdv7eVrPu`cDLDt=&^Lixslpq0hP5}e6-FRA<dN`xqp!h)<UW{
zVDVULGmQ991h|y3@tOr$Ti0z-B)RyXJxUTK72uG*3kvc#Ow7e<QUPd-3QN7$$DJG5
zr+=9B&jMo2Xn|u++SGhqFj>><LGOY?>gh$6iX!sD4<1lHdt_SFJhF&j^l}O@jGH4h
zdDb!zEahu#hVU^ztZ;K1oO)+j`APRVx3XI|9-l|7m<Sw(o_vcHpdl`I<aYK!Od!w2
z<OL5Jfpb-feNMP`*6uCICEmwo%z+ZSLA%d^*QjMVWbfOtN4ZHS@(J(?Skv>B4&DH1
zBh&&#;e_}VCkOPptD9yZAjoGKyy~>CWtveP(b2>ecu9m}75b*iuc!Du$sS_|*49&l
zjBtRX(-LADlK2QhnXgZ-lk<%Yk-Nsbbp3xnw7;%@Tk4qHt4^@yygz-MI5cjS6{Lpm
zSkm?9?>0BZZ&)?m%wxUZc;Ib1P0KHcSluDKs;nB#K(~MTs<<rNw2)XL|1v*Q^~zFe
zNnClt_*%3ZUzeg6bLpLRn}|YQle;NGvc`F-O}4Ac=49<SMS6nGftdJFH<aT(%=)?n
zqB<E<kh)5!w>H1g@iPYI5Ugb>B<|ecn7}-&BNZ^lbA^Ci7f*=z$^Ak4Jy9inA{e%0
zv3mzX2r>gnWmhX54K)>{oOGHG!Qr8xA+OS%m2ZyC^=&pC?0;kz;Vspm6IXcn_7ua>
zmy;0P2}Q=6hu}O;i1lY#6tZikiQP?n7xTBG=jl39&wEe|f%)ARRY5`TBq`lW2SNP}
zaDpZn`Q{JFu_HyntUs=GYn+i__?@2H!u=r&MlaXexU+$|HQEWKTyPK@oQ%C*6r)Y7
zBI~rP%DfYlH9ymE-t65Inz<OGDoGY3;9{nMRtg*KzIH<5QVdYt7L2088}(R;j)1nE
zDs(}4Emje%^+=W6)#{jVW`&u88M81)GRECQsN2Pg&HdTeb7y|WR}i5nAZ6^aM30Ef
zem#W~<n}Z5P}t;l)~mE-nw-aO!c=S1T~2&C?@W4y3>%_CNTt!TXiICSI@a1@AthD0
zgHu_r{$!%7x-o;Bgo!W-VGss|{C^OJJ)oE)AMO$VzBp5lJb6sohaU)_fcR2eDfaQ{
zXN08&?jD=^6dB#ivdQ6i8n9tcktU!_O#s_=FbQ|zvDq-@$xOy`epYyx0w+waEA;wq
z{DY|CZLnu|dmMr!`wH6Y{~F8TbHg3Tz+m{#7IMJ}?p032iUPQo!hv3o7ah~7!;_l3
O7?0hi+*<jG>de0mxc<oi


From 0fc58d90f9089539b41e9107fc9d7548d0495a35 Mon Sep 17 00:00:00 2001
From: jkerpe <juliane-98@web.de>
Date: Tue, 21 Nov 2023 14:03:23 +0100
Subject: [PATCH 2/2] Implemented finding of gabryelreyes

---
 lib/APPSensorFusion/App.cpp | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/lib/APPSensorFusion/App.cpp b/lib/APPSensorFusion/App.cpp
index 5bf9ce12..ce7b0888 100644
--- a/lib/APPSensorFusion/App.cpp
+++ b/lib/APPSensorFusion/App.cpp
@@ -176,8 +176,7 @@ void App::sendEndLineDetectionSignal()
     if ((DrivingState::LINE_STATUS_FIND_END_LINE == m_lastLineDetectionStatus) &&
         (DrivingState::LINE_STATUS_END_LINE_DETECTED == lineStatus))
     {
-        EndLineFlag payload;
-        payload.isEndLineDetected = true;
+        EndLineFlag payload = {.isEndLineDetected = true};
         (void)m_smpServer.sendData(m_smpChannelIdEndLine, reinterpret_cast<uint8_t*>(&payload), sizeof(payload));
     }