File content as of revision 115:156b8234f2de:

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

char protocolReceivedPayload[PAYLOAD_MAX_SIZE];

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())
        {
            char receivedCommand = getVerifiedPayload(protocolReceivedPayload);
            switch(receivedCommand)
            {
                case COMMAND_PWM:
                    pulsewidth_propulsion = protocolReceivedPayload[0] << 8;
                    pulsewidth_propulsion += protocolReceivedPayload[1];
                    pulsewidth_direction = protocolReceivedPayload[2] << 8;
                    pulsewidth_direction += protocolReceivedPayload[3];
                    propulsion.pulsewidth_us(pulsewidth_propulsion);
                    direction.pulsewidth_us(pulsewidth_direction);
                    break;
                    
                case COMMAND_ASSERVISSEMENT: 
                    commandSpeed = convertBytesToFloat(protocolReceivedPayload, 0);
                    pulsewidth_direction = protocolReceivedPayload[4] << 8;
                    pulsewidth_direction += protocolReceivedPayload[5];
                    direction.pulsewidth_us(pulsewidth_direction);
                    break;
                    
                case COMMAND_PARAMETRES:
                {                       
                    float kp = convertBytesToFloat(protocolReceivedPayload, 0);
                    float ki = convertBytesToFloat(protocolReceivedPayload, 4);                                                
                    setPIDParameters(kp, ki);
                    break;   
                }
            }
        }        
        
        //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)
}