Diff: move.cpp

Revision:

0:178a07cd3e39

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