Diff: main.cpp

Revision:

13:c22e150048ae

Parent:

12:2847b345b5cf

Child:

14:a243e6a78b2c

diff -r 2847b345b5cf -r c22e150048ae main.cpp
--- a/main.cpp	Fri Mar 13 13:12:55 2015 +0000
+++ b/main.cpp	Mon Mar 16 16:58:13 2015 +0000
@@ -25,6 +25,9 @@
 
 void set_direction( int direction, float duty_l, float duty_r)
 {
+    desired_duty_r = duty_r; //USED FOR PD_CONTROL()
+    desired_duty_l = duty_l;
+
     switch( direction ) {
         case 0x11: { //forward
             motor_r.write( duty_r);
@@ -99,7 +102,7 @@
         h_f_r = 1;
         hall_rdt = hall_rt2-hall_rt1;
     }
-    //blue.printf("Hall_R dt : %f\n", hall_rdt);
+    blue.printf("Hall_R dt : %f\n", hall_rdt);
 
 }
 void halll()
@@ -124,31 +127,62 @@
         h_f_l = 1;
         hall_ldt = hall_lt2-hall_lt1;
     }
-    //blue.printf("Hall_L dt : %f\n", hall_ldt);
+    blue.printf("Hall_L dt : %f\n", hall_ldt);
 
 }
-int main()
+void PD_control ()
 {
-    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);
+    //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
+        } 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);
+            }
+            //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??
+                }
+            }
 
-    hall_l.rise(&halll);
+        }
+
+
+
+    }
+    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);
 //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.3,0.3);
-    while(1) {
-        led = !led;
-        wait(0.5);
+        motor_rf=0;
+        motor_rb=0;
+        motor_lf=0;
+        motor_lb=0;
+        wait(1);
+        set_direction(0x11, 0.2,0.2);
+        while(1) {
+        }
     }
-}