Diff: main.cpp

Revision:

1:30e01a866efa

Parent:

0:5c26b3940003

Child:

2:e2b6fe03e630

--- a/main.cpp	Wed Oct 26 01:12:51 2011 +0000
+++ b/main.cpp	Wed Apr 22 21:36:17 2015 +0000
@@ -1,28 +1,227 @@
-// Magician robot motor test
+// ECE 4180 Gesture Sensor Controlled Robot Project: Robot Comm, Heading and Motor Control, Obstacle Detection
 
 #include "mbed.h"
 #include "motordriver.h"
- 
-DigitalOut myled(LED1);
+
+//#define DEBUG
 
-//See http://mbed.org/cookbook/Motor
-//Connections to dual H-brdige driver for the two drive motors
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+AnalogIn IR(p20);//Input for IR sensor, 0.60 seems like a good object detect distance
+InterruptIn REN(p15);//INTERRUPT IN PIN FOR RIGHT ENCODER
+InterruptIn LEN(p16);//INTERRUPT IN PIN FOR LEFT ENCODER
+int countR = 0;
+int countL = 0;
+
+
+Serial pc(USBTX, USBRX);//PC serial for debug
+Serial xbee1(p13, p14);//xbee serial connection
+
+int count = 0; //counter for reading through debug command array;
+char debugcommand[] = "FRFRFRFRS";//direction commands for debug: forward, right, forward, back, forward, left, forward, stop
+//int num = 0;
+char command = 'W'; // switch case variable to be used for commands from wireless comm, default w for wait
+bool collision_flag = 0;
+
 Motor  left(p21, p22, p23, 1); // pwm, fwd, rev, has brake feature
 Motor right(p26, p25, p24, 1);
+bool x = true;
+//Functions for updating/reading encoder
+void Rcount() //count Right encoder increments
+{
+    countR++;
+}
+void Lcount() //count Left encoder increments
+{
+    countL++;
+}
+void clearEN() //clear the encoder values
+{
+    countR = 0;
+    countL = 0;
+}
 
-int main() {
-    while (1) {
-        myled=1;
-// Sweep the motor speed from full-speed reverse (-1.0) to full speed forwards (1.0)
-        for (float s= -1.0; s < 1.0 ; s += 0.01) {
-            left.speed(s);
-            right.speed(s);
-            wait(0.02);
+
+int main()
+{
+    float sl = 0.65;//preset wheel speed left
+    float sr = 0.65*0.92;//preset wheel speed right
+    //************************************************************
+
+
+
+    //************************************************************
+    REN.fall(&Rcount);
+    LEN.fall(&Lcount);
+
+    led1 = 1;
+    led2 = 1;
+    led3 = 1;
+    led4 = 1;
+
+    wait(7);
+    led1 = 0;
+    led2 = 0;
+    led3 = 0;
+    led4 = 0;
+
+    bool x = true;//loop variable
+
+
+    while (x) {//program loop
+
+        //************************************************************
+        if(IR >=0.60) {
+            left.stop(1);
+            right.stop(1);
+            collision_flag = 1;
+        }
+        if(collision_flag) {
+            xbee1.putc('C');
+            pc.printf("C\n\r");
+            collision_flag = 0;
+        }
+        if(xbee1.readable()) {
+            command = xbee1.getc();
+            pc.printf("Command: %c\n\r",command);
         }
-// Turn off motors - coast one and brake one
-        left.coast();
-        right.stop(1);
-        myled=0;
-        wait(1);
+
+        //************************************************************
+
+        switch ( command ) {//switch statement for robot motor control
+            case 'F': //move forward
+                pc.printf("FORWARD\n\r");
+                led2 = 1;
+                led3 = 1;
+                clearEN();
+                while ((countL <= 500)||(countR <=340)) {//loop specified distance
+                    if(IR >=0.60) {
+                        left.stop(1);
+                        right.stop(1);
+                        collision_flag = 1;
+                        break;
+                    }   //if IR has detected a close object, stop
+                    if(xbee1.readable()) {
+                        command = xbee1.getc();
+                        pc.printf("Command: %c\n\r",command);
+                        break;
+                    }
+                    left.speed(sl);
+                    right.speed(sr);//drift factor associated with our build
+                    pc.printf("RIGHT: %d, LEFT: %d\n\r", countR, countL);
+                    wait(.01);
+                }
+                led2 = 0;
+                led3 = 0;
+                left.stop(1);
+                right.stop(1);
+                command = 'W';
+                break;
+
+            case 'R': //turn 90 deg right
+                pc.printf("RIGHT\n\r");
+                led1 = 1;
+                led2 = 1;
+                clearEN();
+                while ((countL <= 80)) {//loop turn for specified angle
+                    //pc.printf("RIGHT: %d, LEFT: %d\n\r", countR, countL);
+                    left.speed(sl);
+                    right.speed(-sr);//right.speed(-sr);
+                    if(xbee1.readable()) {
+                        command = xbee1.getc();
+                        pc.printf("Command: %c\n\r",command);
+                        break;
+                    }
+                }
+                led1 = 0;
+                led2 = 0;
+                left.stop(1);
+                right.stop(1);
+                command = 'W';
+                //pc.printf("END OF COMMAND:: RIGHT: %d, LEFT: %d\n\r", countR, countL);
+                break;
+
+            case 'L': //turn 90 deg left
+                pc.printf("LEFT\n\r");
+                led3 = 1;
+                led4 = 1;
+
+                clearEN();
+                //loop turn for specified angle
+                while (( countR<=55)) {
+                    left.speed(-sl);
+                    right.speed(sr);
+                    if(xbee1.readable()) {
+                        command = xbee1.getc();
+                        pc.printf("Command: %c\n\r",command);
+                        break;
+                    }
+                }
+                led3 = 0;
+                led4 = 0;
+                left.stop(1);
+                right.stop(1);
+                command = 'W';
+                break;
+
+            case 'B': //turn 180 deg left
+                pc.printf("BACKWARDS\n\r");
+                led1 = 1;
+                led4 = 1;
+                left.speed(-sl);
+                right.speed(-sr);
+                wait(.5);//bump robot backwards by reversing both wheels for half a second
+
+                led1 = 0;
+                led4 = 0;
+                left.stop(1);
+                right.stop(1);
+                command = 'W';
+                break;
+
+            case 'W': //wait for a new command
+                pc.printf("WAIT\n\r");
+                wait(.5);
+                // Code
+#ifdef DEBUG
+                /*num = rand()%3;
+                switch( num ) {
+                    case 1:
+                        command = 'f';
+                        break;
+                    case 2:
+                        command = 'r';
+                        break;
+                    case 3:
+                        command = 'l';
+                        break;
+                    default:
+                        command = 'b';
+                        break;
+                }*/
+                command = debugcommand[count];
+                count++;
+                pc.printf("NEW COMMAND: %c", command);
+#endif
+                command = 'W';
+                break;
+            default: //wait and show error
+                pc.printf("DEFAULT\n\r");
+                left.stop(0);
+                right.stop(0);
+                led1 = 1;
+                led4 = 1;
+                wait(.5);
+                led2 = 1;
+                led3 = 1;
+                led1 =0;
+                led4  = 0;
+                wait(.5);
+                led2 = 0;
+                led3 = 0;
+                break;
+        }
     }
-}
+}
\ No newline at end of file