File content as of revision 117:58148bdd13b7:

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

//Constants
const char speedMessageLength = 6;
const float minimalSpeed = 0.05;
const int terminalOutputFrequency = 50;
const int speedTransmitionFrequency = 250;

//Flag
bool commandEnabled = true;

//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;
Timer tSpeedTx;
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];
    char msgSpeedProtocolOutput[speedMessageLength] = {0};
    
    //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();
    tSpeedTx.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:
                    //Turn off PI
                    commandEnabled = false;
                    //1st. get the values
                    pulsewidth_propulsion = protocolReceivedPayload[0] << 8;
                    pulsewidth_propulsion += protocolReceivedPayload[1];
                    pulsewidth_direction = protocolReceivedPayload[2] << 8;
                    pulsewidth_direction += protocolReceivedPayload[3];
                    //2nd. allocate the values
                    propulsion.pulsewidth_us(pulsewidth_propulsion);
                    direction.pulsewidth_us(pulsewidth_direction);
                    break;
                    
                case COMMAND_ASSERVISSEMENT: 
                    //Turn on PI
                    commandEnabled = true;
                    //1st. 
                    commandSpeed = convertBytesToFloat(protocolReceivedPayload, 0);
                    pulsewidth_direction = protocolReceivedPayload[4] << 8;
                    pulsewidth_direction += protocolReceivedPayload[5];
                    //2nd.
                    direction.pulsewidth_us(pulsewidth_direction);
                    break;
                    
                case COMMAND_PARAMETRES:
                {                
                    //Turn on PI       
                    commandEnabled = true;
                    //1st.
                    float kp = convertBytesToFloat(protocolReceivedPayload, 0);
                    float ki = convertBytesToFloat(protocolReceivedPayload, 4);                                                
                    //2nd.
                    setPIDParameters(kp, ki);
                    break;   
                }
            }
        }        
        
        //If no speed captor interrupt, consider speed = 0
        if (tSpeed.read() > DISTANCE_FOR_HALF_TURN / minimalSpeed)
        {
            currentSpeed = 0;
            tSpeed.reset();
        }
        
        //Write in terminal (will need to get out)
        if (tTerminalWriting.read() > 1.0 / terminalOutputFrequency)
        {
            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();
        }
        
        //Transmit speed to rPi/Jetson
        if(tSpeedTx.read() > 1.0 / speedTransmitionFrequency)
        {
            encodeMessage(msgSpeedProtocolOutput, currentSpeed);
            serial_port.write(msgSpeedProtocolOutput, speedMessageLength);
        }
    }
}

void onRxInterrupt()
{
    char c;
    
    // Read the data to clear the receive interrupt.
    if (serial_port.read(&c, 1)) 
        decodeMessage(c);
}

void onSpeedCaptorInterrupt()
{
    //Measure car speed
    double currentSpeedPeriod = tSpeed.read();
    currentSpeed = DISTANCE_FOR_HALF_TURN / currentSpeedPeriod;
    tSpeed.reset();
}

void onTickerAsservissementInterrupt()
{
    //Command car's speed
    if (commandEnabled)
        pulsewidth_propulsion = PID(commandSpeed - currentSpeed, CONTROL_RATE);
    propulsion.pulsewidth_us(pulsewidth_propulsion);
    
    //Open loop (for transformation parameters)
    //pulsewidth_propulsion = PID(commandSpeed, CONTROL_RATE);
    //propulsion.pulsewidth_us(pulsewidth_propulsion);
}