File content as of revision 8:32133eeb7037:

#include "mbed.h"
#include "millis.h"
#include "PwmIn.h"


// I/O Declaration_________________________________________________

DigitalOut myled(LED_GREEN);
DigitalOut red(LED_RED);
Serial pc(USBTX, USBRX);
//Serial pc(PTE22, PTE23);

//OUTPUTS

PwmOut leftESC(PTA4);

PwmOut rightESC(PTA5);

PwmOut Servo(PTA12);

DigitalOut LED(PTC1);


//INPUTS

PwmIn steeringAngle(PTD0);

PwmIn motorSpeed(PTD5);


InterruptIn eBrake(PTD2);

InterruptIn RPM_one(PTD4);

InterruptIn RPM_two(PTA13);

AnalogIn   poti(PTB3);

AnalogIn   poti2(PTB1);

DigitalIn  fast(PTD1);



//Declarations for RPM measurement___________________
Timer RPM_timer_one;
long RPM_spent_time_one = 0;
long RPM_val_one = 0;

Timer RPM_timer_two;
long RPM_spent_time_two = 0;
long RPM_val_two = 0;

const int numReadings_one = 8;
int RPMs_one[numReadings_one];
int readIndex_one = 0;
int total_one = 0;
int averageRPM_one = 0;
long speed_one = 0;

const int numReadings_two = 8;
int RPMs_two[numReadings_two];
int readIndex_two = 0;
int total_two = 0;
int averageRPM_two = 0;
long speed_two = 0;
//__________________________________________________________

//dings
float prev_val = 0;


//INTERRUPT SERVICE ROUNTINES_______________________________
void RPM_one_ISR()
{
    RPM_one.rise(NULL);
    RPM_spent_time_one = RPM_timer_one.read_high_resolution_us();
    RPM_timer_one.reset();
    RPM_one.rise(&RPM_one_ISR);
}

void RPM_two_ISR()
{
    RPM_two.rise(NULL);
    RPM_spent_time_two = RPM_timer_two.read_high_resolution_us();
    RPM_timer_two.reset();
    RPM_two.rise(&RPM_two_ISR);
}

bool brake = false;
void eBrake_ISR()
{
    myled = !myled;
    brake = true;
}
void eBrake2_ISR()
{
    myled = !myled;
    brake = false;
}

//___________________________________________________________

