File content as of revision 113:2f8f255e99f2:

/* 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"

//Define PWM
#define PWM_PROP    D9
#define PWM_DIR     D10

//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);

//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.1)
        {
            sprintf(terminalOutput, "Vcons = %d, Vmes = %d, PWM = %d\r", (int)commandSpeed, (int)currentSpeed, 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);
}