Diff: main.cpp

Revision:

14:a243e6a78b2c

Parent:

13:c22e150048ae

Child:

16:be88d07a24fe

diff -r c22e150048ae -r a243e6a78b2c main.cpp
--- a/main.cpp	Mon Mar 16 16:58:13 2015 +0000
+++ b/main.cpp	Tue Mar 17 15:13:37 2015 +0000
@@ -22,6 +22,9 @@
 float time1_l, time2_l, time3_l, time1_r, time2_r, time3_r;
 //Bluetooth Module to debug
 Serial blue(PTC4,PTC3);
+//********************************************************
+float desired_duty_r; //USED FOR PD_CONTROL() SET IN set_dir()
+float desired_duty_l;
 
 void set_direction( int direction, float duty_l, float duty_r)
 {
@@ -75,6 +78,7 @@
             break;
         }
     }
+    init_PD ();
 }
 
 //HALL EFFECT INITIALISATIONS
@@ -130,59 +134,81 @@
     blue.printf("Hall_L dt : %f\n", hall_ldt);
 
 }
+float   pres_duty_r, pres_duty_l; 
+float   a_ratio;
+float   desired_ratio; 
+float   perc_r;
+float   perc_l;
+float   perc_tot;
+float   threshold; 
+float   limit;
+float   inc_value; 
+
+void init_PD ()
+{
+    //all these values have to be global; this function gets called each time there's a new direction/desired speed
+    a_ratio = hall_ldt/hall_rdt; //actual_ratio
+    desired_ratio = desired_duty_l/desired_duty_r; //desired_duty comes from set_direction()
+    perc_r = (1-((desired_duty_r-0.1)/desired_duty_r));//absolute vlaue of +-0.1 duty means a differnt percentage error for each duty value
+    perc_l = (1-((desired_duty_l-0.1)/desired_duty_l));
+    perc_tot = perc_r + perc_l;
+    threshold = desired_ratio*perc_tot; //dynamic threshold that will change according to duties
+    limit = 0.2*((desired_duty_l + desired_duty_r)/2); //twenty percent of the average duty
+    inc_value = 0.05*((desired_duty_l + desired_duty_r)/2); //probably need to change 5% from testing
+}
 void PD_control ()
 {
-    //PSUEDO
-    a_ratio = hall_ldt/hall_rdt; //actual_ratio
+
     if(a_ratio < (desired_ratio - threshold)) { //if right is too big (bottom heavy denomotator)
-        if (desired_duty_l < (duty_l + up_limit)) { //If you've not already set the duty too high.  up_limit to be decided from testing
-            duty_l =+ inc_value; //Make Left go faster.  Incremental value requires testing.
-            motor_l.write( duty_l);//actually changing the duty here
+        if (pres_duty_l < (desired_duty_l + limit)) { //If you've not already set the duty too high.  up_limit to be decided from testing
+            pres_duty_l =+ inc_value; //Make Left go faster.  Incremental value requires testing.
+            motor_l.write( pres_duty_l);//actually changing the duty here
         } else {
-            if (desired_duty_r > (duty_r + low_limit)) { // if right isn't too small already
-                duty_r =- inc_value; //make right go slower
-                motor_r.write(duty_r);
+            if (pres_duty_r > (desired_duty_r - limit)) { // if right isn't too small already
+                pres_duty_r =- inc_value; //make right go slower
+                motor_r.write(pres_duty_r);
             }
             //What to do when right is low limt AND left is up limit AND we've still not met ratio??
         }
-        else {
-            if(a_ratio > (desired_ratio + threshold)) {
-                if (desired_duty_r < (duty_r + up_limit)) { //If you've not already set the duty too high.  up_limit to be decided from testing
-                    duty_r =+ inc_value; //Make Left go faster.  Incremental value requires testing.
-                    motor_r.write( duty_r);//actually changing the duty here
-                } else {
-                    if (desired_duty_l > (duty_l + low_limit)) { // if right isn't too small already
-                        duty_l =- inc_value; //make right go slower
-                        motor_l.write(duty_l);
-                    }
-                    //What to do when left is low limt AND right is up limit AND we've still not met ratio??
+    } else {
+        if(a_ratio > (desired_ratio + threshold)) {
+            if (pres_duty_r < (desired_duty_r + limit)) { //If you've not already set the duty too high.  up_limit to be decided from testing
+                pres_duty_r =+ inc_value; //Make Left go faster.  Incremental value requires testing.
+                motor_r.write( pres_duty_r);//actually changing the duty here
+            } else {
+                if (pres_duty_l > (desired_duty_l + limit)) { // if right isn't too small already
+                    pres_duty_l =- inc_value; //make right go slower
+                    motor_l.write(pres_duty_l);
                 }
+                //What to do when left is low limt AND right is up limit AND we've still not met ratio??
             }
+        }
 
-        }
+    }
 
 
 
-    }
-    int main() {
-        h_f_r = 2;
-        h_f_l = 2;
-        //attaching hall_r and hall_r so that it calls hall() on a rising and falling edge
-        hall_r.rise(&hallr);
+}
+int main()
+{
+    h_f_r = 2;
+    h_f_l = 2;
+    //attaching hall_r and hall_r so that it calls hall() on a rising and falling edge
+    hall_r.rise(&hallr);
 
-        hall_l.rise(&halll);
+    hall_l.rise(&halll);
 //Set PWM frequency to 1000Hz
 
 
-        motor_l.period( 1.0f / (float) PWM_FREQ);
-        motor_r.period( 1.0f / (float) PWM_FREQ);
+    motor_l.period( 1.0f / (float) PWM_FREQ);
+    motor_r.period( 1.0f / (float) PWM_FREQ);
 //Initialise direction to nothing.
-        motor_rf=0;
-        motor_rb=0;
-        motor_lf=0;
-        motor_lb=0;
-        wait(1);
-        set_direction(0x11, 0.2,0.2);
-        while(1) {
-        }
+    motor_rf=0;
+    motor_rb=0;
+    motor_lf=0;
+    motor_lb=0;
+    wait(1);
+    set_direction(0x11, 0.2,0.2);
+    while(1) {
     }
+}