Diff: main.cpp

Revision:

17:54da4359134f

Parent:

15:3f6626f68836

Child:

18:6e8eae661cf3

diff -r 3f6626f68836 -r 54da4359134f main.cpp
--- a/main.cpp	Thu Mar 19 13:40:00 2015 +0000
+++ b/main.cpp	Fri Mar 20 16:59:28 2015 +0000
@@ -1,5 +1,6 @@
 #include "mbed.h"
-
+#include <string>
+using namespace std;
 Serial blue(PTC4,PTC3);
 
 //motor select pins
@@ -41,7 +42,7 @@
             motor_rb=0;
             motor_lf=1;
             motor_lb=0;
-            blue.printf("Going forward, right:%f ; left:%f\n", duty_r, duty_l);
+            //   blue.printf("Going forward, right:%f ; left:%f\n", duty_r, duty_l);
             break;
         }
         case 0x00: { //backward
@@ -52,7 +53,7 @@
             motor_rb=1;
             motor_lf=0;
             motor_lb=1;
-            blue.printf("Going backward, right:%f ; left:%f\n", duty_r, duty_l);
+            //   blue.printf("Going backward, right:%f ; left:%f\n", duty_r, duty_l);
             break;
         }
         case 0x01: {  //spin left  --     Right forward, left backward
@@ -63,7 +64,7 @@
             motor_rb=0;
             motor_lf=0;
             motor_lb=1;
-            blue.printf("Spinning Left, right:%f ; left:%f\n", duty_r, duty_l);
+            //     blue.printf("Spinning Left, right:%f ; left:%f\n", duty_r, duty_l);
             break;
         }
         case 0x10: {    //spin right
@@ -74,40 +75,55 @@
             motor_rb=1;
             motor_lf=1;
             motor_lb=0;
-            blue.printf("Spinning Right, right:%f ; left:%f\n", duty_r, duty_l);
+            //   blue.printf("Spinning Right, right:%f ; left:%f\n", duty_r, duty_l);
             break;
         }
     }
 }
 
-
 void motor_result(int val)
 {
+
     led= 0;
     switch(val) {
         case 0: //lost
-
+            //  blue.printf("0000\n");
             set_direction(0x00, 0,0);
             break;
         case 1:
-
-            set_direction(0x11, 0.5,0.6);
+            //  blue.printf("0001\n");
+            set_direction(0x11, 0.3,0.1);
             break;
         case 2:
-
+            // blue.printf("0010\n");
+            set_direction(0x11, 0.3,0.2);
             break;
         case 4:
-
+            // blue.printf("0100\n");
+            set_direction(0x11, 0.2,0.3);
             break;
         case 6:
-            set_direction(0x11, 0.5,0.5);
-
+            //  blue.printf("0110\n");
+            set_direction(0x11, 0.3,0.3);
+            break;
+        case 12:
+            //   blue.printf("1100\n");
+            set_direction(0x11, 0.2,0.3);
+            break;
+        case 3:
+            // blue.printf("0011\n");
+            set_direction(0x11, 0.3,0.2);
             break;
         case 8:
+            //blue.printf("1000\n");
+            set_direction(0x11, 0.1,0.3);
+            break;
+        default:
+            //blue.printf("%i\n", val);
+            set_direction(0x00, 0,0);
+    }
 
-            set_direction(0x11, 0.6,0.5);
-            break;
-    }
+
 }
 int sensor_read()
 {
@@ -158,7 +174,7 @@
 }
 int main()
 {
-    level = 0.3; //Analogout Level for black line into comparator
+    level = 0.58; //Analogout Level for black line into comparator
 //Set PWM frequency to 1000Hz
     motor_l.period( 1.0f / (float) PWM_FREQ);
     motor_r.period( 1.0f / (float) PWM_FREQ);
@@ -168,9 +184,26 @@
     motor_lf=0;
     motor_lb=0;
     int val =0;
+    int old_val=0;
     while(1) {
+        old_val = val;
         val = sensor_read();
-        motor_result(val);
+        if(!(old_val == val)) {
+            string binary = "";
+            int mask = 1;
+            for(int i = 0; i < 4; i++) {
+                if((mask&val) >= 1)         binary = "1"+binary;
+                else                        binary = "0"+binary;
+                mask = mask << 1;
+            }
+            blue.printf("%s\n", binary);
+            wait(0.2);
+            motor_result(val);
+        }
+
+        // if(!(old_val == val)) {
+        //     motor_result(val);
+        // }
     }
     // while(1) {
     //Working