File content as of revision 10:53d54dceea50:

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


// I/O Declaration_________________________________________________

//LEDs
RASCarLED leftLed(PTC7, PTC0, PTC3);
RASCarLED rightLed(PTC4, PTC5, PTC6);

DigitalOut green(LED_GREEN);
DigitalOut red(LED_RED);
DigitalOut blue(LED_BLUE);

//Debugging out comment out for either USB or Bluetooth
//Serial pc(USBTX, USBRX);
Serial pc(PTE22, PTE23);

//OUTPUTS

Beep buzzer(PTA2);

PwmOut leftESC(PTA4);

PwmOut rightESC(PTA5);

PwmOut Servo(PTA12);

DigitalOut LED(PTC1);

PwmOut SpeedReturnChannel(PTD4);



//INPUTS____________________

//von K66F
PwmIn steeringAngle(PTD0);


PwmIn motorSpeed(PTD5);

//von Sensoren und Inputs

//bremssignal von k66f
InterruptIn eBrake(PTD2);

//RPM Sensor
InterruptIn RPM_one(PTD7);

InterruptIn RPM_two(PTD6);

//externe Potentiometer
AnalogIn   poti(PTC2);

AnalogIn   poti2(PTB1);

//schalter um fahren freizugeben
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;
bool locked = false;

int SLAM1 = 0;
int SLAM2 = 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);
    SLAM1++;
}

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);
    SLAM2++;
}

bool brake = false;

