.
Dependencies: L432KC_SPI_Pey_Lal
main.cpp
- Committer:
- voltxd
- Date:
- 2022-05-18
- Revision:
- 114:c1f7be27aa5d
- Parent:
- 113:2f8f255e99f2
- Child:
- 115:156b8234f2de
File content as of revision 114:c1f7be27aa5d:
/* mbed Microcontroller Library * Copyright (c) 2019 ARM Limited * SPDX-License-Identifier: Apache-2.0 */ #include "mbed.h" #include "platform/mbed_thread.h" #include "protocol.h" #include "asservissement.hpp" #include "toolbox.hpp" #include "main.hpp" //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); //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() { //Test Serial port serial_port.write("Test\n\r",strlen("Test\n\r")); 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); propulsion.pulsewidth_us(pulsewidth_propulsion); thread_sleep_for(2000); //Init. Direction PWM direction.period_us(20000); direction.pulsewidth_us(pulsewidth_direction); //Timers init. tSpeed.start(); tTerminalWriting.start(); //Infinite loop while(1) { //If decoding has ended, get and changes PWM values. if(isDataAvailable()) { getVerifiedPWMValues(&pulsewidth_propulsion, &pulsewidth_direction); propulsion.pulsewidth_us(pulsewidth_propulsion); 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.02) { sprintf(terminalOutput, "Vcons = %d m/s, Vmes = %d mm/s, PWM = %d\n\r", (int)commandSpeed, (int)(currentSpeed*1000), pulsewidth_propulsion); serial_port.write(terminalOutput, strlen(terminalOutput)); tTerminalWriting.reset(); } } } void onRxInterrupt() { char c; // Read the data to clear the receive interrupt. if (serial_port.read(&c, 1)) { decodeMessage(c); //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); //Open loop (for coef) }