Diff: main.cpp

Revision:

11:f07662c64e26

Parent:

9:92895704e1a4

Child:

15:3f6626f68836

diff -r 92895704e1a4 -r f07662c64e26 main.cpp
--- a/main.cpp	Thu Mar 12 14:44:56 2015 +0000
+++ b/main.cpp	Fri Mar 13 10:55:39 2015 +0000
@@ -67,6 +67,7 @@
         sensor_select(x);
         //blue = !blue;
         wait(0.1);
+        blue.printf("X is %i,   ", x);
 
         if (input == 1) {
             val=val+2^x; //could you comment on what this is for?
@@ -74,20 +75,16 @@
             val=val;//+2^x;
         }
     }
-    blue.printf("%i\n",val);
+    blue.printf("\nVAL : %i\n",val);
     return val;
 }
 
 
 void set_direction( int direction, float duty_l, float duty_r)
 {
-
-
-   // 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(  duty_r);
+            motor_r.write( duty_r);
             //Look up ternary statements -- {condition ? value_if_true : value_if_false} This makes 0<result<1
             motor_l.write( duty_l);
 
@@ -95,6 +92,8 @@
             motor_rb=0;
             motor_lf=1;
             motor_lb=0;
+            blue.printf("Going forward, right:%f ; left:%f\n", duty_r, duty_l);
+            break;
         }
         case 0x00: { //backward
             motor_r.write( duty_r);
@@ -104,6 +103,8 @@
             motor_rb=1;
             motor_lf=0;
             motor_lb=1;
+            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( duty_r);
@@ -113,6 +114,8 @@
             motor_rb=0;
             motor_lf=0;
             motor_lb=1;
+            blue.printf("Spinning Left, right:%f ; left:%f\n", duty_r, duty_l);
+            break;
         }
         case 0x10: {    //spin right
             motor_r.write( duty_r);
@@ -122,6 +125,8 @@
             motor_rb=1;
             motor_lf=1;
             motor_lb=0;
+            blue.printf("Spinning Right, right:%f ; left:%f\n", duty_r, duty_l);
+            break;
         }
     }
 }
@@ -131,27 +136,32 @@
     //Line_right = !Line_right;
     switch( sensor_values ) {
         case 0x00: { //no lines robot is lost!!!!
+            blue.printf("no lines robot is lost!!!!\n");
             set_direction(0x00, 1.0, 1.0);
             wait(1);
             break;
         }
         case 0x01: { //to far left turn right 0001
+            blue.printf("0001\n");
             set_direction(0x11, 0.0, 1.0); //this doesn't turn right; this goes straight at half speed
             wait(1);
             Line_right = !Line_right;
             break;
         }
         case 0x02: {  //left turn right       0010
+            blue.printf("0010\n");
             set_direction(0x11, 0, 0.5);
             wait(1);
             break;
         }
         case 0x03: {  //far left turn right   0011
+            blue.printf("0011\n");
             set_direction(0x11, 0.0, 0.0);
             wait(1);
             break;
         }
         case 0x4: {    //right turn left      0100
+            blue.printf("0100\n");
             set_direction(0x11, 0.5, 0.0);
             wait(1);
             break;
@@ -161,16 +171,19 @@
             break;
         }
         case 0x6: {    //perfect              0110
+            blue.printf("perfect, 0110\n");
             set_direction(0x00, 1.0, 1.0);
             wait(1);
             break;
         }
         case 0x7: {//corner junction or plate 0111
+            blue.printf("0111\n");
             set_direction(0x00, 0.0, 0.0);
             wait(1);
             break;
         }
         case 0x8: {//to far right turn left   1000
+            blue.printf("1000\n");
             set_direction(0x11, 1.0, 0.0);
             wait(1);
             break;
@@ -190,6 +203,7 @@
             break;
         }
         case 0xC: { //far right turn left     1100
+            blue.printf("1100\n");
             set_direction(0x00, 0.0, 0.0);
             wait(1);
             break;
@@ -199,11 +213,13 @@
             break;
         }
         case 0xE: {    //corner or plate      1110
+            blue.printf("1110\n");
             set_direction(0x00, 0.0, 0.0);
             wait(1);
             break;
         }
         case 0xF: {    //corner               1111
+            blue.printf("1111\n");
             set_direction(0x11, 0.0, 0.0);
             wait(1);
             break;
@@ -223,36 +239,43 @@
     motor_lb=0;
 
     while(1) {
-        /* NOT WORKING?!?!?! 
-        
-        led = !led;
-        set_direction(0x11, 1.0,1.0);//forward full speed
-        wait(4);
-        led = !led;
-        set_direction(0x11, 0.5 ,0); //forward left half
-        wait(4);
-        led = !led;
-        set_direction(0x11, 0 ,0.5); //forward right half
-        wait(4);
-        led = !led;
-        set_direction(0x00, 0 ,0); //stop
-        wait(4);
-        led = !led;
-        set_direction(0x00, 1.0 ,1.0); //backward full speed
-        wait(4);
-        led = !led;
-        set_direction(0x00, 0.5 ,0);
-        wait(4);
-        led = !led;
-        set_direction(0x00, 0 ,0.5);
-        
+        //Working
+        /*
+                led = !led;
+                wait(4);
+                blue.printf("Forward, full\n");
+                set_direction(0x11, 1.0,1.0);//forward full speed
+                wait(4);
+                blue.printf("Forward, left half\n");
+                led = !led;
+                set_direction(0x11, 0.5 ,0); //forward left half
+                wait(4);
+                blue.printf("Forward, right half\n");
+                led = !led;
+                set_direction(0x11, 0 ,0.5); //forward right half
+                wait(4);
+                led = !led;
+                blue.printf("stop\n");
+                set_direction(0x00, 0 ,0); //stop
+                wait(4);
+                led = !led;
+                blue.printf("Back, full\n");
+                set_direction(0x00, 1.0 ,1.0); //backward full speed
+                wait(4);
+                led = !led;
+                blue.printf("Back, left\n");
+                set_direction(0x00, 0.5 ,0);
+                wait(4);
+                blue.printf("Back, right\n");
+                led = !led;
+                set_direction(0x00, 0 ,0.5);
+
         */
-        
+
         //Sensor Code + Motor Test
-        //int values = 0;
-        //values = sensor_read();
-
-        //change_direction(values);
+        int values = 0;
+        values = sensor_read();
+        change_direction(values);
 
     }
 }