void eBrake_ISR()
{
    //myled = !myled;
    brake = true;
}
void eBrake2_ISR()
{
    //myled = !myled;
    brake = false;
    locked = 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 set_speed = 1150;    //desired RPM value
float Kp = 0;
float Ki = 0;
float Kd = 0;

float leftRPM, rightRPM;

float PID_errorL, PID_error_preL, PID_outL, PID_sumL, PID_valL;

float PID_errorR, PID_error_preR, PID_outR, PID_sumR, PID_valR;





//----------------------------------------------------------------------

void calcPIDleft()
{
    //save error from last reading
    PID_error_preL = PID_errorL;
    
    //calculate actual error
    PID_errorL = set_speed - leftRPM;
    
    //basic pid formula
    PID_valL = Kp * PID_errorL + Ki * PID_sumL + Kd * (PID_errorL - PID_error_preL);
    
    //store output
    PID_outL = PID_valL;

    //sum up the integral
    PID_sumL += PID_errorL;

    //limit the integral to above and below 500
    if (PID_sumL > 500) {
        PID_sumL = 500;
    } else if (PID_sumL < -500) {
        PID_sumL = -500;
    }


}

void calcPIDright()
{
    //save error from last reading
    PID_error_preR = PID_errorR;
    
    //calculate actual error
    PID_errorR = set_speed - rightRPM;
    
    //basic pid formula
    PID_valR = Kp * PID_errorR + Ki * PID_sumR + Kd * (PID_errorR - PID_error_preR);
    
    //store output
    PID_outR = PID_valR;

    //sum up the integral
    PID_sumR += PID_errorR;

    //limit the integral to above and below 500
    if (PID_sumR > 500) {
        PID_sumR = 500;
    } else if (PID_sumR < -500) {
        PID_sumR = -500;
    }

    
}






//ESP variables

float ESP_left = 0;
float ESP_right = 0;

//ideal 3
float gain = 1;

float centerRPM = 0;

float k66conversion = 0;


float prev_angle = 0;

int angle(int pwm){
 
 return map(steeringAngle.pulsewidth()*1000000,1000,2000,-45,45);
    
}    




int beep = 0;

float temp1 = 0;
float temp2 = 0;

//______________________MAIN____________________________________________________

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

    

    //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 servo_pulsewidth = 0;
    float pre_pulsewidth = 0;

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

    //ESC Settings
    leftESC.period_ms(10);
    rightESC.period_ms(10);

    //ARMING ESCS
    leftESC.pulsewidth_us(1500);  //write them to idle position
    rightESC.pulsewidth_us(1500);
    
    //wait 4 seconds to arm escs
    wait(0.9f);   
    wait(0.9f);
    wait(0.2f);
    wait(0.9f);   
    wait(0.9f);
    wait(0.2f);
    
    blue = !green;
    green = !red;
    red = !blue;
    



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


    //MAIN LOOP_________________________________________________________________
    while (true) {

        if(beep >= 10000){
            buzzer.beep(600,10);
             beep = 0;    
        } 
        beep++;

        //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
        calcPIDleft();
        calcPIDright();
        
        
        //torque vectoring
        ESP_left = ( -0.0228 * (servo_pulsewidth) + 35.5581 ) * gain;
        ESP_right = ( 0.0228 * (servo_pulsewidth) - 35.5581) * gain;
        
        //drifting
        //ESP_right = ( -0.0228 * (servo_pulsewidth) + 35.5581 ) * gain;
        //ESP_left = ( 0.0228 * (servo_pulsewidth) - 35.5581) * gain;
        
                
        //groundspeed return channel to K66
        centerRPM = (leftRPM + rightRPM) / 2;
        k66conversion = map( centerRPM , 0 , 8000 , 1000 , 2000 );
        SpeedReturnChannel.pulsewidth_us( k66conversion );
        
        
        
        
        

        //check for drivng switch to be pressed
        if( ( (motorSpeed.pulsewidth()*1000000) <= 1480 || (motorSpeed.pulsewidth()*1000000) >= 1516 ) && !brake && fast) {
            
            leftESC.pulsewidth_us( 1500 + ( ( (0.07518797 * (set_speed ) ) + PID_outL )  ) + ESP_left);
            rightESC.pulsewidth_us( 1500 + ( ( (0.07518797 * (set_speed ) ) + PID_outR )  ) + ESP_right);
            //leftESC.pulsewidth_us(1500 + (int)floor(poti_val));
            //rightESC.pulsewidth_us(1500 + (int)floor(poti_val));
            LED = 1;
            leftLed.setGreen();
            rightLed.setGreen();
            //locked = false;
        }else if(brake){
            leftLed.setRed();
            rightLed.setRed();
            leftESC.pulsewidth_us(1500);
            rightESC.pulsewidth_us(1500);
        }
        else {
            leftLed.setBlue();
            rightLed.setBlue();
            
            leftESC.pulsewidth_us(1500);
            rightESC.pulsewidth_us(1500);
            LED = 0;
        }

        //PASSTHROUGH FOR STEERING
        pre_pulsewidth = servo_pulsewidth;
        
        servo_pulsewidth = steeringAngle.pulsewidth();
        servo_pulsewidth = servo_pulsewidth * 1000000;
        
        //noise reduction and filtering of pwm signal from k66
        if( servo_pulsewidth > 2000 ){
            servo_pulsewidth = pre_pulsewidth;
        }    
        
        Servo.pulsewidth_us(servo_pulsewidth);
        
        //poti_val = abs( (poti.read() * 1000) + 1000);
        //Servo.pulsewidth_us( 1500 );
        //SpeedReturnChannel.pulsewidth_us( 1500 );
        
       
        
       
      
       
       
       
       
       
       if(pre_pulsewidth != servo_pulsewidth){
       SLAM1 = (SLAM1+SLAM2)/2;
       temp1 = SLAM1*cos(angle(servo_pulsewidth * 1000000)+prev_angle)*2;
       temp2 = SLAM1*sin(angle(servo_pulsewidth * 1000000)+prev_angle)*2;
      
       pc.printf("X" "%f" "\n" , temp1 );    
       pc.printf("Y" "%f" "\n" , temp2 );
     
       SLAM1 = 0;
       SLAM2 = 0;
       }
       
       
       
       
       
       
       
       
       
       
       
       

        
        prev_angle += angle(servo_pulsewidth * 1000000);

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

        poti_val = abs(poti.read() * 4000);
       
        
        
        
       
        
        
        if( (motorSpeed.pulsewidth()*1000000) <= 1100 ) set_speed = poti_val;
        
        if( (motorSpeed.pulsewidth()*1000000) >= 1500 ) set_speed = map(motorSpeed.pulsewidth()*1000000 , 1500 , 2000 , 0 , 7000 );
        
        //Kp = poti_val;
        
        
        //DEBUGGING___________________________________
        /*
        pc.printf("motorSpeed:");
        pc.printf("%f", motorSpeed.pulsewidth()*1000000);
        pc.printf("\t");
        */
        /*
        pc.printf("leftRPM:");
        pc.printf("%f", leftRPM);
        pc.printf("\t");
        
        pc.printf("rightRPM:");
        pc.printf("%f", rightRPM);
        pc.printf("\t");
        
        pc.printf("ESP_left:");
        pc.printf("%f", ESP_left);
        pc.printf("\t");
        
        pc.printf("ESP_right:");
        pc.printf("%f", ESP_right);
        pc.printf("\t");
        
        pc.printf("steering angle:");
        pc.printf("%f", (servo_pulsewidth));
        pc.printf("\t");
        
        
        
        pc.printf("PID_OUT:");
        pc.printf("%f", 1500+PID_outL );
        pc.printf("\t");
        
        
        pc.printf("set_speed:");
        pc.printf("%f ", set_speed);
        pc.printf("\t");
        
        
        pc.printf("gain:");
        pc.printf("%f", gain);
        pc.printf("\t");
        
        
        //necessary to keep values centered
        pc.printf("zero: 0.0 \t \n" );
        */
        //_____________________________________________
        
    }
    //END OF MAIN LOOP__________________________________________________________
}