File content as of revision 1:f0e37d1d1452:

////////////////////////////////////////
//      Controlling Thorttle          //
//                                    //
////////////////////////////////////////
/* 
    Pingout:
    Nucleo-L432KC
    PA_0 ---> Analogin ---> Read Potentiometer 1
    PA_1 ---> Analogin ---> Read Potentiometer 2
    PA_3 ----> DigitalOut ---> to Led MOSFET
    LED1 ----> DigitalOut ----> Error Indication
    CANBUS:
    PA_11 ---> CANBUS TX
    PA_12 ----> CANBUS RX
    Motor:
    PA_8 (D9) --- PWMOUT
    PA_5 ---> Digitalout ---> Motor FWD direction
    PA_6 ---> Digitalout ---> Motor RWD direction
*/

///////////////
// Libraries //
///////////////
    
#include "PID.h"
#include "Motor.h"
#include <Map.hpp>
#include "mbed.h"
///////////////
// #defines  //
///////////////

/////////////
// Objects //
/////////////

//thorttle potentiometer

AnalogIn analog_value1(PA_0);
AnalogIn analog_value2(PA_1);

//led indicator

DigitalOut myled(LED1);

// brake signalout

DigitalOut mybrake(PA_3);

// can bus

CAN can1(PA_11, PA_12);

//Motor Control

Motor myMotor(PA_8, PA_5, PA_6);

//Brake signal 

//Serial

Serial pc(USBTX,USBRX);

//Timer

Timer t;
Timer can;

///////////////
// variables //
///////////////
int startup = 0;
int counter = 0;
int errorcounter = 0;

// PID

// pid output limits

const float output_lower_limit = -255;          
const float output_upper_limit = 255;

//pid constants

const float kp = 1.2;
const float ki = 0.5;
const float kd = 3;

//Time sample

const float Ts = 0.001;

//Thorttle and pedal intital values
float  thorttle1, thorttle2, sumThorttle, subThorttle; 
float mdagree, pdagree = 0;
float mythorttle,sumthorttle = 0;

//Motor speed pwm -1<speed<1

float speed = 0;

// Define PID object //

PID pid(&pdagree, &mdagree, &speed, output_lower_limit, output_upper_limit,kp, ki, kd, Ts);

//////////////////
//define Mapping//
//////////////////

Map mapvaltovolt = Map(0, 1, 0, 3300); // Maping Volt value from 0-1 to 0-3300
Map mtodagree = Map(490, 3150, 0, 255); // Maping Volt value from 490-3150 to 0 - 255
Map nspeed = Map(-255, 255, -1, 1); // Maping out put speed from -255-255 to -1-1


////////////////////
//define functions///
////////////////////

// After pid calculate and Sensor data recieve this function command the output to the motor

void BrakeOn() {
    mybrake = 1;
    }
void BrakeOff() {
    mybrake = 0;
    }
void commandMotor(){ 
    float Motorcommand = nspeed.Calculate(speed);
    myMotor.speed(Motorcommand);
}  

// if there is Error this function Turn the Motor speed to 0 // Close the Thorttle and then The engine is off
void sendError() { 
    myMotor.speed(0);
    //pid.stop();
    while (1) {
        BrakeOn();
        //pc.printf("Error is: %.4f\n\r", 1);
        wait(1);
        BrakeOff();
        wait(1);
        }
    }
    
// get Data overcanbus    
void getData() {
    //pc.printf("%d", 1); 
    CANMessage msg;
        if(can1.read(msg)) {
            if (can.read() > 0) {
                can.stop();
                can.reset();
                }
            //pc.printf("%d", msg.data[0]);
            if (msg.data[2]==1){
                sendError();
                }
            if (msg.data[3]==1){
                BrakeOn();
                }
            if (msg.data[3]==0){
                BrakeOff();
                }
            if(msg.data[0]<30){
                pdagree = 30;
                }
            if(msg.data[0]>30){
                pdagree = msg.data[0]*1.5;
                }
            //pc.printf("Subthorttle is: %.4f\n\r", pdagree);
            //pc.printf("Error is: %.4f\n\r", msg.data[2]);
            }
        if (!can1.read(msg)) {
            can.start();
            }
        if (can.read() > 0.1) { // if message not recieved more then 0.1 sec send error
            can.stop();
            can.reset();
            sendError();
            }
        }
                
//reading Thorttle sensors

void readSensors() { 
    thorttle1 = analog_value1.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
    thorttle2 = analog_value2.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
    thorttle1 = mapvaltovolt.Calculate(thorttle1);
    thorttle2 = mapvaltovolt.Calculate(thorttle2);
    sumThorttle = thorttle1+thorttle2;
    subThorttle = abs(3500-sumThorttle);
    //pc.printf("matzeret1 is: %.4f, matzeret2 is:%.4f\n\r", thorttle1, thorttle2);
    //pc.printf("Subthorttle is: %.4f\n\r", subThorttle);
    if (subThorttle<350) { //checking error
        mythorttle = mtodagree.Calculate(thorttle1);
        sumthorttle = sumthorttle + mythorttle;
        counter++;
        if (t.read()>0) {
            errorcounter = 0;
            t.stop();
            t.reset();
            }
        if (counter == 10) {
            mdagree = sumthorttle/10;
            getData();
            counter = 0;
            sumthorttle = 0;
            }
        }
    if (subThorttle>350) {
            if (errorcounter == 0) {
                t.start();
                errorcounter = 1;
                }
            if (t.read() > 0.1) {
                t.stop();
                t.reset();
                sendError();
                }
            }
}
int main() {
    pid.start();
    while(1){
        readSensors();
        pid.sample();
        commandMotor();
        //wait(Ts);
        }
}