Diff: main.cpp

Revision:

0:178a07cd3e39

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Dec 16 03:45:52 2011 +0000
@@ -0,0 +1,213 @@
+#include "mbed.h"
+#include "bees.h"
+
+#define DEBUG 1
+#define V_PER_IN 0.0098 // mV/in
+
+#ifdef DEBUG
+Serial pc(USBTX, USBRX); // tx, rx
+#endif
+
+DigitalOut myled(LED1);
+
+AnalogIn srf(p20);
+Serial xbee(p9, p10);
+DigitalOut xbee_reset(p11);
+
+
+///
+AnalogIn r_encoder(p15);
+AnalogIn l_encoder(p16);
+
+DigitalOut r_charge(p5);
+DigitalOut l_charge(p6);
+
+Motor r_motor(p21,p30,p29);
+Motor l_motor(p22,p7,p8);
+
+volatile int prev_r_enc = 0;
+volatile int prev_l_enc = 0;
+
+volatile int r_tics = 0;
+volatile int l_tics = 0;
+
+volatile int r_goal = 0;
+volatile int l_goal = 0;
+
+volatile float r_C = -1;
+volatile float l_C = -1;
+
+Ticker charge_period;
+Ticker discharge_period;
+
+int directions[2*DIRECTIONS+1];
+
+MovementState move_state = Stopped;
+///
+//DoType dotype;
+
+
+int main() {
+    Bee_Type type;
+    Operation_Mode mode;
+    float distance;
+    
+    QTIsensor_init();
+    charge_period.attach(&QTIsensor_charge,0.02);
+    wait_ms(0.5);
+    discharge_period.attach(&QTIsensor_discharge,0.02);
+    
+    // init
+    xbee_reset = 0;
+    wait_ms(1);
+    xbee_reset = 1;
+    // set the type of robot
+    type = Leader;
+    if(type == Leader)
+        mode = Blaze;
+    else if (type == Follower)
+        mode = GetDirections;
+        
+    //calibrate(0.5, 40);  
+    //move(100);  
+    
+    //wait(2);
+    //move_state = Moving;
+    
+    //move(0.5, 80);
+    
+    distance = 10000 * srf;
+    srand((int)distance);
+    
+    while(1) {
+        if(DEBUG) {
+            pc.printf("Left : %d  Goal: %d\n\r", l_tics, l_goal);
+            pc.printf("Right: %d  Goal: %d\n\r", r_tics, r_goal);
+        }
+        
+        //wait(0.2);
+        distance = (254 - 6) * srf;
+        
+        if (DEBUG)
+            pc.printf("Range: %f\n\r", distance);
+        
+        if(distance <= 8)
+            rotate(1);
+        
+        //distance = srf / V_PER_IN;
+        //if(DEBUG)
+        //    pc.printf("Distance: %f in\n\r", distance);
+        
+        //if(xbee.readable())
+        //    directions[1] = xbee.getc();
+        
+        /*
+            This state machine swithces between the operating 
+            modes of the robot and calls the appropriate 
+            function in bees.cpp
+        */
+        
+        //calibrate(0.5,50);
+        
+        /*switch(mode) {
+            case Seek:
+                if(DEBUG)
+                    pc.printf("Seeking Pollen\n\r");
+                if(find_pollen())
+                    mode = Return;
+                break;
+            case Return:
+                if(DEBUG)
+                    pc.printf("Returning to Base\n\r");
+                if(return_home()) {
+                    if(type == Seeker)
+                        mode = SendDirections;
+                    else if(type == Gatherer)
+                        mode = Gather;
+                }
+                break;
+            case SendDirections:
+                if(DEBUG)
+                    pc.printf("Sending Directions\n\r");
+                if(send_directions())
+                    mode = Seek;
+                break;
+            case GetDirections:
+                if(DEBUG)
+                    pc.printf("Waiting for Directions\n\r");
+                if(get_directions())
+                    mode = Gather;
+                break;
+            case Gather:
+                if(DEBUG)
+                    pc.printf("Gathering Pollen\n\r");
+                if(goto_pollen())
+                    mode = Gather;
+                break;
+            default:
+                if(DEBUG)
+                    pc.printf("Broken :(\n\r");
+        }*/
+        
+        switch(mode) {
+            case Blaze:
+                if(DEBUG)
+                    pc.printf("Creating Path\n\r");
+                blaze();
+                //pc.printf("now exiting blaze\n\r");
+                mode = Copy;
+                break;
+            case SendDirections:
+                if(DEBUG)
+                    pc.printf("Sending Directions\n\r");
+                send_directions();
+                break;
+            case GetDirections:
+                if(DEBUG)
+                    pc.printf("Waiting for Directions\n\r");
+                if(get_directions())
+                    mode = Copy;
+                break;
+            case Copy:
+                if(DEBUG)
+                    pc.printf("Moving\n\r");
+                if(copy()) {
+                    if(type == Leader)
+                        mode = SendDirections;
+                }
+                break;
+            default:
+                if(DEBUG)
+                    pc.printf("Broken :(\n\r");
+        }
+        //pc.printf("now checking move_state\n\r");
+        switch(move_state) {
+            case Stopped:
+                //do nothing
+                r_motor.speed(NEUTRAL);
+                l_motor.speed(NEUTRAL);
+                break;
+            /*case Stop:
+                //stop the motors
+                r_motor.stop();
+                l_motor.stop();
+                mov_state = Stopped;*/
+            case Moving:
+                //sample encoders
+                //update_movement(0.5);
+                //pc.printf("now checking encoders right = %d goal = %d, left = %d goal = %d\n\r",r_tics,r_goal,l_tics,l_goal);
+                if ((r_tics >= r_goal) || (l_tics >= l_goal)) {
+                    r_motor.speed(NEUTRAL);
+                    l_motor.speed(NEUTRAL);
+                    move_state = Stopped;
+                }
+                
+                break;
+        }
+        
+        myled = 1;
+        wait(0.2);
+        myled = 0;
+        wait(0.2);
+    }
+}