Diff: main.cpp

Revision:

2:e2b6fe03e630

Parent:

1:30e01a866efa

--- a/main.cpp	Wed Apr 22 21:36:17 2015 +0000
+++ b/main.cpp	Mon Apr 27 15:33:02 2015 +0000
@@ -4,7 +4,11 @@
 #include "motordriver.h"
 
 //#define DEBUG
+//************************************************************
 
+//Declarations
+
+//************************************************************
 DigitalOut led1(LED1);
 DigitalOut led2(LED2);
 DigitalOut led3(LED3);
@@ -12,23 +16,30 @@
 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 countR = 0;//global encoder count variables
 int countL = 0;
 
 
-Serial pc(USBTX, USBRX);//PC serial for debug
+//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
+char debugcommand[] = "FS";//direction commands for debug sequence, can be used without xbee controll
 //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;
+char command = 'W'; // switch case variable to be used for commands from wireless comm, intialize W for wait
+bool collision_flag = 0;//flag to set high for collision handling
+bool once = 0;//send collision feedback once
 
 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
+
+bool x = true;//loop variable
+//************************************************************
+
+//Helper Functions
+
+//************************************************************
+//Functions for updating/reading encoder with interrupts
 void Rcount() //count Right encoder increments
 {
     countR++;
@@ -42,30 +53,23 @@
     countR = 0;
     countL = 0;
 }
+//************************************************************
 
+//Main
+
+//************************************************************
 
 int main()
 {
     float sl = 0.65;//preset wheel speed left
-    float sr = 0.65*0.92;//preset wheel speed right
-    //************************************************************
-
+    float sr = 0.65*0.92;//preset wheel speed right, account for wobble of axel
 
-
-    //************************************************************
-    REN.fall(&Rcount);
+    REN.fall(&Rcount);//attach counting functions for encoder interrupts
     LEN.fall(&Lcount);
 
-    led1 = 1;
-    led2 = 1;
-    led3 = 1;
-    led4 = 1;
-
-    wait(7);
-    led1 = 0;
-    led2 = 0;
-    led3 = 0;
-    led4 = 0;
+    led1 = 1;led2 = 1;led3 = 1;led4 = 1;
+    wait(5);//setup period
+    led1 = 0;led2 = 0;led3 = 0;led4 = 0;
 
     bool x = true;//loop variable
 
@@ -73,109 +77,103 @@
     while (x) {//program loop
 
         //************************************************************
-        if(IR >=0.60) {
-            left.stop(1);
+        if((IR >=0.60) && (collision_flag == 0)) {//if object sensed and flag not already raised
+            left.stop(1);//stop if flag not raised
             right.stop(1);
-            collision_flag = 1;
+            collision_flag = 1;//raise flag
         }
-        if(collision_flag) {
+        if(IR < 0.60) {//if no object sensed, reset flags
+            collision_flag = 0;
+            once = 0;
+        }
+        if((collision_flag && !(once))) {//flag high and feeback not yet sense
             xbee1.putc('C');
-            pc.printf("C\n\r");
-            collision_flag = 0;
+            //pc.printf("C\n\r");
+            once = 1;
         }
-        if(xbee1.readable()) {
+        if(xbee1.readable()) {//if new command readable
             command = xbee1.getc();
-            pc.printf("Command: %c\n\r",command);
+            //pc.printf("Command: %c\n\r",command);
         }
-
         //************************************************************
 
         switch ( command ) {//switch statement for robot motor control
             case 'F': //move forward
-                pc.printf("FORWARD\n\r");
-                led2 = 1;
-                led3 = 1;
+                //pc.printf("FORWARD\n\r");
+                led2 = 1;led3 = 1;//middle indicator lights
                 clearEN();
-                while ((countL <= 500)||(countR <=340)) {//loop specified distance
+                while ((countL <= 500)||(countR <=340)) {//loop specified distance, axel wobble led to different encoder increment rates
                     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()) {
+                    if(xbee1.readable()) {// if new command while going forward
                         command = xbee1.getc();
-                        pc.printf("Command: %c\n\r",command);
+                        //pc.printf("Command: %c\n\r",command);
                         break;
                     }
+                    //pc.printf("Speed set\n\r");
                     left.speed(sl);
-                    right.speed(sr);//drift factor associated with our build
-                    pc.printf("RIGHT: %d, LEFT: %d\n\r", countR, countL);
+                    right.speed(0.69);//drift factor associated with our build
+                    //pc.printf("RIGHT: %d, LEFT: %d\n\r", countR, countL);
                     wait(.01);
                 }
-                led2 = 0;
-                led3 = 0;
+                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;
+            case 'R': //turn ~180 degrees right
+                //pc.printf("RIGHT\n\r");
+                led1 = 1;led2 = 1;
                 clearEN();
-                while ((countL <= 80)) {//loop turn for specified angle
+                left.speed(sl);right.speed(-sr);
+                while ((countL <= 40)) { //countR <= 20)) {//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()) {
+                    //right.speed(-sr);
+                    if(xbee1.readable()) {//if new command
                         command = xbee1.getc();
-                        pc.printf("Command: %c\n\r",command);
+                        //pc.printf("Command: %c\n\r",command);
                         break;
                     }
                 }
-                led1 = 0;
-                led2 = 0;
+                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;
-
+            case 'L': //turn 30 deg left
+                //pc.printf("LEFT\n\r");
+                led3 = 1;led4 = 1;
                 clearEN();
                 //loop turn for specified angle
-                while (( countR<=55)) {
+                while ((countR<=55)) {
                     left.speed(-sl);
                     right.speed(sr);
                     if(xbee1.readable()) {
                         command = xbee1.getc();
-                        pc.printf("Command: %c\n\r",command);
+                        //pc.printf("Command: %c\n\r",command);
                         break;
                     }
                 }
-                led3 = 0;
-                led4 = 0;
+                led3 = 0;led4 = 0;
                 left.stop(1);
                 right.stop(1);
                 command = 'W';
                 break;
 
-            case 'B': //turn 180 deg left
+            case 'B': //bump robot backwards
                 pc.printf("BACKWARDS\n\r");
-                led1 = 1;
-                led4 = 1;
+                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;
+                led1 = 0;led4 = 0;
                 left.stop(1);
                 right.stop(1);
                 command = 'W';
@@ -186,7 +184,7 @@
                 wait(.5);
                 // Code
 #ifdef DEBUG
-                /*num = rand()%3;
+                /*num = rand()%3;//randomization code, will run different commands indefinetly
                 switch( num ) {
                     case 1:
                         command = 'f';
@@ -201,26 +199,22 @@
                         command = 'b';
                         break;
                 }*/
-                command = debugcommand[count];
+                command = debugcommand[count];//parse through debug command array
                 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;
+                led1 = 1;led4 = 1;
                 wait(.5);
-                led2 = 1;
-                led3 = 1;
-                led1 =0;
-                led4  = 0;
+                led2 = 1;led3 = 1;
+                led1 =0;led4  = 0;
                 wait(.5);
-                led2 = 0;
-                led3 = 0;
+                led2 = 0;led3 = 0;
                 break;
         }
     }