Diff: main.cpp

Revision:

2:3d0be48abcf2

Parent:

1:801f0b9a862a

Child:

3:58726d2e11f0

--- a/main.cpp	Thu Mar 13 20:45:56 2014 +0000
+++ b/main.cpp	Sat Mar 15 22:37:06 2014 +0000
@@ -7,13 +7,19 @@
 #include "stdio.h"
 #include "LPC17xx.h"
 
-#define PIN_TRIGGER    (p12)
-#define PIN_ECHO       (p11)
+#define PIN_TRIGGERL    (p12)
+#define PIN_ECHOL       (p11)
+#define PIN_TRIGGERR    (p29)
+#define PIN_ECHOR       (p30)
 #define PULSE_PER_REV  (1192)
 #define WHEEL_CIRCUM   (12.56637)
 #define DIST_PER_PULSE (0.01054225722682)
 #define MTRS_TO_INCH   (39.3701)
 #define MAX_SPEED      (0.3*127)
+#define PPRL           (965)
+#define PPRR           (1075)
+#define LEFT           (1)
+#define RIGHT          (0)
 
 float range, pid_return;
 void errFunction(void);
@@ -22,18 +28,19 @@
 //Hardware Initialization
 Serial bt(p13,p14);
 Serial pc(USBTX,USBRX);
-HCSR04 rangeFinder( PIN_TRIGGER, PIN_ECHO );
-PID pid1(2.0,2.0,0.0,0.02);
+HCSR04 rangeFinderLeft( PIN_TRIGGERL, PIN_ECHOL );
+HCSR04 rangeFinderRight( PIN_TRIGGERR, PIN_ECHOR );
+PID pid1(15.0,0.0,4.0,0.02);
 PololuQik2 motors(p9, p10, p8, p15, errFunction, cRc);
-QEI leftEncoder(p17,p18,NC,1192,QEI::X4_ENCODING);
-//QEI rightEncoder(p19,p20,NC,1192,QEI::X4_ENCODING);
+QEI rightEncoder(p17,p18,NC,PPRR,QEI::X4_ENCODING);
+QEI leftEncoder(p16,p15,NC,PPRR,QEI::X4_ENCODING);
 //InterruptIn encoder(p29);
 
 
 //Functions
 
-void wall_follow(void);
-void wall_follow2(int *currentLocation);
+void wall_follow(int side);
+void wall_follow2(int side);
 void wall_follow3(int &currentLocation, int &WaveOpening);
 void leftTurn(void);
 void rightTurn(void);
@@ -41,130 +48,184 @@
 
 //Variables
 
-int main(void){
-    int location=0;
+int main(void)
+{
+    float location=0;
 
     pc.baud(115200);
     bt.baud(115200);
-    motors.begin();  
-        
-    /*motors.setMotor0Speed(MAX_SPEED); //left
-    motors.setMotor1Speed(MAX_SPEED); //right
-    wait_ms(350);
-    */
-    //wall_follow();
-    //wall_follow2(&location);
-    pc.printf("%d\n\r",location);
+    motors.begin();
+    
+    //motors.setMotor0Speed(MAX_SPEED); //right
+    //motors.setMotor1Speed(MAX_SPEED); //left
     
+    //motors.stopBothMotors();
+
+
+    //wall_follow(RIGHT);
+    leftEncoder.reset();
+    rightEncoder.reset();
+    wall_follow2(LEFT);
+    location=(abs(leftEncoder.getPulses()*11.12/PPRL) + abs(rightEncoder.getPulses()*11.12/PPRR))/2;
+     
+    leftEncoder.reset();
+    rightEncoder.reset();
+    motors.setMotor0Speed(MAX_SPEED); //right
+    motors.setMotor1Speed(MAX_SPEED); //left
+    while(leftEncoder.getPulses()<300 || rightEncoder.getPulses()<300);
     motors.stopBothMotors();
+    
     leftTurn();
-    wait(1);
+    
+    leftEncoder.reset();
+    rightEncoder.reset();
+    motors.setMotor0Speed(MAX_SPEED); //right
+    motors.setMotor1Speed(MAX_SPEED); //left
+    while(abs(leftEncoder.getPulses()*11.12/PPRL) + abs(rightEncoder.getPulses()*11.12/PPRR)/2 <30);
+    motors.stopBothMotors();
+    
+    leftTurn();
+    wall_follow2(RIGHT);
     rightTurn();
     
+
+
+    bt.printf("LOCATION %f\n\r",location);
+
+    motors.stopBothMotors();
+//   leftTurn();
+//   wait(1);
+//   rightTurn();
+
+
 }
 
-void errFunction(void){
-    //Nothing    
+void errFunction(void)
+{
+    //Nothing
 }
 
 void us_distance(void)
 {
-        pc.printf("Ultra Sonic\n\r");
-        rangeFinder.startMeas();
-        wait_us(20);
-        if ( (rangeFinder.getMeas(range) == RANGE_MEAS_VALID))
-        {
-            pc.printf("Range = %f\n\r", range);
-        }  
+    pc.printf("Ultra Sonic\n\r");
+    rangeFinderLeft.startMeas();
+    wait_us(20);
+    if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID)) {
+        pc.printf("Range = %f\n\r", range);
+    }
 }
 
