Revision 0:178a07cd3e39, committed 2011-12-16

Comitter:

gsulc

Date:

Fri Dec 16 03:45:52 2011 +0000

Commit message:

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor.lib	Fri Dec 16 03:45:52 2011 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/simon/code/Motor/#f265e441bcd9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/bees.cpp	Fri Dec 16 03:45:52 2011 +0000
@@ -0,0 +1,138 @@
+#include "bees.h"
+
+int find_pollen() {
+    return 1;
+}
+int return_home() {
+    return 1;
+}
+int send_directions() {
+    //while(!xbee.readable()) {}
+    //read header
+    
+    //read contents
+    int i = 0;
+    while(1){
+        xbee.putc(directions[i] + 2);
+        if(DEBUG)
+            pc.printf("Sending  %d\n\r", directions[i]);
+        if(directions[i] == TERMINATE_CHAR) {
+            if (DEBUG)
+                pc.printf("Successfully transfered directions.\n\r");
+            return 1;
+        }
+        i++;
+    }
+    
+    if (DEBUG)
+        pc.printf("Error writing directions.\n\r");
+    return 0;
+}
+
+/*********************************************************
+    Name: get_directions
+    inputs: 
+        void
+    outputs:
+        int: 1 = completed sucessfully; 0 = unsuccessful
+    Purpose: gets directions on where to move
+*********************************************************/
+int get_directions() {
+    //int buff[DIRECTIONS];
+    int i;
+    // wait until the xbee can be read
+    //while(!xbee.readable()) {}
+    //read header
+    
+    //read contents
+    i = 0;
+    while(1){
+        directions[i] = xbee.getc() - 2;
+        if (DEBUG)
+            pc.printf("dir    %d\n\r", directions[i]);
+        if(directions[i] == TERMINATE_CHAR)
+            return 1; //successfully transfered directions
+        i++;
+    }
+    
+    if (DEBUG)
+        pc.printf("Error reading directions.\n\r");
+    return 0;
+}
+
+int gather() {
+    return 1;
+}
+
+int goto_pollen() {
+    return 1;
+}
+
+void blaze() {
+    int i;
+    for (i = 0; i < (2*DIRECTIONS); i++) {
+        directions[i] = rand() % 3 - 1;
+        directions[++i] = rand() % 20 + 51;  
+        if(DEBUG){
+            pc.printf("Directions:  %d,  %d\n\r", directions[i-1], directions[i]);
+        } 
+    }
+    directions[i] = TERMINATE_CHAR;
+    if(DEBUG)
+        pc.printf("Temrinate\n\r");
+}
+
+int copy() {
+    static int direct = 0;
+    static int state = 0;
+    
+    /*if(dotype == GetMove) {
+        current_move = directions[direct];
+        
+    }*/
+    
+    //else if (dotype == DoMove) {
+    
+    static int current_move;
+    
+    if(state == 0) {
+        current_move = directions[direct];
+        if(DEBUG)
+            pc.printf("Oh! got the Move!    %d\n\r", current_move);
+        state = 1;
+    }
+    else if (state == 2) {
+        if(move_state == Stopped)
+            state = 0;
+        if(DEBUG)
+            pc.printf("Waiting for  Next Move\n\r");
+    }
+    
+    else if (state == 1) {
+    if (current_move == TERMINATE_CHAR) {
+        l_motor.speed(NEUTRAL);
+        r_motor.speed(NEUTRAL);
+        move_state = Stopped;
+        return 1;
+    }
+    else if (current_move <= 1) {
+        if(DEBUG)
+            pc.printf("I like to rotate! \n\r");
+        rotate(current_move);
+        //if((r_tics >= r_goal) || (l_tics >= l_goal))
+            direct++;
+    }
+    else if  (current_move >= 20) {
+        if(DEBUG)
+            pc.printf("I like to move it move it! \n\r");
+            
+        move(0.5, current_move);
+        //if((r_tics >= r_goal) || (l_tics >= l_goal))
+            direct++;
+    }
+    state = 2;
+    }
+    
+    return 0;
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/bees.h	Fri Dec 16 03:45:52 2011 +0000
@@ -0,0 +1,34 @@
+#ifndef _BEES_H
+#define _BEES_H
+
+#include "mbed.h"
+#include "move.h"
+
+#define DEBUG 1
+#define TERMINATE_CHAR 253
+#define DIRECTIONS 4
+
+typedef enum {Leader, Follower} Bee_Type;
+typedef enum {Blaze, SendDirections, GetDirections, Copy} Operation_Mode;
+//typedef enum {GetMove, DoMove} DoType;
+
+extern Serial pc;
+extern Serial xbee;
+extern MovementState move_state;
+//int current_move;
+
+//static int direct = 0;
+
+extern int directions[2*DIRECTIONS+1];
+
+int find_pollen();
+int return_home();
+int send_directions();
+int get_directions();
+int gather();
+int goto_pollen();
+void blaze();
+int copy();
+
+
+#endif
\ No newline at end of file

--- /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);
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Dec 16 03:45:52 2011 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/63bcd7ba4912

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/move.cpp	Fri Dec 16 03:45:52 2011 +0000
@@ -0,0 +1,193 @@
+#include "move.h"
+
+//void accelerate(Motor m, float distance) {
+    
+//}
+
+//void move(int x, int y) {
+    
+//}
+
+//void move_line(float distance) {
+    /*while (distance > 0) {
+        //distance -= distance_traveled();
+        left_motor.speed(LEFT_C*FULL_FOWARD);
+        right_motor.speed(RIGHT_C*FULL_FOWARD);
+    }
+    left_motor.stop();
+    right_motor.stop();*/
+//}
+/*
+void rotate(int degrees) {
+}
+
+void rotate(float time, int direction) {
+    //left_motor.speed(0.5);
+    //right_motor.speed(0.5);
+}
+*/
+//void move_arc(int radius, int length, int x, int y) {
+//}
+
+// Shahan
+
+void moveforward(Motor wheel,float speed){
+    wheel.speed(speed);
+}
+
+void movebackward(Motor wheel,float speed){
+    wheel.speed(-speed);
+}
+
+void move(float speed, int tics){
+    /*r_goal = abs(distance)/TICS_PER_MM;
+    l_goal = abs(distance)/TICS_PER_MM;
+    if(distance > 0){
+        r_motor.speed(r_C);
+        l_motor.speed(l_C);
+    }
+    else if(distance < 0){
+        r_motor.speed(-r_C);
+        l_motor.speed(-l_C);
+    }*/
+    float distance = (248) * srf;
+    if(distance <= 14)
+            rotate(1);
+    
+    l_goal = l_tics + tics;
+    r_goal = r_tics + tics;
+    
+    l_motor.speed(speed * l_C);
+    r_motor.speed(speed * r_C);
+    move_state = Moving;
+}
+
+
+void QTIsensor_init(){
+    r_charge = 0;
+    l_charge = 0;
+}
+ 
+void QTIsensor_charge(){
+    r_charge = 1;    
+    l_charge = 1;
+}
+
+void QTIsensor_discharge(){
+    int r_enc_bin = 1, l_enc_bin = 1;
+    if(r_encoder < QTI_THRESH) {
+        r_enc_bin = 0;
+    }
+    r_charge = 0;
+    if(l_encoder < QTI_THRESH) {
+        l_enc_bin = 0;
+    }
+    l_charge = 0;
+    
+    if(prev_r_enc != r_enc_bin)
+        r_tics++;
+    if(prev_l_enc != l_enc_bin)
+        l_tics++;
+    
+    prev_r_enc = r_enc_bin;
+    prev_l_enc = l_enc_bin;
+}
+
+void motors_stop(){
+    r_motor.speed(0);
+    l_motor.speed(0);
+}
+
+
+
+void calibrate(float speed, int rots) {
+    float l_corr = -1, r_corr = -1;
+
+    do {
+    l_goal = l_tics + rots;
+    r_goal = r_tics + rots;
+    
+    wait(4);
+    
+    l_motor.speed(speed * l_C);
+    r_motor.speed(speed * r_C);
+    
+    while (r_tics < r_goal && l_tics < l_goal) { 
+        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();
+    }
+    
+    r_motor.speed(NEUTRAL);
+    l_motor.speed(NEUTRAL);
+    
+    if(r_tics < r_goal) {
+        //slow l down
+        l_corr += 0.02;
+    }
+    else if(l_tics < l_goal) {
+        //slow r down
+        r_corr += 0.02;
+    }
+    else {
+        break;
+    }
+    
+    wait(2);
+    
+    l_goal = l_tics + rots;
+    r_goal = r_tics + rots;
+    
+    l_motor.speed(-speed * l_C);
+    r_motor.speed(-speed * r_C);
+    
+    while (r_tics < r_goal && l_tics < l_goal) {
+        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(1);
+    }
+    
+    r_motor.speed(NEUTRAL);
+    l_motor.speed(NEUTRAL);
+    
+    l_C = l_corr;
+    r_C = r_corr;
+    } while( abs(r_goal - r_tics) > 2 || abs(l_goal - l_tics) > 2);
+}
+
+void rotate(int angle){
+    float speed = 0.2;
+    if(angle != 0) {
+        r_goal = r_tics + 3;
+        l_goal = l_tics + 3;
+        pc.printf("now turning at angle %d\n\r",angle);
+        r_motor.speed(angle*speed*r_C);
+        l_motor.speed(-1*angle*speed*l_C);
+        pc.printf("************  Speed: %f", speed);
+        move_state = Moving;
+    }
+    else {
+        r_goal = r_tics;
+        l_goal = l_tics;
+        move_state = Moving;
+    }
+    
+    
+}
+
+void update_movement(float speed) {
+    int r_diff = r_goal - r_tics;
+    int l_diff = l_goal - l_tics;
+    
+    if (r_diff > l_diff) {
+        l_motor.speed(speed / 1.2 *l_C);
+        wait_us(1 * (r_diff - l_diff));
+        l_motor.speed(speed*l_C);
+    }
+    
+    else if (r_diff < l_diff) {
+        r_motor.speed(speed / 1.2 *r_C);
+        wait_us(1 * (l_diff - r_diff));
+        r_motor.speed(speed*r_C);
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/move.h	Fri Dec 16 03:45:52 2011 +0000
@@ -0,0 +1,72 @@
+#ifndef _MOVE_H
+#define _MOVE_H
+
+#include "mbed.h"
+#include "Motor.h"
+
+#define LEFT_C -1
+#define RIGHT_C 1
+#define FULL_FOWARD 1
+#define FULL_REVERSE -1
+#define NEUTRAL 0
+#define LEFT -1
+#define RIGHT 1
+
+
+#define QTI_THRESH 0.48
+
+#define TICS_PER_MM 40.8407045
+
+typedef enum {Stopped, Moving} MovementState;
+extern MovementState move_state;
+
+extern AnalogIn r_encoder;
+extern AnalogIn l_encoder;
+extern AnalogIn srf;
+volatile extern int prev_r_enc;
+volatile extern int prev_l_enc;
+
+extern DigitalOut r_charge;
+extern DigitalOut l_charge;
+
+extern DigitalOut motor_reset;
+extern Motor r_motor;
+extern Motor l_motor;
+
+extern Serial pc;
+
+volatile extern int r_tics;
+volatile extern int l_tics;
+
+volatile extern int r_goal;
+volatile extern int l_goal;
+
+volatile extern float r_C;
+volatile extern float l_C;
+
+void moveforward(Motor right,float speed);
+void movebackward(Motor right,float speed);
+void move(float speed, int tics);
+void motors_stop();
+void rotate(int);
+
+
+void QTIsensor_init();
+void QTIsensor_charge();
+void QTIsensor_discharge();
+
+//Mine
+//extern MovementState move_state;
+
+//void accelerate(Motor m, float distance);
+//void move(int x, int y);
+//void move_line(float distance);
+//void rotate_deg(int degrees);
+//void rotate_t(float time);
+//void move_arc(int radius, int length, int x, int y);
+
+void calibrate(float speed, int rots);
+void update_movement(float speed);
+
+#endif
+ 
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sensors.h	Fri Dec 16 03:45:52 2011 +0000
@@ -0,0 +1,8 @@
+#ifndef _SENSORS_H
+#define _SENSORS_H
+
+#include "mbed.h"
+
+
+
+#endif
\ No newline at end of file