.
Dependencies: L432KC_SPI_Pey_Lal
Diff: main.cpp
- Revision:
- 113:2f8f255e99f2
- Parent:
- 112:478ae92cb106
- Child:
- 114:c1f7be27aa5d
--- a/main.cpp Tue Apr 26 11:11:49 2022 +0000 +++ b/main.cpp Tue May 17 22:44:42 2022 +0000 @@ -6,39 +6,58 @@ #include "mbed.h" #include "platform/mbed_thread.h" #include "protocol.h" - -//Define SPI -#define SPI3_MOSI D11 -#define SPI3_MISO D12 -#define SPI3_SCLK D13 -#define SPI3_CS A3 +#include "asservissement.hpp" //Define PWM #define PWM_PROP D9 #define PWM_DIR D10 -//fontion interrupt SP +//Define interrupt Input +#define SPEED_CAPTOR A4 + +//Define COEF +#define DISTANCE_FOR_HALF_TURN 3 * 3.14 //TODO: mettre vrai valeur +#define CONTROL_RATE 1000.0 + +//fontion interrupt void onRxInterrupt(); +void onSpeedCaptorInterrupt(); +void onTickerAsservissementInterrupt(); //Declaration PWM outputs PwmOut propulsion(PWM_PROP); PwmOut direction(PWM_DIR); +//Declaration analog input +InterruptIn speedCaptor(SPEED_CAPTOR); + //Declaration Links static UnbufferedSerial serial_port(USBTX, USBRX,115200); -SPISlave device(SPI3_MOSI, SPI3_MISO, SPI3_SCLK, SPI3_CS); // mosi, miso, sclk, ssel + +//Timers +Timer tSpeed; +Timer tTerminalWriting; +Ticker tickAsserv; + +//Var. +double currentSpeed = 0; +double commandSpeed = 5; + +//Declaration PWM variables +uint32_t pulsewidth_direction = 1100; +uint32_t pulsewidth_propulsion = 1500; int main() -{ - //Declaration PWM variables - uint32_t pulsewidth_direction = 1100; - uint32_t pulsewidth_propulsion = 1500; - +{ //Test Serial port serial_port.write("Test\n\r",strlen("Test\n\r")); - //Interrupt SP + char terminalOutput[256]; + + //Interrupt serial_port.attach(&onRxInterrupt, SerialBase::RxIrq); + speedCaptor.fall(&onSpeedCaptorInterrupt); + tickAsserv.attach(&onTickerAsservissementInterrupt, 1.0 / CONTROL_RATE); //Init. propulsion PWM propulsion.period_us(20000); @@ -50,21 +69,13 @@ direction.period_us(20000); direction.pulsewidth_us(pulsewidth_direction); - //Init. SPI Link - device.format(8); + //Timers init. + tSpeed.start(); + tTerminalWriting.start(); //Infinite loop while(1) { - //If SPI received a char, decode it then reply bullshit. - if(device.receive()) - { - char c = device.read(); - decodeMessage(c); - serial_port.write(&c, 1); - device.reply(0b10101010); - } - //If decoding has ended, get and changes PWM values. if(isDataAvailable()) { @@ -73,6 +84,20 @@ direction.pulsewidth_us(pulsewidth_direction); } + //If no speed captor interrupt, consider speed = 0 + if (tSpeed.read() > 0.25) + { + currentSpeed = 0; + tSpeed.reset(); + } + + //Write in terminal + if (tTerminalWriting.read() > 0.1) + { + sprintf(terminalOutput, "Vcons = %d, Vmes = %d, PWM = %d\r", (int)commandSpeed, (int)currentSpeed, pulsewidth_propulsion); + serial_port.write(terminalOutput, strlen(terminalOutput)); + tTerminalWriting.reset(); + } } } @@ -82,9 +107,25 @@ // Read the data to clear the receive interrupt. - if (serial_port.read(&c, 1)) { + if (serial_port.read(&c, 1)) + { decodeMessage(c); - serial_port.write(&c, 1); + //serial_port.write(&c, 1); } +} + +void onSpeedCaptorInterrupt() +{ + //Measure car speed + double currentSpeedPeriod = tSpeed.read(); + currentSpeed = DISTANCE_FOR_HALF_TURN / currentSpeedPeriod; + tSpeed.reset(); +} + +void onTickerAsservissementInterrupt() +{ + //Command car's speed + pulsewidth_propulsion = PID(commandSpeed - currentSpeed, CONTROL_RATE); + propulsion.pulsewidth_us(pulsewidth_propulsion); } \ No newline at end of file