Diff: main.cpp

Revision:

9:92895704e1a4

Parent:

8:eefbd3880d28

Child:

10:2970279fce70

Child:

11:f07662c64e26

--- a/main.cpp	Thu Mar 12 12:18:25 2015 +0000
+++ b/main.cpp	Thu Mar 12 14:44:56 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,119 +27,193 @@
 //LED to test
 DigitalOut led(LED_RED);
 
-//Hall Effect Sensors
-InterruptIn hall_l (PTA1); //Hall Effect Sensor Output RobotLeft
-InterruptIn hall_r (PTA2); //Hall Effect Sensor Output RobotRight
-float time1_l, time2_l, time3_l, time1_r, time2_r, time3_r;
-//Bluetooth Module to debug
-Serial blue(PTC4,PTC3);
-
-void set_direction( int direction, float speed, float angle)
+void sensor_select(int x)//converts int to hex for display
 {
-    /* *******************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)
+    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;
+    }
 
-                 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)
+}
+int sensor_read()
+{
+    int val=0;
+    int x = 0;
 
-                 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%)
+    //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);
 
-                 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
+        if (input == 1) {
+            val=val+2^x; //could you comment on what this is for?
+        } else {
+            val=val;//+2^x;
+        }
+    }
+    blue.printf("%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)
+
+   // 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(  r_result  < 0 ? 0    :     r_result > 1 ? 1    :       r_result);
+            motor_r.write(  duty_r);
             //Look up ternary statements -- {condition ? value_if_true : value_if_false} This makes 0<result<1
-            motor_l.write(  l_result < 0 ? 0     :     l_result > 1 ? 1    :       l_result);
+            motor_l.write( duty_l);
 
             motor_rf=1;
             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);
-            motor_l.write( l_result  < 0 ? 0    :     l_result > 1 ? 1    :       l_result);
+            motor_r.write( duty_r);
+            motor_l.write( duty_l);
 
             motor_rf=0;
             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);
-            motor_l.write( l_result  < 0 ? 0    :     l_result > 1 ? 1    :       l_result);
+            motor_r.write( duty_r);
+            motor_l.write( duty_l);
 
             motor_rf=1;
             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);
-            motor_l.write( l_result  < 0 ? 0    :     l_result > 1 ? 1    :       l_result);
+            motor_r.write( duty_r);
+            motor_l.write( duty_l);
 
             motor_rf=0;
             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);
-
         }
     }
 }
-timer h_r, h_l;
-float hall_rt1, hall_lt1, hall_rt2, hall_lt2//hall right time & left time
-int h_f_r, h_f_l //hallflagright and left
-void hallr() //this just reading the various times.
+
+void change_direction( int sensor_values) //right hand sensors only
 {
-    if (h_f_r==2) {
-        start.h_r();
-    } else if(h_f_r == 1) {
-        hall_rt1 = read.h_r();
-        h_f_r = 0;
-    } else if (h_f_l == 0) {
-        hall_rt2 = read.h_r();
-        h_f_r = 1;
+    //Line_right = !Line_right;
+    switch( sensor_values ) {
+        case 0x00: { //no lines robot is lost!!!!
+            set_direction(0x00, 1.0, 1.0);
+            wait(1);
+            break;
+        }
+        case 0x01: { //to far left turn right 0001
+            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
+            set_direction(0x11, 0, 0.5);
+            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.5, 0.0);
+            wait(1);
+            break;
+        }
+        case 0x5: {    //unknown **           0101
+            set_direction(0x11, 0.0, 0.0);
+            break;
+        }
+        case 0x6: {    //perfect              0110
+            set_direction(0x00, 1.0, 1.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, 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
+            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
+            set_direction(0x00, 0.0, 0.0);
+            wait(1);
+            break;
+        }
+        case 0xF: {    //corner               1111
+            set_direction(0x11, 0.0, 0.0);
+            wait(1);
+            break;
+        }
     }
-
-}
-void halll()
-{
-
-
 }
 int main()
 {
-    h_f_r = 2;
-    h_f_l = 2;
-    //attaching hall_r and hall_r so that it calls hall() on a rising and falling edge
-    hall_r.rise(&hallr());
-    hall_r.fall(&hallr());
-
-    hall_l.rise(&halll());
-    hall_l.fall(&halll());
+    level = 0.3; //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);
 //Initialise direction to nothing.
@@ -136,202 +221,43 @@
     motor_rb=0;
     motor_lf=0;
     motor_lb=0;
-    int direction = 0x11;
-    char rem_dir[2]; //remote_direction
 
+    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);
+        
+        */
+        
+        //Sensor Code + Motor Test
+        //int values = 0;
+        //values = sensor_read();
+
+        //change_direction(values);
+
+    }
+}
 
 
 
 
 
-    /*BLUETOOTH TEST
-
-    while(1) {
-
-
-        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
-        }
-
-        //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')   ) {
-
-            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);
-
-        /* 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;
-        */
-
-    //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
-
-    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
-
-      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);
-      }*/
-
-
-
-}