/ Mbed OS BagnoleAuto_PA_PAUL

.

Dependencies:   L432KC_SPI_Pey_Lal

main.cpp@115:156b8234f2de, 2022-05-18 (annotated)

Committer:
voltxd
Date:
Wed May 18 18:07:09 2022 +0000
Revision:
115:156b8234f2de
Parent:
114:c1f7be27aa5d
Child:
116:6dfcafa00e42
.

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 111:f11575e7c79b 13 //Declaration PWM outputs
voltxd 108:2fd41d299a8c 14 PwmOut propulsion(PWM_PROP);
voltxd 108:2fd41d299a8c 15 PwmOut direction(PWM_DIR);
ajuton 107:0965c72c798e 16
voltxd 113:2f8f255e99f2 17 //Declaration analog input
voltxd 113:2f8f255e99f2 18 InterruptIn speedCaptor(SPEED_CAPTOR);
voltxd 113:2f8f255e99f2 19
voltxd 111:f11575e7c79b 20 //Declaration Links
voltxd 112:478ae92cb106 21 static UnbufferedSerial serial_port(USBTX, USBRX,115200);
voltxd 113:2f8f255e99f2 22
voltxd 113:2f8f255e99f2 23 //Timers
voltxd 113:2f8f255e99f2 24 Timer tSpeed;
voltxd 113:2f8f255e99f2 25 Timer tTerminalWriting;
voltxd 113:2f8f255e99f2 26 Ticker tickAsserv;
voltxd 113:2f8f255e99f2 27
voltxd 113:2f8f255e99f2 28 //Var.
voltxd 113:2f8f255e99f2 29 double currentSpeed = 0;
voltxd 113:2f8f255e99f2 30 double commandSpeed = 5;
voltxd 113:2f8f255e99f2 31
voltxd 113:2f8f255e99f2 32 //Declaration PWM variables
voltxd 113:2f8f255e99f2 33 uint32_t pulsewidth_direction = 1100;
voltxd 113:2f8f255e99f2 34 uint32_t pulsewidth_propulsion = 1500;
mbed_official 102:6979ad8bc0bc 35
voltxd 115:156b8234f2de 36 char protocolReceivedPayload[PAYLOAD_MAX_SIZE];
voltxd 115:156b8234f2de 37
mbed_official 82:abf1b1785bd7 38 int main()
voltxd 113:2f8f255e99f2 39 {
voltxd 111:f11575e7c79b 40 //Test Serial port
voltxd 111:f11575e7c79b 41 serial_port.write("Test\n\r",strlen("Test\n\r"));
voltxd 109:4ee7ffc8f175 42
voltxd 113:2f8f255e99f2 43 char terminalOutput[256];
voltxd 113:2f8f255e99f2 44
voltxd 113:2f8f255e99f2 45 //Interrupt
voltxd 112:478ae92cb106 46 serial_port.attach(&onRxInterrupt, SerialBase::RxIrq);
voltxd 113:2f8f255e99f2 47 speedCaptor.fall(&onSpeedCaptorInterrupt);
voltxd 113:2f8f255e99f2 48 tickAsserv.attach(&onTickerAsservissementInterrupt, 1.0 / CONTROL_RATE);
voltxd 112:478ae92cb106 49
voltxd 111:f11575e7c79b 50 //Init. propulsion PWM
ajuton 107:0965c72c798e 51 propulsion.period_us(20000);
voltxd 109:4ee7ffc8f175 52 propulsion.pulsewidth_us(pulsewidth_propulsion);
voltxd 109:4ee7ffc8f175 53
voltxd 108:2fd41d299a8c 54 thread_sleep_for(2000);
voltxd 109:4ee7ffc8f175 55
voltxd 111:f11575e7c79b 56 //Init. Direction PWM
ajuton 107:0965c72c798e 57 direction.period_us(20000);
ajuton 107:0965c72c798e 58 direction.pulsewidth_us(pulsewidth_direction);
voltxd 109:4ee7ffc8f175 59
voltxd 113:2f8f255e99f2 60 //Timers init.
voltxd 113:2f8f255e99f2 61 tSpeed.start();
voltxd 113:2f8f255e99f2 62 tTerminalWriting.start();
voltxd 108:2fd41d299a8c 63
voltxd 111:f11575e7c79b 64 //Infinite loop
voltxd 111:f11575e7c79b 65 while(1)
voltxd 111:f11575e7c79b 66 {
voltxd 111:f11575e7c79b 67 //If decoding has ended, get and changes PWM values.
voltxd 110:a6d1d3525014 68 if(isDataAvailable())
voltxd 109:4ee7ffc8f175 69 {
voltxd 115:156b8234f2de 70 char receivedCommand = getVerifiedPayload(protocolReceivedPayload);
voltxd 115:156b8234f2de 71 switch(receivedCommand)
voltxd 115:156b8234f2de 72 {
voltxd 115:156b8234f2de 73 case COMMAND_PWM:
voltxd 115:156b8234f2de 74 pulsewidth_propulsion = protocolReceivedPayload[0] << 8;
voltxd 115:156b8234f2de 75 pulsewidth_propulsion += protocolReceivedPayload[1];
voltxd 115:156b8234f2de 76 pulsewidth_direction = protocolReceivedPayload[2] << 8;
voltxd 115:156b8234f2de 77 pulsewidth_direction += protocolReceivedPayload[3];
voltxd 115:156b8234f2de 78 propulsion.pulsewidth_us(pulsewidth_propulsion);
voltxd 115:156b8234f2de 79 direction.pulsewidth_us(pulsewidth_direction);
voltxd 115:156b8234f2de 80 break;
voltxd 115:156b8234f2de 81
voltxd 115:156b8234f2de 82 case COMMAND_ASSERVISSEMENT:
voltxd 115:156b8234f2de 83 commandSpeed = convertBytesToFloat(protocolReceivedPayload, 0);
voltxd 115:156b8234f2de 84 pulsewidth_direction = protocolReceivedPayload[4] << 8;
voltxd 115:156b8234f2de 85 pulsewidth_direction += protocolReceivedPayload[5];
voltxd 115:156b8234f2de 86 direction.pulsewidth_us(pulsewidth_direction);
voltxd 115:156b8234f2de 87 break;
voltxd 115:156b8234f2de 88
voltxd 115:156b8234f2de 89 case COMMAND_PARAMETRES:
voltxd 115:156b8234f2de 90 {
voltxd 115:156b8234f2de 91 float kp = convertBytesToFloat(protocolReceivedPayload, 0);
voltxd 115:156b8234f2de 92 float ki = convertBytesToFloat(protocolReceivedPayload, 4);
voltxd 115:156b8234f2de 93 setPIDParameters(kp, ki);
voltxd 115:156b8234f2de 94 break;
voltxd 115:156b8234f2de 95 }
voltxd 115:156b8234f2de 96 }
voltxd 111:f11575e7c79b 97 }
voltxd 112:478ae92cb106 98
voltxd 113:2f8f255e99f2 99 //If no speed captor interrupt, consider speed = 0
voltxd 113:2f8f255e99f2 100 if (tSpeed.read() > 0.25)
voltxd 113:2f8f255e99f2 101 {
voltxd 113:2f8f255e99f2 102 currentSpeed = 0;
voltxd 113:2f8f255e99f2 103 tSpeed.reset();
voltxd 113:2f8f255e99f2 104 }
voltxd 113:2f8f255e99f2 105
voltxd 113:2f8f255e99f2 106 //Write in terminal
voltxd 114:c1f7be27aa5d 107 if (tTerminalWriting.read() > 0.02)
voltxd 113:2f8f255e99f2 108 {
voltxd 114:c1f7be27aa5d 109 sprintf(terminalOutput, "Vcons = %d m/s, Vmes = %d mm/s, PWM = %d\n\r", (int)commandSpeed, (int)(currentSpeed*1000), pulsewidth_propulsion);
voltxd 113:2f8f255e99f2 110 serial_port.write(terminalOutput, strlen(terminalOutput));
voltxd 113:2f8f255e99f2 111 tTerminalWriting.reset();
voltxd 113:2f8f255e99f2 112 }
Jonathan Austin 0:2757d7abb7d9 113 }
Jonathan Austin 0:2757d7abb7d9 114 }
voltxd 112:478ae92cb106 115
voltxd 112:478ae92cb106 116 void onRxInterrupt()
voltxd 112:478ae92cb106 117 {
voltxd 112:478ae92cb106 118 char c;
voltxd 112:478ae92cb106 119
voltxd 112:478ae92cb106 120
voltxd 112:478ae92cb106 121 // Read the data to clear the receive interrupt.
voltxd 113:2f8f255e99f2 122 if (serial_port.read(&c, 1))
voltxd 113:2f8f255e99f2 123 {
voltxd 112:478ae92cb106 124 decodeMessage(c);
voltxd 113:2f8f255e99f2 125 //serial_port.write(&c, 1);
voltxd 112:478ae92cb106 126 }
voltxd 112:478ae92cb106 127
voltxd 113:2f8f255e99f2 128 }
voltxd 113:2f8f255e99f2 129
voltxd 113:2f8f255e99f2 130 void onSpeedCaptorInterrupt()
voltxd 113:2f8f255e99f2 131 {
voltxd 113:2f8f255e99f2 132 //Measure car speed
voltxd 113:2f8f255e99f2 133 double currentSpeedPeriod = tSpeed.read();
voltxd 113:2f8f255e99f2 134 currentSpeed = DISTANCE_FOR_HALF_TURN / currentSpeedPeriod;
voltxd 113:2f8f255e99f2 135 tSpeed.reset();
voltxd 113:2f8f255e99f2 136 }
voltxd 113:2f8f255e99f2 137
voltxd 113:2f8f255e99f2 138 void onTickerAsservissementInterrupt()
voltxd 113:2f8f255e99f2 139 {
voltxd 113:2f8f255e99f2 140 //Command car's speed
voltxd 113:2f8f255e99f2 141 pulsewidth_propulsion = PID(commandSpeed - currentSpeed, CONTROL_RATE);
voltxd 113:2f8f255e99f2 142 propulsion.pulsewidth_us(pulsewidth_propulsion);
voltxd 114:c1f7be27aa5d 143
voltxd 114:c1f7be27aa5d 144 //Open loop (for coef)
voltxd 112:478ae92cb106 145 }