.
Dependencies: L432KC_SPI_Pey_Lal
main.cpp@116:6dfcafa00e42, 2022-05-23 (annotated)
- Committer:
- voltxd
- Date:
- Mon May 23 12:55:02 2022 +0000
- Revision:
- 116:6dfcafa00e42
- Parent:
- 115:156b8234f2de
- Child:
- 117:58148bdd13b7
vitesse
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 | 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 | 116:6dfcafa00e42 | 86 | //1st. get the values |
voltxd | 115:156b8234f2de | 87 | pulsewidth_propulsion = protocolReceivedPayload[0] << 8; |
voltxd | 115:156b8234f2de | 88 | pulsewidth_propulsion += protocolReceivedPayload[1]; |
voltxd | 115:156b8234f2de | 89 | pulsewidth_direction = protocolReceivedPayload[2] << 8; |
voltxd | 115:156b8234f2de | 90 | pulsewidth_direction += protocolReceivedPayload[3]; |
voltxd | 116:6dfcafa00e42 | 91 | //2nd. allocate the values |
voltxd | 115:156b8234f2de | 92 | propulsion.pulsewidth_us(pulsewidth_propulsion); |
voltxd | 115:156b8234f2de | 93 | direction.pulsewidth_us(pulsewidth_direction); |
voltxd | 115:156b8234f2de | 94 | break; |
voltxd | 115:156b8234f2de | 95 | |
voltxd | 115:156b8234f2de | 96 | case COMMAND_ASSERVISSEMENT: |
voltxd | 116:6dfcafa00e42 | 97 | //1st. |
voltxd | 115:156b8234f2de | 98 | commandSpeed = convertBytesToFloat(protocolReceivedPayload, 0); |
voltxd | 115:156b8234f2de | 99 | pulsewidth_direction = protocolReceivedPayload[4] << 8; |
voltxd | 115:156b8234f2de | 100 | pulsewidth_direction += protocolReceivedPayload[5]; |
voltxd | 116:6dfcafa00e42 | 101 | //2nd. |
voltxd | 115:156b8234f2de | 102 | direction.pulsewidth_us(pulsewidth_direction); |
voltxd | 115:156b8234f2de | 103 | break; |
voltxd | 115:156b8234f2de | 104 | |
voltxd | 115:156b8234f2de | 105 | case COMMAND_PARAMETRES: |
voltxd | 115:156b8234f2de | 106 | { |
voltxd | 116:6dfcafa00e42 | 107 | //1st. |
voltxd | 115:156b8234f2de | 108 | float kp = convertBytesToFloat(protocolReceivedPayload, 0); |
voltxd | 115:156b8234f2de | 109 | float ki = convertBytesToFloat(protocolReceivedPayload, 4); |
voltxd | 116:6dfcafa00e42 | 110 | //2nd. |
voltxd | 115:156b8234f2de | 111 | setPIDParameters(kp, ki); |
voltxd | 115:156b8234f2de | 112 | break; |
voltxd | 115:156b8234f2de | 113 | } |
voltxd | 115:156b8234f2de | 114 | } |
voltxd | 111:f11575e7c79b | 115 | } |
voltxd | 112:478ae92cb106 | 116 | |
voltxd | 113:2f8f255e99f2 | 117 | //If no speed captor interrupt, consider speed = 0 |
voltxd | 116:6dfcafa00e42 | 118 | if (tSpeed.read() > DISTANCE_FOR_HALF_TURN / minimalSpeed) |
voltxd | 113:2f8f255e99f2 | 119 | { |
voltxd | 113:2f8f255e99f2 | 120 | currentSpeed = 0; |
voltxd | 113:2f8f255e99f2 | 121 | tSpeed.reset(); |
voltxd | 113:2f8f255e99f2 | 122 | } |
voltxd | 113:2f8f255e99f2 | 123 | |
voltxd | 116:6dfcafa00e42 | 124 | //Write in terminal (will need to get out) |
voltxd | 116:6dfcafa00e42 | 125 | if (tTerminalWriting.read() > 1.0 / terminalOutputFrequency) |
voltxd | 113:2f8f255e99f2 | 126 | { |
voltxd | 114:c1f7be27aa5d | 127 | sprintf(terminalOutput, "Vcons = %d m/s, Vmes = %d mm/s, PWM = %d\n\r", (int)commandSpeed, (int)(currentSpeed*1000), pulsewidth_propulsion); |
voltxd | 113:2f8f255e99f2 | 128 | serial_port.write(terminalOutput, strlen(terminalOutput)); |
voltxd | 113:2f8f255e99f2 | 129 | tTerminalWriting.reset(); |
voltxd | 113:2f8f255e99f2 | 130 | } |
voltxd | 116:6dfcafa00e42 | 131 | |
voltxd | 116:6dfcafa00e42 | 132 | //Transmit speed to rPi/Jetson |
voltxd | 116:6dfcafa00e42 | 133 | if(tSpeedTx.read() > 1.0 / speedTransmitionFrequency) |
voltxd | 116:6dfcafa00e42 | 134 | { |
voltxd | 116:6dfcafa00e42 | 135 | encodeMessage(msgSpeedProtocolOutput, currentSpeed); |
voltxd | 116:6dfcafa00e42 | 136 | serial_port.write(msgSpeedProtocolOutput, speedMessageLength); |
voltxd | 116:6dfcafa00e42 | 137 | } |
Jonathan Austin |
0:2757d7abb7d9 | 138 | } |
Jonathan Austin |
0:2757d7abb7d9 | 139 | } |
voltxd | 112:478ae92cb106 | 140 | |
voltxd | 112:478ae92cb106 | 141 | void onRxInterrupt() |
voltxd | 112:478ae92cb106 | 142 | { |
voltxd | 112:478ae92cb106 | 143 | char c; |
voltxd | 116:6dfcafa00e42 | 144 | |
voltxd | 112:478ae92cb106 | 145 | // Read the data to clear the receive interrupt. |
voltxd | 113:2f8f255e99f2 | 146 | if (serial_port.read(&c, 1)) |
voltxd | 112:478ae92cb106 | 147 | decodeMessage(c); |
voltxd | 113:2f8f255e99f2 | 148 | } |
voltxd | 113:2f8f255e99f2 | 149 | |
voltxd | 113:2f8f255e99f2 | 150 | void onSpeedCaptorInterrupt() |
voltxd | 113:2f8f255e99f2 | 151 | { |
voltxd | 113:2f8f255e99f2 | 152 | //Measure car speed |
voltxd | 113:2f8f255e99f2 | 153 | double currentSpeedPeriod = tSpeed.read(); |
voltxd | 113:2f8f255e99f2 | 154 | currentSpeed = DISTANCE_FOR_HALF_TURN / currentSpeedPeriod; |
voltxd | 113:2f8f255e99f2 | 155 | tSpeed.reset(); |
voltxd | 113:2f8f255e99f2 | 156 | } |
voltxd | 113:2f8f255e99f2 | 157 | |
voltxd | 113:2f8f255e99f2 | 158 | void onTickerAsservissementInterrupt() |
voltxd | 113:2f8f255e99f2 | 159 | { |
voltxd | 113:2f8f255e99f2 | 160 | //Command car's speed |
voltxd | 113:2f8f255e99f2 | 161 | pulsewidth_propulsion = PID(commandSpeed - currentSpeed, CONTROL_RATE); |
voltxd | 113:2f8f255e99f2 | 162 | propulsion.pulsewidth_us(pulsewidth_propulsion); |
voltxd | 114:c1f7be27aa5d | 163 | |
voltxd | 114:c1f7be27aa5d | 164 | //Open loop (for coef) |
voltxd | 112:478ae92cb106 | 165 | } |