main.cpp

00001 /* mbed Microcontroller Library
00002  * Copyright (c) 2019 ARM Limited
00003  * SPDX-License-Identifier: Apache-2.0
00004  */
00005 
00006 #include "mbed.h"
00007 #include "platform/mbed_thread.h"
00008 #include "protocol.h"
00009 #include "asservissement.hpp"
00010 #include "toolbox.hpp"
00011 #include "main.hpp"
00012 
00013 //Constants
00014 const char speedMessageLength = 6;
00015 const float minimalSpeed = 0.05;
00016 const int terminalOutputFrequency = 50;
00017 const int speedTransmitionFrequency = 250;
00018 
00019 //Flag
00020 bool commandEnabled = true;
00021 
00022 //Declaration PWM outputs
00023 PwmOut propulsion(PWM_PROP);
00024 PwmOut direction(PWM_DIR);
00025 
00026 //Declaration analog input
00027 InterruptIn speedCaptor(SPEED_CAPTOR);
00028 
00029 //Declaration Links
00030 static UnbufferedSerial serial_port(USBTX, USBRX,115200);
00031 
00032 //Timers
00033 Timer tSpeed;
00034 Timer tTerminalWriting;
00035 Timer tSpeedTx;
00036 Ticker tickAsserv;
00037 
00038 //Var.
00039 double currentSpeed = 0;
00040 double commandSpeed = 5;
00041 
00042 //Declaration PWM variables
00043 uint32_t pulsewidth_direction = 1100;
00044 uint32_t pulsewidth_propulsion = 1500;
00045 
00046 char protocolReceivedPayload[PAYLOAD_MAX_SIZE];
00047 
00048 int main()
00049 {    
00050     //Test Serial port
00051     serial_port.write("Test\n\r",strlen("Test\n\r"));
00052     
00053     char terminalOutput[256];
00054     char msgSpeedProtocolOutput[speedMessageLength] = {0};
00055     
00056     //Interrupt
00057     serial_port.attach(&onRxInterrupt, SerialBase::RxIrq);
00058     speedCaptor.fall(&onSpeedCaptorInterrupt);
00059     tickAsserv.attach(&onTickerAsservissementInterrupt, 1.0 / CONTROL_RATE);
00060     
00061     //Init. propulsion PWM
00062     propulsion.period_us(20000);
00063     propulsion.pulsewidth_us(pulsewidth_propulsion);
00064     
00065     thread_sleep_for(2000);
00066     
00067     //Init. Direction PWM
00068     direction.period_us(20000);
00069     direction.pulsewidth_us(pulsewidth_direction);
00070     
00071     //Timers init.
00072     tSpeed.start();
00073     tTerminalWriting.start();
00074     tSpeedTx.start();
00075     
00076     //Infinite loop
00077     while(1) 
00078     {
00079         //If decoding has ended, get and changes PWM values.
00080         if(isDataAvailable())
00081         {
00082             char receivedCommand = getVerifiedPayload(protocolReceivedPayload);
00083             switch(receivedCommand)
00084             {
00085                 case COMMAND_PWM:
00086                     //Turn off PI
00087                     commandEnabled = false;
00088                     //1st. get the values
00089                     pulsewidth_propulsion = protocolReceivedPayload[0] << 8;
00090                     pulsewidth_propulsion += protocolReceivedPayload[1];
00091                     pulsewidth_direction = protocolReceivedPayload[2] << 8;
00092                     pulsewidth_direction += protocolReceivedPayload[3];
00093                     //2nd. allocate the values
00094                     propulsion.pulsewidth_us(pulsewidth_propulsion);
00095                     direction.pulsewidth_us(pulsewidth_direction);
00096                     break;
00097                     
00098                 case COMMAND_ASSERVISSEMENT: 
00099                     //Turn on PI
00100                     commandEnabled = true;
00101                     //1st. 
00102                     commandSpeed = convertBytesToFloat(protocolReceivedPayload, 0);
00103                     pulsewidth_direction = protocolReceivedPayload[4] << 8;
00104                     pulsewidth_direction += protocolReceivedPayload[5];
00105                     //2nd.
00106                     direction.pulsewidth_us(pulsewidth_direction);
00107                     break;
00108                     
00109                 case COMMAND_PARAMETRES:
00110                 {                
00111                     //Turn on PI       
00112                     commandEnabled = true;
00113                     //1st.
00114                     float kp = convertBytesToFloat(protocolReceivedPayload, 0);
00115                     float ki = convertBytesToFloat(protocolReceivedPayload, 4);                                                
00116                     //2nd.
00117                     setPIDParameters(kp, ki);
00118                     break;   
00119                 }
00120             }
00121         }        
00122         
00123         //If no speed captor interrupt, consider speed = 0
00124         if (tSpeed.read() > DISTANCE_FOR_HALF_TURN / minimalSpeed)
00125         {
00126             currentSpeed = 0;
00127             tSpeed.reset();
00128         }
00129         
00130         //Write in terminal (will need to get out)
00131         if (tTerminalWriting.read() > 1.0 / terminalOutputFrequency)
00132         {
00133             sprintf(terminalOutput, "Vcons = %d m/s, Vmes = %d mm/s, PWM = %d\n\r", (int)commandSpeed, (int)(currentSpeed*1000), pulsewidth_propulsion);
00134             serial_port.write(terminalOutput, strlen(terminalOutput));
00135             tTerminalWriting.reset();
00136         }
00137         
00138         //Transmit speed to rPi/Jetson
00139         if(tSpeedTx.read() > 1.0 / speedTransmitionFrequency)
00140         {
00141             encodeMessage(msgSpeedProtocolOutput, currentSpeed);
00142             serial_port.write(msgSpeedProtocolOutput, speedMessageLength);
00143         }
00144     }
00145 }
00146 
00147 void onRxInterrupt()
00148 {
00149     char c;
00150     
00151     // Read the data to clear the receive interrupt.
00152     if (serial_port.read(&c, 1)) 
00153         decodeMessage(c);
00154 }
00155 
00156 void onSpeedCaptorInterrupt()
00157 {
00158     //Measure car speed
00159     double currentSpeedPeriod = tSpeed.read();
00160     currentSpeed = DISTANCE_FOR_HALF_TURN / currentSpeedPeriod;
00161     if (pulsewidth_propulsion > 1500)
00162         currentSpeed = -currentSpeed;
00163     tSpeed.reset();
00164 }
00165 
00166 void onTickerAsservissementInterrupt()
00167 {
00168     //Command car's speed
00169     if (commandEnabled)
00170         pulsewidth_propulsion = PID(commandSpeed - currentSpeed, CONTROL_RATE);
00171     propulsion.pulsewidth_us(pulsewidth_propulsion);
00172     
00173     //Open loop (for transformation parameters)
00174     //pulsewidth_propulsion = PID(commandSpeed, CONTROL_RATE);
00175     //propulsion.pulsewidth_us(pulsewidth_propulsion);
00176 }