Diff: main.cpp

Revision:

116:6dfcafa00e42

Parent:

115:156b8234f2de

Child:

117:58148bdd13b7

diff -r 156b8234f2de -r 6dfcafa00e42 main.cpp
--- a/main.cpp	Wed May 18 18:07:09 2022 +0000
+++ b/main.cpp	Mon May 23 12:55:02 2022 +0000
@@ -10,6 +10,15 @@
 #include "toolbox.hpp"
 #include "main.hpp"
 
+//Constants
+const char speedMessageLength = 6;
+const float minimalSpeed = 0.05;
+const int terminalOutputFrequency = 50;
+const int speedTransmitionFrequency = 250;
+
+//Flag
+bool commandEnabled = true;
+
 //Declaration PWM outputs
 PwmOut propulsion(PWM_PROP);
 PwmOut direction(PWM_DIR);
@@ -23,6 +32,7 @@
 //Timers
 Timer tSpeed;
 Timer tTerminalWriting;
+Timer tSpeedTx;
 Ticker tickAsserv;
 
 //Var.
@@ -41,6 +51,7 @@
     serial_port.write("Test\n\r",strlen("Test\n\r"));
     
     char terminalOutput[256];
+    char msgSpeedProtocolOutput[speedMessageLength] = {0};
     
     //Interrupt
     serial_port.attach(&onRxInterrupt, SerialBase::RxIrq);
@@ -60,6 +71,7 @@
     //Timers init.
     tSpeed.start();
     tTerminalWriting.start();
+    tSpeedTx.start();
     
     //Infinite loop
     while(1) 
@@ -71,25 +83,31 @@
             switch(receivedCommand)
             {
                 case COMMAND_PWM:
+                    //1st. get the values
                     pulsewidth_propulsion = protocolReceivedPayload[0] << 8;
                     pulsewidth_propulsion += protocolReceivedPayload[1];
                     pulsewidth_direction = protocolReceivedPayload[2] << 8;
                     pulsewidth_direction += protocolReceivedPayload[3];
+                    //2nd. allocate the values
                     propulsion.pulsewidth_us(pulsewidth_propulsion);
                     direction.pulsewidth_us(pulsewidth_direction);
                     break;
                     
                 case COMMAND_ASSERVISSEMENT: 
+                    //1st. 
                     commandSpeed = convertBytesToFloat(protocolReceivedPayload, 0);
                     pulsewidth_direction = protocolReceivedPayload[4] << 8;
                     pulsewidth_direction += protocolReceivedPayload[5];
+                    //2nd.
                     direction.pulsewidth_us(pulsewidth_direction);
                     break;
                     
                 case COMMAND_PARAMETRES:
                 {                       
+                    //1st.
                     float kp = convertBytesToFloat(protocolReceivedPayload, 0);
                     float ki = convertBytesToFloat(protocolReceivedPayload, 4);                                                
+                    //2nd.
                     setPIDParameters(kp, ki);
                     break;   
                 }
@@ -97,34 +115,36 @@
         }        
         
         //If no speed captor interrupt, consider speed = 0
-        if (tSpeed.read() > 0.25)
+        if (tSpeed.read() > DISTANCE_FOR_HALF_TURN / minimalSpeed)
         {
             currentSpeed = 0;
             tSpeed.reset();
         }
         
-        //Write in terminal
-        if (tTerminalWriting.read() > 0.02)
+        //Write in terminal (will need to get out)
+        if (tTerminalWriting.read() > 1.0 / terminalOutputFrequency)
         {
             sprintf(terminalOutput, "Vcons = %d m/s, Vmes = %d mm/s, PWM = %d\n\r", (int)commandSpeed, (int)(currentSpeed*1000), pulsewidth_propulsion);
             serial_port.write(terminalOutput, strlen(terminalOutput));
             tTerminalWriting.reset();
         }
+        
+        //Transmit speed to rPi/Jetson
+        if(tSpeedTx.read() > 1.0 / speedTransmitionFrequency)
+        {
+            encodeMessage(msgSpeedProtocolOutput, currentSpeed);
+            serial_port.write(msgSpeedProtocolOutput, speedMessageLength);
+        }
     }
 }
 
 void onRxInterrupt()
 {
     char c;
-
-
+    
     // Read the data to clear the receive interrupt.
     if (serial_port.read(&c, 1)) 
-    {
         decodeMessage(c);
-        //serial_port.write(&c, 1);
-    }
-
 }
 
 void onSpeedCaptorInterrupt()