File content as of revision 0:1825e249cba5:

#include "PID.h"
#include "Motor.h"
#include <Map.hpp>
#include "mbed.h"
#define RATE 0.1
 
AnalogIn analog_value1(PA_0);
AnalogIn analog_value2(PA_1);
AnalogIn analog_value3(PA_3);
AnalogIn analog_value4(PA_4);
Motor myMotor(PA_8, PA_5, PA_6);
int counter = 0;
int counter1 = 0;
//static float setpoint, feedback, output;
const float output_lower_limit = -255;          
const float output_upper_limit = 255;
const float kp = 1.5;
const float ki = 1;
const float kd = 0.001;
const float Ts = 0.001;
float pedal1, pedal2, matzeret1, matzeret2, sumpedal, summetzeret, subpedal, submetzeret;
float mdagree = 0, pdagree = 0;
float mypedal,sumPedal,mythorttle,sumthorttle = 0;
float speed = 0;
Map mapvaltovolt = Map(0, 1, 0, 3300);
Map pedaltodagree = Map(865, 1220, 0, 255);
Map mtodagree = Map(490, 3150, 0, 255);
Map nspeed = Map(-255, 255, -1, 1);
PID pid(&pdagree, &mdagree, &speed, output_lower_limit, output_upper_limit,kp, ki, kd, Ts);
Ticker sensor_sample_timer;
Ticker motor_cmd_timer;

//DigitalOut led(LED1);
Serial pc(USBTX,USBRX);
void commandMotor(){
    counter1++;
    if (counter1 == 20){
        pc.printf("Error %.4f\n\r", pid.getError());
        counter1 = 0;
        }
    float Motorcommand = nspeed.Calculate(speed);
    //pc.printf("Motor speed %.4f\n\r", Motorcommand);
    if (Motorcommand > 1) {
        myMotor.speed(1);
    }
    else {
        myMotor.speed(Motorcommand);
    } 
}
void readSensors(){
    
    pedal1 = analog_value1.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
    pedal2 = analog_value2.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
    matzeret1 = analog_value3.read();
    pedal1 = mapvaltovolt.Calculate(pedal1);
    pedal2 = mapvaltovolt.Calculate(pedal2);
    matzeret1 = mapvaltovolt.Calculate(matzeret1);
    /*matzeret2 = analog_value4.read();
    matzeret2 = matzeret2 * ConvertToVolt; */
    //pc.printf("pedal1 is: %.4f, matzeret1 is:%.4f\n\r", pedal1, matzeret1);
    sumpedal = pedal1+pedal2;
    //pc.printf("sumpedal is: %.4f\n\r", sumpedal);
    subpedal = abs(3500-sumpedal);
    //pc.printf("Subpedal is: %.4f\n\r", subpedal);
    if (subpedal<175) {
        mypedal = pedaltodagree.Calculate(pedal1);
        sumPedal = sumPedal + mypedal;
        //setpoint = throttleCmd;
        //pc.printf("setpedal dagree %.4f\n\r", pdagree);
        mythorttle = mtodagree.Calculate(matzeret1);
        sumthorttle = sumthorttle + mythorttle;
        //feedback = throttlePosition;
        //pc.printf("processvalue dagree %.4f\n\r", mdagree);
        counter++;
        if (counter == 10) {
            pdagree = sumPedal/10;
            mdagree = sumthorttle/10;
            counter = 0;
            sumPedal = 0;
            sumthorttle = 0;
        }  
        
    }
}
int main() {
    sensor_sample_timer.attach(readSensors, 0.0001);
    pid.start();
    motor_cmd_timer.attach(commandMotor, 0.0005);
}