Diff: main.cpp

Revision:

15:3f6626f68836

Parent:

11:f07662c64e26

Child:

17:54da4359134f

--- a/main.cpp	Fri Mar 13 10:55:39 2015 +0000
+++ b/main.cpp	Thu Mar 19 13:40:00 2015 +0000
@@ -10,7 +10,7 @@
 
 
 DigitalIn input(PTC1);  //input from sensor array
-DigitalOut Line_right(LED_GREEN);//no line detected
+//DigitalOut Line_right(LED_GREEN);//no line detected
 //DigitalOut blue(LED_BLUE);
 
 BusOut sensor(PTD7,PTE1,PTE0);//multiplex sensors
@@ -27,57 +27,6 @@
 //LED to test
 DigitalOut led(LED_RED);
 
-void sensor_select(int x)//converts int to hex for display
-{
-    switch(x) {
-        case 0:
-            sensor = 0x2;
-            break;
-        case 1:
-            sensor = 0x1;
-            break;
-        case 2:
-            sensor = 0x0;
-            break;
-        case 3:
-            sensor = 0x3;
-            break;
-        case 4:
-            sensor = 0x5;
-            break;
-        case 5:
-            sensor = 0x7;
-            break;
-        case 6:
-            sensor = 0x6;
-            break;
-        case 7:
-            sensor = 0x4;
-            break;
-    }
-
-}
-int sensor_read()
-{
-    int val=0;
-    int x = 0;
-
-    //static int values[8];
-    for(x = 0; x <= 3; x++ ) { //why 3? Don't you want to select through 8 sensors therefore x<=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?
-        } else {
-            val=val;//+2^x;
-        }
-    }
-    blue.printf("\nVAL : %i\n",val);
-    return val;
-}
 
 
 void set_direction( int direction, float duty_l, float duty_r)
@@ -131,100 +80,81 @@
     }
 }
 
-void change_direction( int sensor_values) //right hand sensors only
+
+void motor_result(int val)
 {
-    //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);
+    led= 0;
+    switch(val) {
+        case 0: //lost
+
+            set_direction(0x00, 0,0);
+            break;
+        case 1:
+
+            set_direction(0x11, 0.5,0.6);
             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;
+        case 2:
+
+            break;
+        case 4:
+
             break;
-        }
-        case 0x02: {  //left turn right       0010
-            blue.printf("0010\n");
-            set_direction(0x11, 0, 0.5);
-            wait(1);
+        case 6:
+            set_direction(0x11, 0.5,0.5);
+
+            break;
+        case 8:
+
+            set_direction(0x11, 0.6,0.5);
             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;
-        }
-        case 0x5: {    //unknown **           0101
-            set_direction(0x11, 0.0, 0.0);
-            break;
-        }
-        case 0x6: {    //perfect              0110
-            blue.printf("perfect, 0110\n");
-            set_direction(0x00, 1.0, 1.0);
-            wait(1);
-            break;
+    }
+}
+int sensor_read()
+{
+    int val=0;
+    int x = 0;
+    int sens[4] = {0};
+    for (x = 0; x < 4; x++) {
+        switch(x) {
+            case 0:
+                sensor = 0x3;
+                break;
+            case 1:
+                sensor = 0x0;
+                break;
+            case 2:
+                sensor = 0x1;
+                break;
+            case 3:
+                sensor = 0x2;
+                break;
+            case 4:
+                sensor = 0x5;
+                break;
+            case 5:
+                sensor = 0x7;
+                break;
+            case 6:
+                sensor = 0x6;
+                break;
+            case 7:
+                sensor = 0x4;
+                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;
-        }
-        case 0x9: {    //unknown              1001
-            set_direction(0x11, 0.0, 0.0);
-            wait(1);
-            break;
-        }
-        case 0xA: {    //unknown              1010
-            set_direction(0x11, 0.0, 0.0);
-            break;
-        }
-        case 0xB: {    //unknown              1011
-            set_direction(0x11, 0.0, 0.0);
-            wait(1);
-            break;
-        }
-        case 0xC: { //far right turn left     1100
-            blue.printf("1100\n");
-            set_direction(0x00, 0.0, 0.0);
-            wait(1);
-            break;
-        }
-        case 0xD: {    //unknown              1101
-            set_direction(0x11, 0.0, 0.0);
-            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;
-        }
+
+        sens[x] = input;
+        //blue = !blue;
+
+
+
     }
+    for(x = 3; x >= 0; x--) {
+        val = val << 1;
+        val = val + sens[x];
+    }
+    //blue.printf("%i\n",val);
+
+    return val;
 }
 int main()
 {
@@ -237,47 +167,51 @@
     motor_rb=0;
     motor_lf=0;
     motor_lb=0;
-
+    int val =0;
     while(1) {
-        //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);
+        val = sensor_read();
+        motor_result(val);
+    }
+    // while(1) {
+    //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);
+    //Sensor Code + Motor Test
+    //int values = 0;
+    // values = sensor_read();
+    // change_direction(values);
 
-    }
+    //}
 }