.
Dependencies: L432KC_SPI_Pey_Lal
main.cpp@114:c1f7be27aa5d, 2022-05-18 (annotated)
- Committer:
- voltxd
- Date:
- Wed May 18 16:12:59 2022 +0000
- Revision:
- 114:c1f7be27aa5d
- Parent:
- 113:2f8f255e99f2
- Child:
- 115:156b8234f2de
ratio;
Who changed what in which revision?
User | Revision | Line number | New 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 | |
mbed_official | 82:abf1b1785bd7 | 36 | int main() |
voltxd | 113:2f8f255e99f2 | 37 | { |
voltxd | 111:f11575e7c79b | 38 | //Test Serial port |
voltxd | 111:f11575e7c79b | 39 | serial_port.write("Test\n\r",strlen("Test\n\r")); |
voltxd | 109:4ee7ffc8f175 | 40 | |
voltxd | 113:2f8f255e99f2 | 41 | char terminalOutput[256]; |
voltxd | 113:2f8f255e99f2 | 42 | |
voltxd | 113:2f8f255e99f2 | 43 | //Interrupt |
voltxd | 112:478ae92cb106 | 44 | serial_port.attach(&onRxInterrupt, SerialBase::RxIrq); |
voltxd | 113:2f8f255e99f2 | 45 | speedCaptor.fall(&onSpeedCaptorInterrupt); |
voltxd | 113:2f8f255e99f2 | 46 | tickAsserv.attach(&onTickerAsservissementInterrupt, 1.0 / CONTROL_RATE); |
voltxd | 112:478ae92cb106 | 47 | |
voltxd | 111:f11575e7c79b | 48 | //Init. propulsion PWM |
ajuton | 107:0965c72c798e | 49 | propulsion.period_us(20000); |
voltxd | 109:4ee7ffc8f175 | 50 | propulsion.pulsewidth_us(pulsewidth_propulsion); |
voltxd | 109:4ee7ffc8f175 | 51 | |
voltxd | 108:2fd41d299a8c | 52 | thread_sleep_for(2000); |
voltxd | 109:4ee7ffc8f175 | 53 | |
voltxd | 111:f11575e7c79b | 54 | //Init. Direction PWM |
ajuton | 107:0965c72c798e | 55 | direction.period_us(20000); |
ajuton | 107:0965c72c798e | 56 | direction.pulsewidth_us(pulsewidth_direction); |
voltxd | 109:4ee7ffc8f175 | 57 | |
voltxd | 113:2f8f255e99f2 | 58 | //Timers init. |
voltxd | 113:2f8f255e99f2 | 59 | tSpeed.start(); |
voltxd | 113:2f8f255e99f2 | 60 | tTerminalWriting.start(); |
voltxd | 108:2fd41d299a8c | 61 | |
voltxd | 111:f11575e7c79b | 62 | //Infinite loop |
voltxd | 111:f11575e7c79b | 63 | while(1) |
voltxd | 111:f11575e7c79b | 64 | { |
voltxd | 111:f11575e7c79b | 65 | //If decoding has ended, get and changes PWM values. |
voltxd | 110:a6d1d3525014 | 66 | if(isDataAvailable()) |
voltxd | 109:4ee7ffc8f175 | 67 | { |
voltxd | 110:a6d1d3525014 | 68 | getVerifiedPWMValues(&pulsewidth_propulsion, &pulsewidth_direction); |
voltxd | 109:4ee7ffc8f175 | 69 | propulsion.pulsewidth_us(pulsewidth_propulsion); |
voltxd | 109:4ee7ffc8f175 | 70 | direction.pulsewidth_us(pulsewidth_direction); |
voltxd | 111:f11575e7c79b | 71 | } |
voltxd | 112:478ae92cb106 | 72 | |
voltxd | 113:2f8f255e99f2 | 73 | //If no speed captor interrupt, consider speed = 0 |
voltxd | 113:2f8f255e99f2 | 74 | if (tSpeed.read() > 0.25) |
voltxd | 113:2f8f255e99f2 | 75 | { |
voltxd | 113:2f8f255e99f2 | 76 | currentSpeed = 0; |
voltxd | 113:2f8f255e99f2 | 77 | tSpeed.reset(); |
voltxd | 113:2f8f255e99f2 | 78 | } |
voltxd | 113:2f8f255e99f2 | 79 | |
voltxd | 113:2f8f255e99f2 | 80 | //Write in terminal |
voltxd | 114:c1f7be27aa5d | 81 | if (tTerminalWriting.read() > 0.02) |
voltxd | 113:2f8f255e99f2 | 82 | { |
voltxd | 114:c1f7be27aa5d | 83 | sprintf(terminalOutput, "Vcons = %d m/s, Vmes = %d mm/s, PWM = %d\n\r", (int)commandSpeed, (int)(currentSpeed*1000), pulsewidth_propulsion); |
voltxd | 113:2f8f255e99f2 | 84 | serial_port.write(terminalOutput, strlen(terminalOutput)); |
voltxd | 113:2f8f255e99f2 | 85 | tTerminalWriting.reset(); |
voltxd | 113:2f8f255e99f2 | 86 | } |
Jonathan Austin |
0:2757d7abb7d9 | 87 | } |
Jonathan Austin |
0:2757d7abb7d9 | 88 | } |
voltxd | 112:478ae92cb106 | 89 | |
voltxd | 112:478ae92cb106 | 90 | void onRxInterrupt() |
voltxd | 112:478ae92cb106 | 91 | { |
voltxd | 112:478ae92cb106 | 92 | char c; |
voltxd | 112:478ae92cb106 | 93 | |
voltxd | 112:478ae92cb106 | 94 | |
voltxd | 112:478ae92cb106 | 95 | // Read the data to clear the receive interrupt. |
voltxd | 113:2f8f255e99f2 | 96 | if (serial_port.read(&c, 1)) |
voltxd | 113:2f8f255e99f2 | 97 | { |
voltxd | 112:478ae92cb106 | 98 | decodeMessage(c); |
voltxd | 113:2f8f255e99f2 | 99 | //serial_port.write(&c, 1); |
voltxd | 112:478ae92cb106 | 100 | } |
voltxd | 112:478ae92cb106 | 101 | |
voltxd | 113:2f8f255e99f2 | 102 | } |
voltxd | 113:2f8f255e99f2 | 103 | |
voltxd | 113:2f8f255e99f2 | 104 | void onSpeedCaptorInterrupt() |
voltxd | 113:2f8f255e99f2 | 105 | { |
voltxd | 113:2f8f255e99f2 | 106 | //Measure car speed |
voltxd | 113:2f8f255e99f2 | 107 | double currentSpeedPeriod = tSpeed.read(); |
voltxd | 113:2f8f255e99f2 | 108 | currentSpeed = DISTANCE_FOR_HALF_TURN / currentSpeedPeriod; |
voltxd | 113:2f8f255e99f2 | 109 | tSpeed.reset(); |
voltxd | 113:2f8f255e99f2 | 110 | } |
voltxd | 113:2f8f255e99f2 | 111 | |
voltxd | 113:2f8f255e99f2 | 112 | void onTickerAsservissementInterrupt() |
voltxd | 113:2f8f255e99f2 | 113 | { |
voltxd | 113:2f8f255e99f2 | 114 | //Command car's speed |
voltxd | 113:2f8f255e99f2 | 115 | pulsewidth_propulsion = PID(commandSpeed - currentSpeed, CONTROL_RATE); |
voltxd | 113:2f8f255e99f2 | 116 | propulsion.pulsewidth_us(pulsewidth_propulsion); |
voltxd | 114:c1f7be27aa5d | 117 | |
voltxd | 114:c1f7be27aa5d | 118 | //Open loop (for coef) |
voltxd | 114:c1f7be27aa5d | 119 | |
voltxd | 112:478ae92cb106 | 120 | } |