Diff: main.cpp

Revision:

7:38def1917030

Parent:

4:6db8e0babea7

Child:

8:eefbd3880d28

--- a/main.cpp	Fri Mar 06 15:18:14 2015 +0000
+++ b/main.cpp	Wed Mar 11 23:42:10 2015 +0000
@@ -16,6 +16,9 @@
 //LED to test
 DigitalOut led(LED_RED);
 
+//Bluetooth Module to debug
+Serial blue(PTC4,PTC3);
+
 void set_direction( int direction, float speed, float angle)
 {
     /* *******************Example Use*****************
@@ -51,6 +54,7 @@
             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);
         }
         case 0x00: { //backward
             motor_r.write( r_result  < 0 ? 0    :     r_result > 1 ? 1    :       r_result);
@@ -60,6 +64,8 @@
             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);
+
         }
         case 0x01: {  //spin left  --     Right forward, left backward
             motor_r.write( r_result  < 0 ? 0    :     r_result > 1 ? 1    :       r_result);
@@ -69,6 +75,8 @@
             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);
+
         }
         case 0x10: {    //spin right
             motor_r.write( r_result  < 0 ? 0    :     r_result > 1 ? 1    :       r_result);
@@ -78,6 +86,8 @@
             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);
+
         }
     }
 }
@@ -91,144 +101,195 @@
     motor_rb=0;
     motor_lf=0;
     motor_lb=0;
+    int direction = 0x11;
+    char rem_dir[2]; //remote_direction
 
     while(1) {
-        
-        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
+        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
+        }
 
-    while(1) { //infinite loop
-        //***********************************************
-        myled = !myled;
+        //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')   ) {
 
-        //Set duty cycle at 100% AKA Full Speed
-        motor_r.write(1.0);
-        motor_l.write(1.0);
+            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);
 
-        motor_rf=1;
-        motor_rb=0;
-        motor_lf=1;
-        motor_lb=0; //go forwards
-        wait(2);
-        //**************************************
-        myled = !myled;
+        /* 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;
+        */
 
-        motor_r.write(0.75);
-        motor_l.write(0.75);
+        //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
 
-        motor_rf=1;
-        motor_rb=0;
-        motor_lf=1;
-        motor_lb=0; //go forwards
-        //
-        wait(2);
+    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)
 
-        myled = !myled;
+    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)
 
-        motor_r.write(0.5);
-        motor_l.write(0.5);
+    wait(2);
+    led = !led;
+    set_direction(0x11, 0.3, 0.5); //robot pivots forard, to the right , at 80% speed (80% right; 0% left)
 
-        motor_rf=1;
-        motor_rb=0;
-        motor_lf=1;
-        motor_lb=0;
+    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
+
+    }*/
 
 
-        wait(2);
-        myled = !myled;
+    /*  // 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_r.write(0.30);
-        motor_l.write(0.30);
+          motor_rf=1;
+          motor_rb=0;
+          motor_lf=1;
+          motor_lb=0; //go forwards
+          wait(2);
+          //**************************************
+          myled = !myled;
 
-        motor_rf=1;
-        motor_rb=0;
-        motor_lf=1;
-        motor_lb=0;
+          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;
 
-        wait(2);
+          motor_r.write(0.5);
+          motor_l.write(0.5);
 
-        myled = !myled;
-        motor_r.write(1.0);
-        motor_l.write(1.0);
+          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=0;
-        motor_rb=1;
-        motor_lf=0;
-        motor_lb=1; //go backwards
-        //
-        wait(2);
+          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);
+          myled = !myled;
+          motor_r.write(0.5);
+          motor_l.write(0.5);
 
-        motor_rf=0;
-        motor_rb=0;
-        motor_lf=0;
-        motor_lb=1; //
+          motor_rf=0;
+          motor_rb=0;
+          motor_lf=0;
+          motor_lb=1; //
 
-        wait(2);
-        myled = !myled;
+          wait(2);
+          myled = !myled;
 
-        motor_rf=0;
-        motor_rb=1;
-        motor_lf=0;
-        motor_lb=0; //reverse turn
+          motor_rf=0;
+          motor_rb=1;
+          motor_lf=0;
+          motor_lb=0; //reverse turn
 
-        wait(2);
+          wait(2);
 
-        myled = !myled;
-        motor_r.write(0.5);
-        motor_l.write(1.0);
+          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);
-    }*/
-        
+          motor_rf=1;
+          motor_rb=0;
+          motor_lf=1;
+          motor_lb=0; //go forwards
+          //
+          wait(4);
+      }*/
+
 
 
 }