s
Dependencies: PwmIn mbed RASCarLED buzzer millis
main.cpp
- Committer:
- LordScarface
- Date:
- 2020-01-25
- Revision:
- 8:32133eeb7037
- Parent:
- 7:986d5298b118
- Child:
- 9:92283a284936
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__________________________________________________________ }