/ Mbed OS BagnoleAuto_PA_PAUL

.

Dependencies:   L432KC_SPI_Pey_Lal

main.cpp@113:2f8f255e99f2, 2022-05-17 (annotated)

Committer:
voltxd
Date:
Tue May 17 22:44:42 2022 +0000
Revision:
113:2f8f255e99f2
Parent:
112:478ae92cb106
Child:
114:c1f7be27aa5d
.

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"
ajuton 107:0965c72c798e 10
voltxd 108:2fd41d299a8c 11 //Define PWM
voltxd 112:478ae92cb106 12 #define PWM_PROP D9
voltxd 112:478ae92cb106 13 #define PWM_DIR D10
voltxd 112:478ae92cb106 14
voltxd 113:2f8f255e99f2 15 //Define interrupt Input
voltxd 113:2f8f255e99f2 16 #define SPEED_CAPTOR A4
voltxd 113:2f8f255e99f2 17
voltxd 113:2f8f255e99f2 18 //Define COEF
voltxd 113:2f8f255e99f2 19 #define DISTANCE_FOR_HALF_TURN 3 * 3.14 //TODO: mettre vrai valeur
voltxd 113:2f8f255e99f2 20 #define CONTROL_RATE 1000.0
voltxd 113:2f8f255e99f2 21
voltxd 113:2f8f255e99f2 22 //fontion interrupt
voltxd 112:478ae92cb106 23 void onRxInterrupt();
voltxd 113:2f8f255e99f2 24 void onSpeedCaptorInterrupt();
voltxd 113:2f8f255e99f2 25 void onTickerAsservissementInterrupt();
ajuton 107:0965c72c798e 26
voltxd 111:f11575e7c79b 27 //Declaration PWM outputs
voltxd 108:2fd41d299a8c 28 PwmOut propulsion(PWM_PROP);
voltxd 108:2fd41d299a8c 29 PwmOut direction(PWM_DIR);
ajuton 107:0965c72c798e 30
voltxd 113:2f8f255e99f2 31 //Declaration analog input
voltxd 113:2f8f255e99f2 32 InterruptIn speedCaptor(SPEED_CAPTOR);
voltxd 113:2f8f255e99f2 33
voltxd 111:f11575e7c79b 34 //Declaration Links
voltxd 112:478ae92cb106 35 static UnbufferedSerial serial_port(USBTX, USBRX,115200);
voltxd 113:2f8f255e99f2 36
voltxd 113:2f8f255e99f2 37 //Timers
voltxd 113:2f8f255e99f2 38 Timer tSpeed;
voltxd 113:2f8f255e99f2 39 Timer tTerminalWriting;
voltxd 113:2f8f255e99f2 40 Ticker tickAsserv;
voltxd 113:2f8f255e99f2 41
voltxd 113:2f8f255e99f2 42 //Var.
voltxd 113:2f8f255e99f2 43 double currentSpeed = 0;
voltxd 113:2f8f255e99f2 44 double commandSpeed = 5;
voltxd 113:2f8f255e99f2 45
voltxd 113:2f8f255e99f2 46 //Declaration PWM variables
voltxd 113:2f8f255e99f2 47 uint32_t pulsewidth_direction = 1100;
voltxd 113:2f8f255e99f2 48 uint32_t pulsewidth_propulsion = 1500;
mbed_official 102:6979ad8bc0bc 49
mbed_official 82:abf1b1785bd7 50 int main()
voltxd 113:2f8f255e99f2 51 {
voltxd 111:f11575e7c79b 52 //Test Serial port
voltxd 111:f11575e7c79b 53 serial_port.write("Test\n\r",strlen("Test\n\r"));
voltxd 109:4ee7ffc8f175 54
voltxd 113:2f8f255e99f2 55 char terminalOutput[256];
voltxd 113:2f8f255e99f2 56
voltxd 113:2f8f255e99f2 57 //Interrupt
voltxd 112:478ae92cb106 58 serial_port.attach(&onRxInterrupt, SerialBase::RxIrq);
voltxd 113:2f8f255e99f2 59 speedCaptor.fall(&onSpeedCaptorInterrupt);
voltxd 113:2f8f255e99f2 60 tickAsserv.attach(&onTickerAsservissementInterrupt, 1.0 / CONTROL_RATE);
voltxd 112:478ae92cb106 61
voltxd 111:f11575e7c79b 62 //Init. propulsion PWM
ajuton 107:0965c72c798e 63 propulsion.period_us(20000);
voltxd 109:4ee7ffc8f175 64 propulsion.pulsewidth_us(pulsewidth_propulsion);
voltxd 109:4ee7ffc8f175 65
voltxd 108:2fd41d299a8c 66 thread_sleep_for(2000);
voltxd 109:4ee7ffc8f175 67
voltxd 111:f11575e7c79b 68 //Init. Direction PWM
ajuton 107:0965c72c798e 69 direction.period_us(20000);
ajuton 107:0965c72c798e 70 direction.pulsewidth_us(pulsewidth_direction);
voltxd 109:4ee7ffc8f175 71
voltxd 113:2f8f255e99f2 72 //Timers init.
voltxd 113:2f8f255e99f2 73 tSpeed.start();
voltxd 113:2f8f255e99f2 74 tTerminalWriting.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 110:a6d1d3525014 82 getVerifiedPWMValues(&pulsewidth_propulsion, &pulsewidth_direction);
voltxd 109:4ee7ffc8f175 83 propulsion.pulsewidth_us(pulsewidth_propulsion);
voltxd 109:4ee7ffc8f175 84 direction.pulsewidth_us(pulsewidth_direction);
voltxd 111:f11575e7c79b 85 }
voltxd 112:478ae92cb106 86
voltxd 113:2f8f255e99f2 87 //If no speed captor interrupt, consider speed = 0
voltxd 113:2f8f255e99f2 88 if (tSpeed.read() > 0.25)
voltxd 113:2f8f255e99f2 89 {
voltxd 113:2f8f255e99f2 90 currentSpeed = 0;
voltxd 113:2f8f255e99f2 91 tSpeed.reset();
voltxd 113:2f8f255e99f2 92 }
voltxd 113:2f8f255e99f2 93
voltxd 113:2f8f255e99f2 94 //Write in terminal
voltxd 113:2f8f255e99f2 95 if (tTerminalWriting.read() > 0.1)
voltxd 113:2f8f255e99f2 96 {
voltxd 113:2f8f255e99f2 97 sprintf(terminalOutput, "Vcons = %d, Vmes = %d, PWM = %d\r", (int)commandSpeed, (int)currentSpeed, pulsewidth_propulsion);
voltxd 113:2f8f255e99f2 98 serial_port.write(terminalOutput, strlen(terminalOutput));
voltxd 113:2f8f255e99f2 99 tTerminalWriting.reset();
voltxd 113:2f8f255e99f2 100 }
Jonathan Austin 0:2757d7abb7d9 101 }
Jonathan Austin 0:2757d7abb7d9 102 }
voltxd 112:478ae92cb106 103
voltxd 112:478ae92cb106 104 void onRxInterrupt()
voltxd 112:478ae92cb106 105 {
voltxd 112:478ae92cb106 106 char c;
voltxd 112:478ae92cb106 107
voltxd 112:478ae92cb106 108
voltxd 112:478ae92cb106 109 // Read the data to clear the receive interrupt.
voltxd 113:2f8f255e99f2 110 if (serial_port.read(&c, 1))
voltxd 113:2f8f255e99f2 111 {
voltxd 112:478ae92cb106 112 decodeMessage(c);
voltxd 113:2f8f255e99f2 113 //serial_port.write(&c, 1);
voltxd 112:478ae92cb106 114 }
voltxd 112:478ae92cb106 115
voltxd 113:2f8f255e99f2 116 }
voltxd 113:2f8f255e99f2 117
voltxd 113:2f8f255e99f2 118 void onSpeedCaptorInterrupt()
voltxd 113:2f8f255e99f2 119 {
voltxd 113:2f8f255e99f2 120 //Measure car speed
voltxd 113:2f8f255e99f2 121 double currentSpeedPeriod = tSpeed.read();
voltxd 113:2f8f255e99f2 122 currentSpeed = DISTANCE_FOR_HALF_TURN / currentSpeedPeriod;
voltxd 113:2f8f255e99f2 123 tSpeed.reset();
voltxd 113:2f8f255e99f2 124 }
voltxd 113:2f8f255e99f2 125
voltxd 113:2f8f255e99f2 126 void onTickerAsservissementInterrupt()
voltxd 113:2f8f255e99f2 127 {
voltxd 113:2f8f255e99f2 128 //Command car's speed
voltxd 113:2f8f255e99f2 129 pulsewidth_propulsion = PID(commandSpeed - currentSpeed, CONTROL_RATE);
voltxd 113:2f8f255e99f2 130 propulsion.pulsewidth_us(pulsewidth_propulsion);
voltxd 112:478ae92cb106 131 }