Diff: main.cpp

Revision:

12:2847b345b5cf

Parent:

8:eefbd3880d28

Child:

13:c22e150048ae

--- a/main.cpp	Thu Mar 12 12:18:25 2015 +0000
+++ b/main.cpp	Fri Mar 13 13:12:55 2015 +0000
@@ -23,97 +23,108 @@
 //Bluetooth Module to debug
 Serial blue(PTC4,PTC3);
 
-void set_direction( int direction, float speed, float angle)
+void set_direction( int direction, float duty_l, float duty_r)
 {
-    /* *******************Example Use*****************
-                 set_direction("11", 0.5, 0);  //This would set the robot to go forward, at half speed, straight
-
-                 set_directin( "11", 0.8, 0.2); //this would set robot to go forward, at 100% to the right and 60% to the left (turn right)
-
-                 set_direction("00", 0.3, 0.7); //robot back, 100% rightmotor; 0%left (technically -40%, but 0% is min)
-
-                 set_direction("11", 0.5, 0.5); // robot pivots forward, to the right, full speed (100% duty right ; 0% duty left)
-
-                 set_direction("11", 0.5, -0.5); //robot pivots forward, to the left,  full speed  (0% duty right ; 100% duty left)
-
-                 set_direction("11", 0.3, 0.5); //robot pivots forard, to the right , at 80% speed (80% right; 0% left)
-
-                 set_direction("11", -0.3, 0.5); //robot pivotrs foward, to the right, AT 20% SPEED (20%, 0%)
-
-                 set_direction("10", 0.5, 0);   //robot spins to teh right, at half speed. Left forward 50% ; Right forward 50%
-
-                 set_direction("10", 0.5, 0.3); //robot spins to the right, 80% right forward ; 20% left backward
-
-    */
-
-    float r_result = (speed + angle); //where angle can be postive and negative and will TURN the robot
-    float l_result = (speed - angle); //if turning left, you'd make the angle postive (right motor goes faster left motor goes slwoer)
     switch( direction ) {
         case 0x11: { //forward
-            motor_r.write(  r_result  < 0 ? 0    :     r_result > 1 ? 1    :       r_result);
+            motor_r.write( duty_r);
             //Look up ternary statements -- {condition ? value_if_true : value_if_false} This makes 0<result<1
-            motor_l.write(  l_result < 0 ? 0     :     l_result > 1 ? 1    :       l_result);
+            motor_l.write( duty_l);
 
             motor_rf=1;
             motor_rb=0;
             motor_lf=1;
             motor_lb=0;
-            blue.printf("I'm going forwards, r_motor duty is %f ; l_motor duty is %f", r_result, l_result);
+            blue.printf("Going forward, right:%f ; left:%f\n", duty_r, duty_l);
+            break;
         }
         case 0x00: { //backward
-            motor_r.write( r_result  < 0 ? 0    :     r_result > 1 ? 1    :       r_result);
-            motor_l.write( l_result  < 0 ? 0    :     l_result > 1 ? 1    :       l_result);
+            motor_r.write( duty_r);
+            motor_l.write( duty_l);
 
             motor_rf=0;
             motor_rb=1;
             motor_lf=0;
             motor_lb=1;
-            blue.printf("I'm going backwards, r_motor duty is %f ; l_motor duty is %f", r_result, l_result);
-
+            blue.printf("Going backward, right:%f ; left:%f\n", duty_r, duty_l);
+            break;
         }
         case 0x01: {  //spin left  --     Right forward, left backward
-            motor_r.write( r_result  < 0 ? 0    :     r_result > 1 ? 1    :       r_result);
-            motor_l.write( l_result  < 0 ? 0    :     l_result > 1 ? 1    :       l_result);
+            motor_r.write( duty_r);
+            motor_l.write( duty_l);
 
             motor_rf=1;
             motor_rb=0;
             motor_lf=0;
             motor_lb=1;
-            blue.printf("I'm spinning left, r_motor duty is %f ; l_motor duty is %f", r_result, l_result);
-
+            blue.printf("Spinning Left, right:%f ; left:%f\n", duty_r, duty_l);
+            break;
         }
         case 0x10: {    //spin right
-            motor_r.write( r_result  < 0 ? 0    :     r_result > 1 ? 1    :       r_result);
-            motor_l.write( l_result  < 0 ? 0    :     l_result > 1 ? 1    :       l_result);
+            motor_r.write( duty_r);
+            motor_l.write( duty_l);
 
             motor_rf=0;
             motor_rb=1;
             motor_lf=1;
             motor_lb=0;
-            blue.printf("I'm spinning right, r_motor duty is %f ; l_motor duty is %f", r_result, l_result);
-
+            blue.printf("Spinning Right, right:%f ; left:%f\n", duty_r, duty_l);
+            break;
         }
     }
 }
