s
Dependencies: PwmIn mbed RASCarLED buzzer millis
main.cpp
- Committer:
- LordScarface
- Date:
- 2020-05-04
- Revision:
- 9:92283a284936
- Parent:
- 8:32133eeb7037
File content as of revision 9:92283a284936:
#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__________________________________________________________ }