File content as of revision 7:34118a0098e8:

////////////////////////////////////////
//      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);
int pid_event = 0;
//led indicator

DigitalOut myled(LED1);

// brake signalout

DigitalOut mybrake(PA_3);

// can bus

CAN can1(PA_11, PA_12);

//Xbee

BufferedSerial xbee(PA_9, PA_10);

//Motor Control

Motor myMotor(PA_8, PA_5, PA_6);

//Brake signal 

//Serial

Serial pc(USBTX,USBRX);

//Timer

Timer t;
Timer can;
Timer tpid;

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

// PID

// pid output limits

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

//pid constants

const float kp = 1.5;
const float ki = 3;
const float kd = 0;

//Time sample

const float Ts = 0.001;

//Tickers
Ticker main_ticker;
Ticker motor;
Ticker syserror;

//Flags for indication if something happend - error handling
int pidflag = 0;
int canflag = 0;
int cantimeoutflag = 0; // if can message is not recived more then 100ms
int rsflag = 0;
int terrorflag = 0;
int perrorflag = 0;

//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(230, 3070, 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() { 
    if (errorcounter == 1){
        main_ticker.detach();
        pid.stop();
        syserror.attach(sendError, 0.0005);
        }
    myMotor.speed(0);
}
    /*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)) {
            recievecounter ++;
            //pc.printf("Pedal sensor worng values\n");
            //pc.printf("my data: %d", msg.data[2]);
            if (can.read() > 0) {
                can.stop();
                can.reset();
                }
            if (msg.data[2]==1){
                errorcounter = 1;
                perrorflag = 1;
                sendError();
                }
            if (msg.data[2]==0){
                perrorflag = 0;
                }
            if (msg.data[3]==1){
                BrakeOn();
                }
            if (msg.data[3]==0){
                BrakeOff();
                }
            pdagree = msg.data[0];
            }
        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();
            errorcounter = 1;
            cantimeoutflag = 1;
            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(3300-sumThorttle);
    getData();
    pid.sample();
    commandMotor();
    //pc.printf("matzeret1 is: %.4f, matzeret2 is:%.4f\n\r", thorttle1, thorttle2);
    //pc.printf("Subthorttle is: %.4f\n\r", subThorttle);
     //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;
        counter = 0;
        sumthorttle = 0;
        }
        
    /*if (subThorttle>330) {
            if (errorcounter == 0) {
                t.start();
                errorcounter = 1;
                }
            if (t.read() > 0.1) {
                t.stop();
                t.reset();
                terrorflag = 1;
                errorcounter = 1;
                sendError();
                }
            }*/
}
int main() {
    pc.baud(57600);
    xbee.baud(57600);
    pid.start();
    main_ticker.attach(readSensors, 0.0005);
    while(1){
        //pc.printf("Pedal: %.4f\n\n\r, Thorttle: %.4f\n\n\r, subthorttle: %.4f\n\n\r, recievecounter: %d\n\n\r, ThorttleErrorFlag: %d\n\n\r, PedalErrorFlag: %d\n\n\r, CanTimeoutFlag: %d\n\n\r", pdagree, mdagree, subThorttle, recievecounter, terrorflag, perrorflag, cantimeoutflag);
        xbee.printf("Pedal: %.4f\n\n\r, Thorttle:\n\r, recievecounter: %d\n\n\r, ThorttleErrorFlag: %d\n\n\r,, pdagree, mdagree, subThorttle, recievecounter, terrorflag, perrorflag, cantimeoutflag);
        }
}