-timer h_r, h_l;
-float hall_rt1, hall_lt1, hall_rt2, hall_lt2//hall right time & left time
-int h_f_r, h_f_l //hallflagright and left
+
+//HALL EFFECT INITIALISATIONS
+Timer h_r, h_l;
+float hall_rt1, hall_lt1, hall_rt2, hall_lt2 = 0; //hall right time & left time
+float hall_rdt, hall_ldt;
+int h_f_r, h_f_l; //hallflagright and left
 void hallr() //this just reading the various times.
 {
     if (h_f_r==2) {
-        start.h_r();
+        h_r.start();
+        hall_rt1=0;
+        hall_rt2 =0;
+        blue.printf("Timer R Starts\n");
+        h_f_r =1;
     } else if(h_f_r == 1) {
-        hall_rt1 = read.h_r();
+        hall_rt1 = h_r.read();
+        //blue.printf("rt1 : %f\n", hall_rt1);
         h_f_r = 0;
-    } else if (h_f_l == 0) {
-        hall_rt2 = read.h_r();
+        hall_rdt = hall_rt1-hall_rt2;
+    } else if (h_f_r == 0) {
+        hall_rt2 = h_r.read();
+        //blue.printf("rt2 : %f\n", hall_rt2);
+
         h_f_r = 1;
+        hall_rdt = hall_rt2-hall_rt1;
     }
+    //blue.printf("Hall_R dt : %f\n", hall_rdt);
 
 }
 void halll()
 {
+    blue.printf("Left been triggered\n");
+    if (h_f_l==2) {
+        h_l.start();
+        hall_lt1=0;
+        hall_lt2 =0;
+        blue.printf("Timer L starts\n");
+        h_f_l = 1;
+    } else if(h_f_l == 1) {
+        hall_lt1 = h_l.read();
+        //blue.printf("lt1 : %f\n", hall_lt1);
 
+        h_f_l = 0;
+        hall_ldt = hall_lt1-hall_lt2;
+    } else if (h_f_l == 0) {
+        hall_lt2 = h_l.read();
+        //    blue.printf("lt1 : %f\n", hall_lt1);
+
+        h_f_l = 1;
+        hall_ldt = hall_lt2-hall_lt1;
+    }
+    //blue.printf("Hall_L dt : %f\n", hall_ldt);
 
 }
 int main()
@@ -121,11 +132,9 @@
     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_r.fall(&hallr());
+    hall_r.rise(&hallr);
 
-    hall_l.rise(&halll());
-    hall_l.fall(&halll());
+    hall_l.rise(&halll);
 //Set PWM frequency to 1000Hz
 
 
@@ -136,202 +145,10 @@
     motor_rb=0;
     motor_lf=0;
     motor_lb=0;
