Diff: movement.cpp

Revision:

0:6893dd9a88b3

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/movement.cpp	Wed Dec 14 04:58:43 2011 +0000
@@ -0,0 +1,124 @@
+#include "mbed.h"
+#include "Motor.h"
+#include "movement.h"
+
+
+void moveforward(Motor wheel,float speed){
+    wheel.speed(speed);
+}
+
+void movebackward(Motor wheel,float speed){
+    wheel.speed(-speed);
+}
+
+void move(int distance){
+    rightspokecountrequired = abs(distance)/spokeconversionfactor;
+    leftspokecountrequired = abs(distance)/spokeconversionfactor;
+    if(distance > 0){
+        moveforward(motor_right,rightmotorspeed);
+        moveforward(motor_left,leftmotorspeed);
+    }
+    else if(distance < 0){
+        movebackward(motor_right,rightmotorspeed);
+        movebackward(motor_left,leftmotorspeed);
+    }
+}
+
+
+void QTIsensor_init(){
+    rightsensorcontrol = 0;
+    leftsensorcontrol = 0;
+}
+ 
+void QTIsensor_charge(){
+    rightsensorcontrol = 1;    
+    leftsensorcontrol = 1;
+}
+
+void QTIsensor_discharge(){
+    if(rightsensorval < qtithreshold) rightspokecount++;
+    rightsensorcontrol = 0;
+    if(leftsensorval < qtithreshold) leftspokecount++;
+    leftsensorcontrol = 0;
+}
+
+void motors_stop(){
+    motor_right.speed(0);
+    motor_left.speed(0);
+}
+
+void calibratemotors(){
+    int r,l;
+    pc.printf("now moving forwards\n\r");
+    move(2);
+    while ((rightspokecount < rightspokecountrequired) || (leftspokecount < leftspokecountrequired)){
+        pc.printf("right count = %d, left count = %d",rightspokecount,leftspokecount);
+        wait(0.4);
+    } 
+    motors_stop();
+    pc.printf("now moving backwards\n\r");
+    wait(0.3);
+    //rightspokecountrequired = 2/spokeconversionfactor;
+    //leftspokecountrequired = 2/spokeconversionfactor;
+    move(-2);
+    pc.printf("right required = %d, left required = %d",rightspokecountrequired,leftspokecountrequired);
+    while ((rightspokecount < rightspokecountrequired) && (leftspokecount < leftspokecountrequired)){
+        pc.printf("right count = %d, left count = %d",rightspokecount,leftspokecount);
+        wait(0.4);
+    }
+    r = rightspokecount;
+    l = leftspokecount;
+    while ((rightspokecount < rightspokecountrequired) || (leftspokecount < leftspokecountrequired)){
+        pc.printf("right count = %d, left count = %d",rightspokecount,leftspokecount);
+        wait(0.4);
+    }
+    motors_stop();
+    wait(0.3);
+    pc.printf("now re-calibrating\n\r");
+    if(r != l){
+        if((rightmotorspeed == 1)&&(leftmotorspeed == 1)){
+            if(r>l){
+                if((r - l) > 1){
+                    rightmotorspeed = rightmotorspeed - 0.5;
+                    calibratemotors();
+                }
+            }
+            else if (r<l){
+                if((l - r) > 1){
+                    leftmotorspeed = leftmotorspeed - 0.5;
+                    calibratemotors();
+                }
+            }
+        }
+        else if(rightmotorspeed == 1){
+            if(r>l){
+                if((r - l) > 1){
+                    leftmotorspeed = leftmotorspeed + (1 - leftmotorspeed)*0.5;
+                    calibratemotors();
+                }
+            }
+            else if (r<l){
+                if((l - r) > 1){
+                    leftmotorspeed = leftmotorspeed - (1 - leftmotorspeed)*0.5;
+                    calibratemotors();
+                }
+            }            
+        }
+        else if(leftmotorspeed == 1){
+            if(r>l){
+                if((r - l) > 1){
+                    rightmotorspeed = rightmotorspeed - (1 - rightmotorspeed)*0.5;
+                    calibratemotors();
+                }
+            }
+            else if (r<l){
+                if((l - r) > 1){
+                    rightmotorspeed = rightmotorspeed + (1 - rightmotorspeed)*0.5;                
+                    calibratemotors();
+                }
+            }            
+        }
+    }
+}
+
+