Diff: main.cpp

Revision:

10:2970279fce70

Parent:

9:92895704e1a4

Child:

22:2475678363d5

--- a/main.cpp	Thu Mar 12 14:44:56 2015 +0000
+++ b/main.cpp	Fri Mar 13 10:20:18 2015 +0000
@@ -1,93 +1,30 @@
 #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
 
 //PWM pin (Enable 1 and 2)
-PwmOut motor_l (PTC2);
-PwmOut motor_r (PTE29);
+PwmOut motor_r (PTC2);
+PwmOut motor_l (PTE29);
 
 //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);
-
-        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;
-}
-
+//Bluetooth Module to debug
+Serial blue(PTC4,PTC3);
 
 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)
+    blue.printf("I've been given %i, %f, %f\n", direction, duty_l, duty_r);
     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 +32,8 @@
             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\n", duty_r, duty_l);
+            break;
         }
         case 0x00: { //backward
             motor_r.write( duty_r);
@@ -104,6 +43,9 @@
             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\n", duty_r, duty_l);
+            break;
+
         }
         case 0x01: {  //spin left  --     Right forward, left backward
             motor_r.write( duty_r);
@@ -113,6 +55,9 @@
             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\n", duty_r, duty_l);
+            break;
+
         }
         case 0x10: {    //spin right
             motor_r.write( duty_r);
@@ -122,97 +67,76 @@
             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\n", duty_r, duty_l);
+            break;
+
         }
     }
 }
 
-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(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;
+void manual_mode(){
+    int direction = 0x11;
+    unsigned char rem_dir; //remote_direction
+
+    while(1) {
+
+
+        wait(1);
+        blue.printf("Gees a Direction\n f for forward both, b for backward both, r for spin right (clockwise), l for spin left (anti-clockwise) .. etc. \nDON'T PUT IT IN UPPERCASE PLS\n");
+
+        while ( blue.readable()==0 ) {}               // loops until there is an input from the user
+
+        //for(int i=0; i<2; i++) {
+        rem_dir = blue.getc();    //Store incoming response into array
+        //}
+
+        blue.printf("Thanks, calculating it now, you entered %c\n",rem_dir);
+        if(   ( rem_dir == 'f')   ) {
+            direction = 0x11;
+            blue.printf("You wanted to go straight\n");
         }
-        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;
+        if (   (rem_dir == 'b' )     ) {
+
+            direction = 0x00;
+            blue.printf("You wanted to go backwards\n");
         }
-        case 0x7: {//corner junction or plate 0111
-            set_direction(0x00, 0.0, 0.0);
-            wait(1);
-            break;
+        if (   (rem_dir == 'l' )   ) {
+
+            direction = 0x01;
+            blue.printf("You wanted to spin anti-clockwise\n");
+
         }
-        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;
+        if (   (rem_dir == 'r' )   )  {
+
+            direction = 0x10;
+            blue.printf("You wanted to spin clockwise\n");
         }
-        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;
-        }
+         if (rem_dir =='s') {
+
+            motor_r.write(0);
+            motor_l.write(0);
+            blue.printf("STAAAAHP\n");
+        }else{
+            wait(0.1);
+        blue.printf("What duty left?\n");
+        while ( blue.readable()==0 ) {}               // loops until there is an input from the user
+        char l_speed = blue.getc();
+        wait(0.1);
+        blue.printf("What duty right?\n");
+        while ( blue.readable()==0 ) {}               // loops until there is an input from the user
+        char r_speed = blue.getc();
+        wait(0.1);
+        int l_result = l_speed - '0';
+        int r_result = r_speed - '0';
+        set_direction(direction, (float)l_result/10, (float)r_result/10);
+       }
+        
+
     }
-}
+    
+    }
 int main()
 {
-    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);
@@ -221,43 +145,146 @@
     motor_rb=0;
     motor_lf=0;
     motor_lb=0;
+    while(1){
+        manual_mode();
+        }
 
-    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();
+    /* //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);
 
-        //change_direction(values);
+          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);
+      }*/
 
 
+
+}