Diff: main.cpp

Revision:

2:1feae3cb6731

Parent:

1:2bab3a0bc3bc

Child:

3:b8556c89b389

--- a/main.cpp	Wed Feb 18 14:57:56 2015 +0000
+++ b/main.cpp	Wed Feb 18 16:22:47 2015 +0000
@@ -16,50 +16,74 @@
 //LED to test
 DigitalOut led(LED_RED);
 
-void set_direction( bool direction, float speed, float angle)
+void set_direction( int direction, float speed, float angle)
 {
     /* *******************Example Use*****************
-                 set_direction(1, 0.5, 0);  //This would set the robot to go forward, at half speed, straight
+                 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_directin(1, 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(0, 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(1, 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(1, 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(1, 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(1, -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
 
     */
-    switch(direction) {
-        case '1': { //forward
-            motor_r.write(speed + angle); //where angle can be postive and negative and will TURN the robot
-            motor_l.write(speed - angle);
-            //if turning left, you'd make the angle postive (right motor goes faster left motor goes slwoer)
+
+    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);
+            //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_rf=1;
             motor_rb=0;
             motor_lf=1;
             motor_lb=0;
         }
-        case '0': { //backward
-            motor_r.write(speed + angle); //where angle can be postive and negative and will TURN the robot
-            motor_l.write(speed - angle);
-            //if turning left, you'd make the angle postive (right motor goes faster left motor goes slwoer)
+        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_rf=0;
             motor_rb=1;
             motor_lf=0;
             motor_lb=1;
         }
+        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_rf=1;
+            motor_rb=0;
+            motor_lf=0;
+            motor_lb=1;
+        }
+        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_rf=0;
+            motor_rb=1;
+            motor_lf=1;
+            motor_lb=0;
+        }
     }
 }
-
 int main()
 {
-
-    //Set PWM frequency to 1000Hz
+//Set PWM frequency to 1000Hz
     motor_l.period( 1.0f / (float) PWM_FREQ);
     motor_r.period( 1.0f / (float) PWM_FREQ);
 //Initialise direction to nothing.
@@ -68,36 +92,44 @@
     motor_lf=0;
     motor_lb=0;
 
-
-
     while(1) {
+        
         wait(2);
         led = !led;
-        set_direction(1, 0.5, 0);  //This would set the robot to go forward, at half speed, straight
+        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(1, 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(0x00, 0.3, 0.7); //robot back, 100% rightmotor; 0%left (technically -40%, but 0% is min)
 
         wait(2);
         led = !led;
-        set_direction(0, 0.3, 0.7); //robot back, 100% rightmotor; 0%left (technically -40%, but 0% is min)
+        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(1, 0.5, 0.5); // robot pivots forward, to the right, full speed (100% duty right ; 0% duty left)
+        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(1, 0.5, -0.5); //robot pivots forward, to the left,  full speed  (0% duty right ; 100% duty left)
+        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(1, 0.3, 0.5); //robot pivots forard, to the right , at 80% speed (80% right; 0% left)
+        set_direction(0x11, -0.3, 0.5); //robot pivotrs foward, to the right, AT 20% SPEED (20%, 0%)
 
         wait(2);
         led = !led;
-        set_direction(1, -0.3, 0.5); //robot pivotrs foward, to the right, AT 20% SPEED (20%, 0%)
+        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
+
     }