-void wall_follow(void)
+void wall_follow(int side)
 {
-    while(1){
-        
-        pid1.setInputLimits(5.75, 6);
+    while(1) {
+
+        pid1.setInputLimits(0, 5.0);
         pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
-        pid1.setSetPoint(6.0);
+        pid1.setSetPoint(5.0);
+        if(side){
+            rangeFinderLeft.startMeas();
+            wait_ms(20);
+            rangeFinderLeft.getMeas(range);
+        }
+        else{
+            rangeFinderRight.startMeas();
+            wait_ms(20);
+            rangeFinderRight.getMeas(range);
+            pc.printf("%d\r\n",range);
+        }
         
-        rangeFinder.startMeas();
-        wait_ms(20);
-        if ( (rangeFinder.getMeas(range) == RANGE_MEAS_VALID) && (range < 100.0) && (range > 3.0))
-        {
-            pc.printf("Range = %f\n\r", range);
-        } 
         pid1.setProcessValue(range);
-        pid_return = pid1.compute(); 
-        pc.printf("Range: %f\n      PID:   %f\r\n", range, pid_return);
-        
-        if(pid_return > 0){
-            motors.setMotor0Speed(MAX_SPEED - pid_return);//left
-            motors.setMotor1Speed(MAX_SPEED);
-        }else if(pid_return < 0){
-            motors.setMotor0Speed(MAX_SPEED);
-            motors.setMotor1Speed(MAX_SPEED + pid_return);
-        }else{
-            motors.setMotor0Speed(MAX_SPEED);
-            motors.setMotor1Speed(MAX_SPEED);
+        pid_return = pid1.compute();
+
+        if(pid_return > 0) {
+            if(side){
+                motors.setMotor0Speed(MAX_SPEED - pid_return);//right
+                motors.setMotor1Speed(MAX_SPEED);//left
+            }
+            else{
+                motors.setMotor1Speed(MAX_SPEED - pid_return);//left
+                motors.setMotor0Speed(MAX_SPEED);//right
+            }
+        }else if(pid_return < 0) {
+            if(side){
+                motors.setMotor0Speed(MAX_SPEED);//right
+                motors.setMotor1Speed(MAX_SPEED + pid_return);//left
+            }
+            else{
+                motors.setMotor1Speed(MAX_SPEED);//left
+                motors.setMotor0Speed(MAX_SPEED + pid_return);//right
+            }
+        }else {
+            motors.setMotor0Speed(MAX_SPEED);//right
+            motors.setMotor1Speed(MAX_SPEED);//left
         }
-     }
+    }
 }
 
 /* MODIFIED WALL_FOLLOW FOR NAVIGATION */
 
-void wall_follow2(int *currentLocation)
+void wall_follow2(int side)
 {
     int SeeWaveGap = false;
     int count=0;
-    
-    while(1){
-        
-        pid1.setInputLimits(0.0, 6.0);
+
+    while(1) {
+
+        pid1.setInputLimits(0.0, 5.0);
         pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
-        pid1.setSetPoint(6.0);
-        
-        rangeFinder.startMeas();
-        wait_ms(20);
-        if ( (rangeFinder.getMeas(range) == RANGE_MEAS_VALID) && (range < 100.0) && (range > 3.0))
-        {
-            bt.printf("Range = %f\n", range);
-        } 
-        
+        pid1.setSetPoint(5.0);
+
+        if(side){
+            rangeFinderLeft.startMeas();
+            wait_ms(20);
+            rangeFinderLeft.getMeas(range);
+        }
+        else{
+            rangeFinderRight.startMeas();
+            wait_ms(20);
+            rangeFinderRight.getMeas(range);
+        }
+
+
         /*************CHECK FOR WAVE OPENING*****************/
         /* If after 60 ms the ultrasonic still sees 20+ cm */
         /*      then robot is at wave opening               */
-        
+
         pc.printf("range %f\r\n",range);
-        if(range > 20){
-            currentLocation++;
-            bt.printf("saw gap \r\n");
-            
-            if(SeeWaveGap){
-                motors.stopBothMotors();
-                bt.printf("wavegap\r\n");
-                // AT WAVE OPENING!!!!
-            
-                break;
-            }
-            SeeWaveGap = true;
+        if(range > 20) {
+            motors.stopBothMotors();
+            bt.printf("wavegap\r\n");
+            // AT WAVE OPENING!!!!
+            break;
         }
-           
-        
-            
+
         pid1.setProcessValue(range);
-        pid_return = pid1.compute(); 
-        bt.printf("Range: %f\n      PID:   %f\r\n", range, pid_return);
-        
-        if(pid_return > 0){
-            motors.setMotor0Speed(MAX_SPEED - pid_return);
-            motors.setMotor1Speed(MAX_SPEED);
-        }else if(pid_return < 0){
-            motors.setMotor0Speed(MAX_SPEED);
-            motors.setMotor1Speed(MAX_SPEED + pid_return);
-        }else{
+        pid_return = pid1.compute();
+        //bt.printf("Range: %f\n      PID:   %f\r\n", range, pid_return);
+
+               if(pid_return > 0) {
+            if(side){
+                motors.setMotor0Speed(MAX_SPEED - pid_return);//right
+                motors.setMotor1Speed(MAX_SPEED);//left
+            }
+            else{
+                motors.setMotor1Speed(MAX_SPEED - pid_return);//left
+                motors.setMotor0Speed(MAX_SPEED);//right
+            }
+        }else if(pid_return < 0) {
+            if(side){
+                motors.setMotor0Speed(MAX_SPEED);//right
+                motors.setMotor1Speed(MAX_SPEED + pid_return);//left
+            }
+            else{
+                motors.setMotor1Speed(MAX_SPEED);//left
+                motors.setMotor0Speed(MAX_SPEED + pid_return);//right
+            }
+        } else {
             motors.setMotor0Speed(MAX_SPEED);
             motors.setMotor1Speed(MAX_SPEED);
         }
-     }
+    }
 }
 
 
@@ -173,69 +234,72 @@
 
 void wall_follow3(int &currentLocation, int &WaveOpening)
 {
-    while(1){
-        
-        
-        pid1.setInputLimits(5.75, 6);
+    while(1) {
+
+
+        pid1.setInputLimits(0, 5);
         pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
-        pid1.setSetPoint(6.0);
-        
-        rangeFinder.startMeas();
+        pid1.setSetPoint(5.0);
+
+        rangeFinderLeft.startMeas();
         wait_ms(100);
-        if ( (rangeFinder.getMeas(range) == RANGE_MEAS_VALID) && (range < 100.0) && (range > 3.0))
-        {
+        if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID) && (range < 100.0) && (range > 3.0)) {
             //bt.printf("Range = %f\n", range);
-        } 
-        
+        }
+
         /*************CHECK FOR WAVE OPENING*****************/
         /* If after 100 ms the ultrasonic still sees 20+ cm */
         /*      then robot is at wave opening               */
-        
-        
-        if(range > 20 ){
+
+
+        if(range > 20 ) {
             currentLocation--;
         }
-        
-        if( currentLocation == WaveOpening){
+
+        if( currentLocation == WaveOpening) {
             // AT WAVE OPENING!!!!
-            
+
             break;
         }
-        
-            
+
+
         pid1.setProcessValue(range);
-        pid_return = pid1.compute(); 
+        pid_return = pid1.compute();
         bt.printf("Range: %f\n      PID:   %f", range, pid_return);
-        
-        if(pid_return > 0){
+
+        if(pid_return > 0) {
             motors.setMotor0Speed(MAX_SPEED - pid_return);
             motors.setMotor1Speed(MAX_SPEED);
-        }else if(pid_return < 0){
+        } else if(pid_return < 0) {
             motors.setMotor0Speed(MAX_SPEED);
             motors.setMotor1Speed(MAX_SPEED + pid_return);
-        }else{
+        } else {
             motors.setMotor0Speed(MAX_SPEED);
             motors.setMotor1Speed(MAX_SPEED);
         }
-     }
+    }
 }
 
 void rightTurn(void)
 {
+    float speedL, speedR;
+
+    speedL=speedR= 0.4;
+
     leftEncoder.reset();
-    //rightEncoder.reset();
-    motors.setMotor0Speed(-0.4*127);
-    motors.setMotor1Speed(0.4*127);
-    while(leftEncoder.getPulses()<1400);
+    rightEncoder.reset();
+    motors.setMotor0Speed(-speedR*127);//right
+    motors.setMotor1Speed(speedL*127);//left
+    while(leftEncoder.getPulses()<1050 || rightEncoder.getPulses()>-1050);
     motors.stopBothMotors();
 }
 
 void leftTurn(void)
 {
     leftEncoder.reset();
-    //rightEncoder.reset();
-    motors.setMotor0Speed(0.4*127);
-    motors.setMotor1Speed(-0.4*127);
-    while(leftEncoder.getPulses()>-1500);
+    rightEncoder.reset();
+    motors.setMotor0Speed(0.4*127);// right
+    motors.setMotor1Speed(-0.4*127);// left
+    while(leftEncoder.getPulses()>-1200 || rightEncoder.getPulses()<1200);
     motors.stopBothMotors();
 }