Diff: main.cpp

Revision:

5:22d7fee2e26e

Parent:

4:6db8e0babea7

Child:

6:dd0a91c2994f

--- a/main.cpp	Fri Mar 06 15:18:14 2015 +0000
+++ b/main.cpp	Wed Mar 11 18:00:33 2015 +0000
@@ -1,11 +1,22 @@
 #include "mbed.h"
 
+Serial blue(PTC4,PTC3);
+
 //motor select pins
 DigitalOut motor_lf(PTE2);
 DigitalOut motor_lb(PTE3);
 DigitalOut motor_rf(PTE4);
 DigitalOut motor_rb(PTE5);
 
+
+DigitalIn input(PTC1);  //input from sensor array
+DigitalOut Line_right(LED_GREEN);//no line detected
+//DigitalOut blue(LED_BLUE);
+
+BusOut sensor(PTD7,PTE1,PTE0);//multiplex sensors
+AnalogOut level(PTE30);//set comparator level needs to be tuned for each sensor (could create program)
+
+
 //Frequency of Pulse Width Modulated signal in Hz
 #define PWM_FREQ          1000
 
@@ -16,28 +27,64 @@
 //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;
+    level = 0.3;
+    int x = 0;
+
+    //static int values[8];
+    while( x <= 3 ) {
+        sensor_select(x);
+        //blue = !blue;
+        wait(0.1);
+        
+        if (input == 1) {
+            val=val+2^x;
+        } else {
+            val=val;//+2^x;
+        }
+        x=x+1;
+        
+    }
+    blue.printf("%i\n",val);
+    return val;
+}
+
+
 void set_direction( int direction, float speed, float angle)
 {
-    /* *******************Example Use*****************
-                 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_direction("00", 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("11", 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("11", -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
-
-    */
+   
 
     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)
@@ -81,6 +128,91 @@
         }
     }
 }
+
+void change_direction( int sensor_values) //right hand sensors only
+{
+    //Line_right = !Line_right;
+    switch( sensor_values ) {
+        case 0x00: { //no lines robot is lost!!!!
+            set_direction(0x11, 0.0, 0.0);
+            wait(1);
+            break;
+        }
+        case 0x01: { //to far left turn right 0001
+            set_direction(0x11, 0.5, 0.0);
+            wait(1);
+            Line_right = !Line_right;
+            break;
+        }
+        case 0x02: {  //left turn right       0010
+            set_direction(0x11, 0.0, 0.0);
+            wait(1);
+            break;
+        }
+        case 0x03: {  //far left turn right   0011
+            set_direction(0x11, 0.0, 0.0);
+            wait(1);
+            break;
+        }
+        case 0x4: {    //right turn left      0100
+            set_direction(0x11, 0.0, 0.0);
+            wait(1);
+            break;
+        }
+        case 0x5: {    //unknown **           0101
+            set_direction(0x11, 0.0, 0.0);
+            break;
+        }
+        case 0x6: {    //perfect              0110
+            set_direction(0x11, 0.0, 0.0);
+            wait(1);
+            break;
+        }
+        case 0x7: {//corner junction or plate 0111
+            set_direction(0x00, 0.0, 0.0);
+            wait(1);
+            break;
+        }
+        case 0x8: {//to far right turn left   1000
+            set_direction(0x11, 0.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
+            set_direction(0x11, 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
+            set_direction(0x00, 0.0, 0.0);
+            wait(1);
+            break;
+        }
+        case 0xF: {    //corner               1111
+            set_direction(0x11, 0.0, 0.0);
+            wait(1);
+            break;
+        }
+    }
+}
 int main()
 {
 //Set PWM frequency to 1000Hz
@@ -91,144 +223,20 @@
     motor_rb=0;
     motor_lf=0;
     motor_lb=0;
+    
 
     while(1) {
-        
-        wait(2);
         led = !led;
-        set_direction(0x11, 0.5, 0);  //This would set the robot to go forward, at half speed, straight
+        int values;
         
-        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%
+        values = sensor_read();
         
-        wait(2);
-        led = !led;
-        set_direction(0x10, 0.5, 0.3); //robot spins to the right, 80% right forward ; 20% left backward
-
+        change_direction(values);
+        
     }
-
-
-  /*  // 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_rf=1;
-        motor_rb=0;
-        motor_lf=1;
-        motor_lb=0; //go forwards
-        wait(2);
-        //**************************************
-        myled = !myled;
-
-        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;
-
-        motor_r.write(0.5);
-        motor_l.write(0.5);
-
-        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=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);
-
-        motor_rf=0;
-        motor_rb=0;
-        motor_lf=0;
-        motor_lb=1; //
-
-        wait(2);
-        myled = !myled;
-
-        motor_rf=0;
-        motor_rb=1;
-        motor_lf=0;
-        motor_lb=0; //reverse turn
-
-        wait(2);
-
-        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);
-    }*/
-        
-
-
-}