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