Revision 0:6893dd9a88b3, committed 2011-12-14

Comitter:

Shahan

Date:

Wed Dec 14 04:58:43 2011 +0000

Commit message:

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor.lib	Wed Dec 14 04:58:43 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/main.cpp	Wed Dec 14 04:58:43 2011 +0000
@@ -0,0 +1,73 @@
+#include "mbed.h"
+#include "Motor.h"
+#include "movement.h"
+
+Serial pc(USBTX,USBRX);
+
+DigitalOut myled(LED1);
+
+AnalogIn rightsensorval(p15);
+AnalogIn leftsensorval(p16);
+
+DigitalOut rightsensorcontrol(p5);
+DigitalOut leftsensorcontrol(p6);
+
+Motor motor_right(p21,p7,p8);
+Motor motor_left(p22,p9,p10);
+
+
+volatile int rightspokecount = 0;
+volatile int leftspokecount = 0;
+
+volatile int rightspokecountrequired = 0;
+volatile int leftspokecountrequired = 0;
+
+volatile float rightmotorspeed = 1;
+volatile float leftmotorspeed = 1;
+
+Ticker charge_period;
+Ticker discharge_period;
+
+int main() {
+    
+    QTIsensor_init();
+    charge_period.attach(&QTIsensor_charge,0.02);
+    wait_ms(0.5);
+    discharge_period.attach(&QTIsensor_discharge,0.02);
+    while(1) {
+        //calibratemotors();
+        myled = 1;
+        wait(0.4);
+        move(2);
+        //motors_stop();
+        while((rightspokecount < rightspokecountrequired) || (leftspokecount < leftspokecountrequired)){
+            pc.printf("right = %d, left =%d\n\r",rightspokecount,leftspokecount);
+            float right = rightsensorval;
+            float left = leftsensorval;
+            pc.printf("rightval = %f, leftval =%f\n\r",right,left);
+            wait(0.2);
+        }
+        motors_stop();
+        wait(1);
+        move(-2);
+        while((rightspokecount < rightspokecountrequired) || (leftspokecount < leftspokecountrequired)){
+            pc.printf("right = %d, left =%d\n\r",rightspokecount,leftspokecount);
+            float right = rightsensorval;
+            float left = leftsensorval;
+            pc.printf("rightval = %f, leftval =%f\n\r",right,left);
+            wait(0.2);
+        }
+        motors_stop();
+        //motor_reset = 1;
+        //goforward(right,left,0.5);
+        //wait(1.5);
+        
+        myled = 0;
+        wait(0.4);
+        //gobackward(right,left,0.5);
+        //wait(1.5);
+    }
+}
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Dec 14 04:58:43 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/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();
+                }
+            }            
+        }
+    }
+}
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/movement.h	Wed Dec 14 04:58:43 2011 +0000
@@ -0,0 +1,45 @@
+#include "mbed.h"
+#include "Motor.h"
+
+#ifndef _MOVEMENT_H
+#define _MOVEMENT_H
+
+#define qtithreshold 0.3
+
+#define spokeconversionfactor 0.5
+
+
+extern AnalogIn rightsensorval;
+extern AnalogIn leftsensorval;
+
+extern DigitalOut rightsensorcontrol;
+extern DigitalOut leftsensorcontrol;
+
+extern DigitalOut motor_reset;
+extern Motor motor_right;
+extern Motor motor_left;
+
+extern Serial pc;
+
+volatile extern int rightspokecount;
+volatile extern int leftspokecount;
+
+volatile extern int rightspokecountrequired;
+volatile extern int leftspokecountrequired;
+
+volatile extern float rightmotorspeed;
+volatile extern float leftmotorspeed;
+
+void moveforward(Motor right,float speed);
+void movebackward(Motor right,float speed);
+void move(int distance);
+void motors_stop();
+void calibratemotors();
+
+
+void QTIsensor_init();
+void QTIsensor_charge();
+void QTIsensor_discharge();
+
+#endif
+