Diff: main.cpp

Revision:

113:2f8f255e99f2

Parent:

112:478ae92cb106

Child:

114:c1f7be27aa5d

diff -r 478ae92cb106 -r 2f8f255e99f2 main.cpp
--- a/main.cpp	Tue Apr 26 11:11:49 2022 +0000
+++ b/main.cpp	Tue May 17 22:44:42 2022 +0000
@@ -6,39 +6,58 @@
 #include "mbed.h"
 #include "platform/mbed_thread.h"
 #include "protocol.h"
-
-//Define SPI
-#define SPI3_MOSI   D11
-#define SPI3_MISO   D12
-#define SPI3_SCLK   D13
-#define SPI3_CS     A3
+#include "asservissement.hpp"
 
 //Define PWM
 #define PWM_PROP    D9
 #define PWM_DIR     D10
 
-//fontion interrupt SP
+//Define interrupt Input
+#define SPEED_CAPTOR A4
+
+//Define COEF
+#define DISTANCE_FOR_HALF_TURN 3 * 3.14         //TODO: mettre vrai valeur
+#define CONTROL_RATE 1000.0
+
+//fontion interrupt
 void onRxInterrupt();
+void onSpeedCaptorInterrupt();
+void onTickerAsservissementInterrupt();
 
 //Declaration PWM outputs
 PwmOut propulsion(PWM_PROP);
 PwmOut direction(PWM_DIR);
 
+//Declaration analog input
+InterruptIn speedCaptor(SPEED_CAPTOR);
+
 //Declaration Links
 static UnbufferedSerial serial_port(USBTX, USBRX,115200);
-SPISlave device(SPI3_MOSI, SPI3_MISO, SPI3_SCLK, SPI3_CS); // mosi, miso, sclk, ssel
+
+//Timers
+Timer tSpeed;
+Timer tTerminalWriting;
+Ticker tickAsserv;
+
+//Var.
+double currentSpeed = 0;
+double commandSpeed = 5;
+
+//Declaration PWM variables
+uint32_t pulsewidth_direction = 1100;
+uint32_t pulsewidth_propulsion = 1500;
 
 int main()
-{
-    //Declaration PWM variables
-    uint32_t pulsewidth_direction = 1100;
-    uint32_t pulsewidth_propulsion = 1500;
-    
+{    
     //Test Serial port
     serial_port.write("Test\n\r",strlen("Test\n\r"));
     
-    //Interrupt SP
+    char terminalOutput[256];
+    
+    //Interrupt
     serial_port.attach(&onRxInterrupt, SerialBase::RxIrq);
+    speedCaptor.fall(&onSpeedCaptorInterrupt);
+    tickAsserv.attach(&onTickerAsservissementInterrupt, 1.0 / CONTROL_RATE);
     
     //Init. propulsion PWM
     propulsion.period_us(20000);
@@ -50,21 +69,13 @@
     direction.period_us(20000);
     direction.pulsewidth_us(pulsewidth_direction);
     
-    //Init. SPI Link
-    device.format(8);
+    //Timers init.
+    tSpeed.start();
+    tTerminalWriting.start();
     
     //Infinite loop
     while(1) 
     {
-        //If SPI received a char, decode it then reply bullshit.
-        if(device.receive())
-        {
-            char c = device.read();
-            decodeMessage(c);
-            serial_port.write(&c, 1);
-            device.reply(0b10101010);
-        }
-        
         //If decoding has ended, get and changes PWM values.
         if(isDataAvailable())
         {
@@ -73,6 +84,20 @@
             direction.pulsewidth_us(pulsewidth_direction);
         }        
         
+        //If no speed captor interrupt, consider speed = 0
+        if (tSpeed.read() > 0.25)
+        {
+            currentSpeed = 0;
+            tSpeed.reset();
+        }
+        
+        //Write in terminal
+        if (tTerminalWriting.read() > 0.1)
+        {
+            sprintf(terminalOutput, "Vcons = %d, Vmes = %d, PWM = %d\r", (int)commandSpeed, (int)currentSpeed, pulsewidth_propulsion);
+            serial_port.write(terminalOutput, strlen(terminalOutput));
+            tTerminalWriting.reset();
+        }
     }
 }
 
@@ -82,9 +107,25 @@
 
 
     // Read the data to clear the receive interrupt.
-    if (serial_port.read(&c, 1)) {
+    if (serial_port.read(&c, 1)) 
+    {
         decodeMessage(c);
-        serial_port.write(&c, 1);
+        //serial_port.write(&c, 1);
     }
 
+}
+
+void onSpeedCaptorInterrupt()
+{
+    //Measure car speed
+    double currentSpeedPeriod = tSpeed.read();
+    currentSpeed = DISTANCE_FOR_HALF_TURN / currentSpeedPeriod;
+    tSpeed.reset();
+}
+
+void onTickerAsservissementInterrupt()
+{
+    //Command car's speed
+    pulsewidth_propulsion = PID(commandSpeed - currentSpeed, CONTROL_RATE);
+    propulsion.pulsewidth_us(pulsewidth_propulsion);
 }
\ No newline at end of file