/ Mbed OS BagnoleAuto_PA_PAUL

.

Dependencies:   L432KC_SPI_Pey_Lal

main.cpp@117:58148bdd13b7, 2022-05-23 (annotated)

Committer:
voltxd
Date:
Mon May 23 13:05:26 2022 +0000
Revision:
117:58148bdd13b7
Parent:
116:6dfcafa00e42
Child:
118:67d6698069fd
CuttingPIcommandWhenPWMcommand

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mbed_official 82:abf1b1785bd7 1 /* mbed Microcontroller Library
mbed_official 102:6979ad8bc0bc 2 * Copyright (c) 2019 ARM Limited
mbed_official 82:abf1b1785bd7 3 * SPDX-License-Identifier: Apache-2.0
mbed_official 82:abf1b1785bd7 4 */
mbed_official 82:abf1b1785bd7 5
Jonathan Austin 0:2757d7abb7d9 6 #include "mbed.h"
mbed_official 100:ec006d6f3cb6 7 #include "platform/mbed_thread.h"
voltxd 109:4ee7ffc8f175 8 #include "protocol.h"
voltxd 113:2f8f255e99f2 9 #include "asservissement.hpp"
voltxd 114:c1f7be27aa5d 10 #include "toolbox.hpp"
voltxd 114:c1f7be27aa5d 11 #include "main.hpp"
ajuton 107:0965c72c798e 12
voltxd 116:6dfcafa00e42 13 //Constants
voltxd 116:6dfcafa00e42 14 const char speedMessageLength = 6;
voltxd 116:6dfcafa00e42 15 const float minimalSpeed = 0.05;
voltxd 116:6dfcafa00e42 16 const int terminalOutputFrequency = 50;
voltxd 116:6dfcafa00e42 17 const int speedTransmitionFrequency = 250;
voltxd 116:6dfcafa00e42 18
voltxd 116:6dfcafa00e42 19 //Flag
voltxd 116:6dfcafa00e42 20 bool commandEnabled = true;
voltxd 116:6dfcafa00e42 21
voltxd 111:f11575e7c79b 22 //Declaration PWM outputs
voltxd 108:2fd41d299a8c 23 PwmOut propulsion(PWM_PROP);
voltxd 108:2fd41d299a8c 24 PwmOut direction(PWM_DIR);
ajuton 107:0965c72c798e 25
voltxd 113:2f8f255e99f2 26 //Declaration analog input
voltxd 113:2f8f255e99f2 27 InterruptIn speedCaptor(SPEED_CAPTOR);
voltxd 113:2f8f255e99f2 28
voltxd 111:f11575e7c79b 29 //Declaration Links
voltxd 112:478ae92cb106 30 static UnbufferedSerial serial_port(USBTX, USBRX,115200);
voltxd 113:2f8f255e99f2 31
voltxd 113:2f8f255e99f2 32 //Timers
voltxd 113:2f8f255e99f2 33 Timer tSpeed;
voltxd 113:2f8f255e99f2 34 Timer tTerminalWriting;
voltxd 116:6dfcafa00e42 35 Timer tSpeedTx;
voltxd 113:2f8f255e99f2 36 Ticker tickAsserv;
voltxd 113:2f8f255e99f2 37
voltxd 113:2f8f255e99f2 38 //Var.
voltxd 113:2f8f255e99f2 39 double currentSpeed = 0;
voltxd 113:2f8f255e99f2 40 double commandSpeed = 5;
voltxd 113:2f8f255e99f2 41
voltxd 113:2f8f255e99f2 42 //Declaration PWM variables
voltxd 113:2f8f255e99f2 43 uint32_t pulsewidth_direction = 1100;
voltxd 113:2f8f255e99f2 44 uint32_t pulsewidth_propulsion = 1500;
mbed_official 102:6979ad8bc0bc 45
voltxd 115:156b8234f2de 46 char protocolReceivedPayload[PAYLOAD_MAX_SIZE];
voltxd 115:156b8234f2de 47
mbed_official 82:abf1b1785bd7 48 int main()
voltxd 113:2f8f255e99f2 49 {
voltxd 111:f11575e7c79b 50 //Test Serial port
voltxd 111:f11575e7c79b 51 serial_port.write("Test\n\r",strlen("Test\n\r"));
voltxd 109:4ee7ffc8f175 52
voltxd 113:2f8f255e99f2 53 char terminalOutput[256];
voltxd 116:6dfcafa00e42 54 char msgSpeedProtocolOutput[speedMessageLength] = {0};
voltxd 113:2f8f255e99f2 55
voltxd 113:2f8f255e99f2 56 //Interrupt
voltxd 112:478ae92cb106 57 serial_port.attach(&onRxInterrupt, SerialBase::RxIrq);
voltxd 113:2f8f255e99f2 58 speedCaptor.fall(&onSpeedCaptorInterrupt);
voltxd 113:2f8f255e99f2 59 tickAsserv.attach(&onTickerAsservissementInterrupt, 1.0 / CONTROL_RATE);
voltxd 112:478ae92cb106 60
voltxd 111:f11575e7c79b 61 //Init. propulsion PWM
ajuton 107:0965c72c798e 62 propulsion.period_us(20000);
voltxd 109:4ee7ffc8f175 63 propulsion.pulsewidth_us(pulsewidth_propulsion);
voltxd 109:4ee7ffc8f175 64
voltxd 108:2fd41d299a8c 65 thread_sleep_for(2000);
voltxd 109:4ee7ffc8f175 66
voltxd 111:f11575e7c79b 67 //Init. Direction PWM
ajuton 107:0965c72c798e 68 direction.period_us(20000);
ajuton 107:0965c72c798e 69 direction.pulsewidth_us(pulsewidth_direction);
voltxd 109:4ee7ffc8f175 70
voltxd 113:2f8f255e99f2 71 //Timers init.
voltxd 113:2f8f255e99f2 72 tSpeed.start();
voltxd 113:2f8f255e99f2 73 tTerminalWriting.start();
voltxd 116:6dfcafa00e42 74 tSpeedTx.start();
voltxd 108:2fd41d299a8c 75
voltxd 111:f11575e7c79b 76 //Infinite loop
voltxd 111:f11575e7c79b 77 while(1)
voltxd 111:f11575e7c79b 78 {
voltxd 111:f11575e7c79b 79 //If decoding has ended, get and changes PWM values.
voltxd 110:a6d1d3525014 80 if(isDataAvailable())
voltxd 109:4ee7ffc8f175 81 {
voltxd 115:156b8234f2de 82 char receivedCommand = getVerifiedPayload(protocolReceivedPayload);
voltxd 115:156b8234f2de 83 switch(receivedCommand)
voltxd 115:156b8234f2de 84 {
voltxd 115:156b8234f2de 85 case COMMAND_PWM:
voltxd 117:58148bdd13b7 86 //Turn off PI
voltxd 117:58148bdd13b7 87 commandEnabled = false;
voltxd 116:6dfcafa00e42 88 //1st. get the values
voltxd 115:156b8234f2de 89 pulsewidth_propulsion = protocolReceivedPayload[0] << 8;
voltxd 115:156b8234f2de 90 pulsewidth_propulsion += protocolReceivedPayload[1];
voltxd 115:156b8234f2de 91 pulsewidth_direction = protocolReceivedPayload[2] << 8;
voltxd 115:156b8234f2de 92 pulsewidth_direction += protocolReceivedPayload[3];
voltxd 116:6dfcafa00e42 93 //2nd. allocate the values
voltxd 115:156b8234f2de 94 propulsion.pulsewidth_us(pulsewidth_propulsion);
voltxd 115:156b8234f2de 95 direction.pulsewidth_us(pulsewidth_direction);
voltxd 115:156b8234f2de 96 break;
voltxd 115:156b8234f2de 97
voltxd 115:156b8234f2de 98 case COMMAND_ASSERVISSEMENT:
voltxd 117:58148bdd13b7 99 //Turn on PI
voltxd 117:58148bdd13b7 100 commandEnabled = true;
voltxd 116:6dfcafa00e42 101 //1st.
voltxd 115:156b8234f2de 102 commandSpeed = convertBytesToFloat(protocolReceivedPayload, 0);
voltxd 115:156b8234f2de 103 pulsewidth_direction = protocolReceivedPayload[4] << 8;
voltxd 115:156b8234f2de 104 pulsewidth_direction += protocolReceivedPayload[5];
voltxd 116:6dfcafa00e42 105 //2nd.
voltxd 115:156b8234f2de 106 direction.pulsewidth_us(pulsewidth_direction);
voltxd 115:156b8234f2de 107 break;
voltxd 115:156b8234f2de 108
voltxd 115:156b8234f2de 109 case COMMAND_PARAMETRES:
voltxd 117:58148bdd13b7 110 {
voltxd 117:58148bdd13b7 111 //Turn on PI
voltxd 117:58148bdd13b7 112 commandEnabled = true;
voltxd 116:6dfcafa00e42 113 //1st.
voltxd 115:156b8234f2de 114 float kp = convertBytesToFloat(protocolReceivedPayload, 0);
voltxd 115:156b8234f2de 115 float ki = convertBytesToFloat(protocolReceivedPayload, 4);
voltxd 116:6dfcafa00e42 116 //2nd.
voltxd 115:156b8234f2de 117 setPIDParameters(kp, ki);
voltxd 115:156b8234f2de 118 break;
voltxd 115:156b8234f2de 119 }
voltxd 115:156b8234f2de 120 }
voltxd 111:f11575e7c79b 121 }
voltxd 112:478ae92cb106 122
voltxd 113:2f8f255e99f2 123 //If no speed captor interrupt, consider speed = 0
voltxd 116:6dfcafa00e42 124 if (tSpeed.read() > DISTANCE_FOR_HALF_TURN / minimalSpeed)
voltxd 113:2f8f255e99f2 125 {
voltxd 113:2f8f255e99f2 126 currentSpeed = 0;
voltxd 113:2f8f255e99f2 127 tSpeed.reset();
voltxd 113:2f8f255e99f2 128 }
voltxd 113:2f8f255e99f2 129
voltxd 116:6dfcafa00e42 130 //Write in terminal (will need to get out)
voltxd 116:6dfcafa00e42 131 if (tTerminalWriting.read() > 1.0 / terminalOutputFrequency)
voltxd 113:2f8f255e99f2 132 {
voltxd 114:c1f7be27aa5d 133 sprintf(terminalOutput, "Vcons = %d m/s, Vmes = %d mm/s, PWM = %d\n\r", (int)commandSpeed, (int)(currentSpeed*1000), pulsewidth_propulsion);
voltxd 113:2f8f255e99f2 134 serial_port.write(terminalOutput, strlen(terminalOutput));
voltxd 113:2f8f255e99f2 135 tTerminalWriting.reset();
voltxd 113:2f8f255e99f2 136 }
voltxd 116:6dfcafa00e42 137
voltxd 116:6dfcafa00e42 138 //Transmit speed to rPi/Jetson
voltxd 116:6dfcafa00e42 139 if(tSpeedTx.read() > 1.0 / speedTransmitionFrequency)
voltxd 116:6dfcafa00e42 140 {
voltxd 116:6dfcafa00e42 141 encodeMessage(msgSpeedProtocolOutput, currentSpeed);
voltxd 116:6dfcafa00e42 142 serial_port.write(msgSpeedProtocolOutput, speedMessageLength);
voltxd 116:6dfcafa00e42 143 }
Jonathan Austin 0:2757d7abb7d9 144 }
Jonathan Austin 0:2757d7abb7d9 145 }
voltxd 112:478ae92cb106 146
voltxd 112:478ae92cb106 147 void onRxInterrupt()
voltxd 112:478ae92cb106 148 {
voltxd 112:478ae92cb106 149 char c;
voltxd 116:6dfcafa00e42 150
voltxd 112:478ae92cb106 151 // Read the data to clear the receive interrupt.
voltxd 113:2f8f255e99f2 152 if (serial_port.read(&c, 1))
voltxd 112:478ae92cb106 153 decodeMessage(c);
voltxd 113:2f8f255e99f2 154 }
voltxd 113:2f8f255e99f2 155
voltxd 113:2f8f255e99f2 156 void onSpeedCaptorInterrupt()
voltxd 113:2f8f255e99f2 157 {
voltxd 113:2f8f255e99f2 158 //Measure car speed
voltxd 113:2f8f255e99f2 159 double currentSpeedPeriod = tSpeed.read();
voltxd 113:2f8f255e99f2 160 currentSpeed = DISTANCE_FOR_HALF_TURN / currentSpeedPeriod;
voltxd 113:2f8f255e99f2 161 tSpeed.reset();
voltxd 113:2f8f255e99f2 162 }
voltxd 113:2f8f255e99f2 163
voltxd 113:2f8f255e99f2 164 void onTickerAsservissementInterrupt()
voltxd 113:2f8f255e99f2 165 {
voltxd 113:2f8f255e99f2 166 //Command car's speed
voltxd 117:58148bdd13b7 167 if (commandEnabled)
voltxd 117:58148bdd13b7 168 pulsewidth_propulsion = PID(commandSpeed - currentSpeed, CONTROL_RATE);
voltxd 113:2f8f255e99f2 169 propulsion.pulsewidth_us(pulsewidth_propulsion);
voltxd 114:c1f7be27aa5d 170
voltxd 117:58148bdd13b7 171 //Open loop (for transformation parameters)
voltxd 117:58148bdd13b7 172 //pulsewidth_propulsion = PID(commandSpeed, CONTROL_RATE);
voltxd 117:58148bdd13b7 173 //propulsion.pulsewidth_us(pulsewidth_propulsion);
voltxd 112:478ae92cb106 174 }