s
Dependencies: PwmIn mbed RASCarLED buzzer millis
Diff: main.cpp
- Revision:
- 8:32133eeb7037
- Parent:
- 7:986d5298b118
- Child:
- 9:92283a284936
--- a/main.cpp Wed Jul 16 09:59:53 2014 +0000 +++ b/main.cpp Sat Jan 25 17:11:19 2020 +0000 @@ -1,17 +1,310 @@ #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() { - int i = 0; - pc.printf("Hello World!\n"); + 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) { - wait(0.5f); // wait a small period of time - pc.printf("%d \n", i); // print the value of variable i - i++; // increment the variable - myled = !myled; // toggle a led + + + //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__________________________________________________________ +} \ No newline at end of file