//MAPPING FUNCTION
float map(float x, float in_min, float in_max, float out_min, float out_max)
{
    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

void setRightESC(float val)
{
    if (val != prev_val) {
        rightESC.write(val);
        prev_val = val;
    }
}

volatile bool newData = false;
volatile float inputs[3];




Timer speedTimer;

//PID-Stuff-------------------------------------------------------------
float PID_error, PID_error_pre, PID_out, PID_sum, PID_val;
float leftRPM, rightRPM;
float set_speed = 300;    //desired RPM value
float Kp = 1;
float Ki = 0;
float Kd = 0;
//----------------------------------------------------------------------

void calcPID()
{
    PID_error_pre = PID_error;
    PID_error = set_speed - leftRPM;

    PID_val = Kp * PID_error + Ki * PID_sum + Kd * (PID_error - PID_error_pre);
    PID_out += PID_val;

    PID_sum += PID_error;

    if (PID_sum > 500) {
        PID_sum = 500;
    } else if (PID_sum < -500) {
        PID_sum = -500;
    }

    if (PID_out > 500) {
        PID_out = 500;
    } else if (PID_out < -500) {
        PID_out = -500;
    }
}


int main()
{
    pc.printf("Ready!\n");

    pc.baud(115200);


    //Settings for Interrrupts for RPM Sensors
    RPM_one.rise(&RPM_one_ISR);
    RPM_timer_one.start();
    RPM_two.rise(&RPM_two_ISR);
    RPM_timer_two.start();

    //Setting for EBrake Interrrupt
    eBrake.rise(&eBrake_ISR);
    eBrake.fall(&eBrake2_ISR);

    //Values for Servo Passthrough
    float dutycycle = 0;
    float pulsewidth = 0;
    float period = 0;

    //Servo Settings
    period = steeringAngle.period();
    Servo.period_ms(20);
    //pc.printf("%f",period

    //ESC Settings
    leftESC.period_us(10000);
    rightESC.period_us(10000);

    //ARMING ESCS
    leftESC.pulsewidth_us(1500);  //write them to idle position
    rightESC.pulsewidth_us(1500);
    wait(0.9f);   //wait 2 Seconds
    wait(0.9f);
    wait(0.2f);
    wait(0.9f);   //wait 2 Seconds
    wait(0.9f);
    wait(0.2f);




    speedTimer.start();
    Timer ppp;
    float poti_val = 0;
    
    bool locked = false;


    //MAIN LOOP_________________________________________________________________
    while (true) {


        //MEASURE RPM___________________________________________________________
        total_one = total_one - RPMs_one[readIndex_one];
        total_two = total_two - RPMs_two[readIndex_two];

        if (RPM_timer_one.read_high_resolution_us() > 500000) {
            RPM_val_one = 0;
        } else if (RPM_spent_time_one > 0) {
            RPM_val_one = 60000000/(RPM_spent_time_one * 8);
        } else {
            RPM_val_one = 0;
        }

        if (RPM_timer_two.read_high_resolution_us() > 500000) {
            RPM_val_two = 0;
        } else if (RPM_spent_time_two > 0) {
            RPM_val_two = 60000000/(RPM_spent_time_two * 8);
        } else {
            RPM_val_two = 0;
        }

        RPMs_one[readIndex_one] = RPM_val_one;
        total_one = total_one + RPMs_one[readIndex_one];
        readIndex_one++;

        RPMs_two[readIndex_two] = RPM_val_two;
        total_two = total_two + RPMs_two[readIndex_two];
        readIndex_two++;

        //set rpms to zero if under certain speed
        if (readIndex_one >= numReadings_one) {
            readIndex_one = 0;
        }

        if (readIndex_two >= numReadings_two) {
            readIndex_two = 0;
        }

        //average the rpms since last run of pid error
        averageRPM_one = total_one / numReadings_one;
        averageRPM_two = total_two / numReadings_two;

        //store pid output
        leftRPM = averageRPM_two;
        rightRPM = averageRPM_one;

        //calculate actual pid error
        calcPID();

        //check for drivng switch to be pressed
        if(fast && !brake) {
            //leftESC.pulsewidth_us((1500 + PID_out < 1500) ? 1500 : (1500 + PID_out > 2000) ? 2000 : 1500 + PID_out);
            //rightESC.pulsewidth_us((1500 + PID_out < 1500) ? 1500 : (1500 + PID_out > 2000) ? 2000 : 1500 + PID_out);
            leftESC.pulsewidth_us(1500 + poti_val);
            rightESC.pulsewidth_us(1500 + poti_val);
            LED = 1;
            locked = false;
        }else if(fast && brake && !locked){
            locked = true;
            leftESC.pulsewidth_us(1400);
            rightESC.pulsewidth_us(1400);
            wait(0.5f);
            leftESC.pulsewidth_us(1500);
            rightESC.pulsewidth_us(1500);
        }
        else {
            leftESC.pulsewidth_us(1500);
            rightESC.pulsewidth_us(1500);
            LED = 0;
        }

        //PASSTHROUGH FOR STEERING
        pulsewidth = steeringAngle.pulsewidth();
        Servo.pulsewidth_us(pulsewidth*1000000);


        //poti_val = map( poti.read() , 0 , 1 , 0 , 1 );
        //wait_ms(10);

        poti_val = poti.read() * 500;
        //Kp = poti_val;
        
        
        //DEBUGGING___________________________________
        /*
        pc.printf("%f ", leftRPM);
        pc.printf(" , ");
        //pc.printf("%.5f ", 1500+PID_out );
        //pc.printf(" , ");
        pc.printf("%f ,", set_speed);
        pc.printf("%f ,", poti_val);
        pc.printf("0.0 \n" );
        */
        //_____________________________________________
        
    }
    //END OF MAIN LOOP__________________________________________________________
}