-    int direction = 0x11;
-    char rem_dir[2]; //remote_direction
-
-
-
-
-
-
-    /*BLUETOOTH TEST
-
+    wait(1);
+    set_direction(0x11, 0.3,0.3);
     while(1) {
-
-
-        wait(1);
-        blue.printf("Gees a Direction\nFF for forward both, BB for backward both, FB for forward left and back right .. etc. \n");
-
-        while ( blue.readable()==0 ) {}               // loops until there is an input from the user
-
-        for(int i=0; i<2; i++) {
-            rem_dir[i] = blue.getc();    //Store incoming response into array
-        }
-
-        //0x46 is F, 0x66 is f
-        //0x42 is B , 0x62 is b
-        if(   (rem_dir[0] == 'F'|'f' )     &&     ( rem_dir[1] == 'F'|'f')   ) {
-            direction = 0x11;
-            blue.printf("You wanted to go straight\n");
-        } else if (   (rem_dir[0] == 'B'|'b' )     &&     ( rem_dir[1] == 'B'|'b')  ) {
-
-            direction = 0x00;
-            blue.printf("You wanted to go backwards\n");
-
-        } else if (   (rem_dir[0] == 'B'|'b' )     &&     ( rem_dir[1] == 'F'|'f')   ) {
-
-            direction = 0x01;
-            blue.printf("You wanted to spin anti-clockwise\n");
-
-        } else if (   (rem_dir[0] == 'F'|'f' )     &&     ( rem_dir[1] == 'B'|'b') )  {
-
-            direction = 0x10;
-            blue.printf("You wanted to spin clockwise\n");
-        } else {}
-         set_direction(direction, 0.5, 0);
-
-        /* SETTING SPEED AND ANGLE TOO
-        blue.printf("Whit speed you going at?\n Send it as a duty percentage");
-        while ( blue.readable()==0 ) {}               // loops until there is an input from the user
-
-        speed = input;
-
-        blue.printf("What angle you going at? percentage again (negative percentage for opposite dir)");
-                while ( blue.readable()==0 ) {}               // loops until there is an input from the user
-
-        angle = input;
-        */
-
-    //set_direction(direction, speed, angle);
-
-    //}
-
-    /* //Also Working Demo
-    wait(2);
-    led = !led;
-    set_direction(0x11, 0.5, 0);  //This would set the robot to go forward, at half speed, straight
-
-    wait(2);
-    led = !led;
-    set_direction(0x11, 0.8, 0.2); //this would set robot to go forward, at 100% to the right and 60% to the left (turn right)
-
-    wait(2);
-    led = !led;
-    set_direction(0x00, 0.3, 0.7); //robot back, 100% rightmotor; 0%left (technically -40%, but 0% is min)
-
-    wait(2);
-    led = !led;
-    set_direction(0x11, 0.5, 0.5); // robot pivots forward, to the right, full speed (100% duty right ; 0% duty left)
-
-    wait(2);
-    led = !led;
-    set_direction(0x11, 0.5, -0.5); //robot pivots forward, to the left,  full speed  (0% duty right ; 100% duty left)
-
-    wait(2);
-    led = !led;
-    set_direction(0x11, 0.3, 0.5); //robot pivots forard, to the right , at 80% speed (80% right; 0% left)
-
-    wait(2);
-    led = !led;
-    set_direction(0x11, -0.3, 0.5); //robot pivotrs foward, to the right, AT 20% SPEED (20%, 0%)
-
-    wait(2);
-    led = !led;
-    set_direction(0x10, 0.5, 0);   //robot spins to teh right, at half speed. Left forward 50% ; Right forward 50%
-
-    wait(2);
-    led = !led;
-    set_direction(0x10, 0.5, 0.3); //robot spins to the right, 80% right forward ; 20% left backward
-
-    }*/
-
-
-    /*  // working demo
-
-      while(1) { //infinite loop
-          //***********************************************
-          myled = !myled;
-
-          //Set duty cycle at 100% AKA Full Speed
-          motor_r.write(1.0);
-          motor_l.write(1.0);
-
-          motor_rf=1;
-          motor_rb=0;
-          motor_lf=1;
-          motor_lb=0; //go forwards
-          wait(2);
-          //**************************************
-          myled = !myled;
-
-          motor_r.write(0.75);
-          motor_l.write(0.75);
-
-          motor_rf=1;
-          motor_rb=0;
-          motor_lf=1;
-          motor_lb=0; //go forwards
-          //
-          wait(2);
-
-          myled = !myled;
-
-          motor_r.write(0.5);
-          motor_l.write(0.5);
-
-          motor_rf=1;
-          motor_rb=0;
-          motor_lf=1;
-          motor_lb=0;
-
-
-          wait(2);
-          myled = !myled;
-
-          motor_r.write(0.30);
-          motor_l.write(0.30);
-
-          motor_rf=1;
-          motor_rb=0;
-          motor_lf=1;
-          motor_lb=0;
-
-          wait(2);
-
-          myled = !myled;
-          motor_r.write(1.0);
-          motor_l.write(1.0);
-
-          motor_rf=0;
-          motor_rb=1;
-          motor_lf=0;
-          motor_lb=1; //go backwards
-          //
-          wait(2);
-
-
-
-          myled = !myled;
-          motor_r.write(0.5);
-          motor_l.write(0.5);
-
-          motor_rf=0;
-          motor_rb=0;
-          motor_lf=0;
-          motor_lb=1; //
-
-          wait(2);
-          myled = !myled;
-
-          motor_rf=0;
-          motor_rb=1;
-          motor_lf=0;
-          motor_lb=0; //reverse turn
-
-          wait(2);
-
-          myled = !myled;
-          motor_r.write(0.5);
-          motor_l.write(1.0);
-
-          motor_rf=1;
-          motor_rb=0;
-          motor_lf=1;
-          motor_lb=0; //go forwards
-          //
-          wait(4);
-      }*/
-
-
-
+        led = !led;
+        wait(0.5);
+    }
 }