Diff: main.cpp

Revision:

1:801f0b9a862a

Parent:

0:ff94cc47fef7

Child:

2:3d0be48abcf2

--- a/main.cpp	Wed Mar 12 21:55:44 2014 +0000
+++ b/main.cpp	Thu Mar 13 20:45:56 2014 +0000
@@ -13,7 +13,7 @@
 #define WHEEL_CIRCUM   (12.56637)
 #define DIST_PER_PULSE (0.01054225722682)
 #define MTRS_TO_INCH   (39.3701)
-#define MAX_SPEED      (0.2*127)
+#define MAX_SPEED      (0.3*127)
 
 float range, pid_return;
 void errFunction(void);
@@ -23,31 +23,43 @@
 Serial bt(p13,p14);
 Serial pc(USBTX,USBRX);
 HCSR04 rangeFinder( PIN_TRIGGER, PIN_ECHO );
-PID pid1(1.0,0.0,0.0,0.1);
+PID pid1(2.0,2.0,0.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);
+//InterruptIn encoder(p29);
+
 
 //Functions
 
 void wall_follow(void);
-void wall_follow2(int &currentLocation);
+void wall_follow2(int *currentLocation);
 void wall_follow3(int &currentLocation, int &WaveOpening);
-
+void leftTurn(void);
+void rightTurn(void);
 void us_distance(void);
 
+//Variables
 
 int main(void){
+    int location=0;
 
     pc.baud(115200);
-    motors.begin();
+    bt.baud(115200);
+    motors.begin();  
         
-    //motors.setMotor0Speed(MAX_SPEED); //left
-    //motors.setMotor1Speed(MAX_SPEED); //right
-    //wait(10);
-    
-    wall_follow();
+    /*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.stopBothMotors();
-    
+    leftTurn();
+    wait(1);
+    rightTurn();
     
 }
 
@@ -70,19 +82,19 @@
 {
     while(1){
         
-        pid1.setInputLimits(1.0, 20.0);
-        pid1.setOutputLimits( -0.3*127, 0.3*127);
-        pid1.setSetPoint(10.0);
+        pid1.setInputLimits(5.75, 6);
+        pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
+        pid1.setSetPoint(6.0);
         
         rangeFinder.startMeas();
-        wait_ms(100);
+        wait_ms(20);
         if ( (rangeFinder.getMeas(range) == RANGE_MEAS_VALID) && (range < 100.0) && (range > 3.0))
         {
-            //pc.printf("Range = %f\n", range);
+            pc.printf("Range = %f\n\r", range);
         } 
         pid1.setProcessValue(range);
         pid_return = pid1.compute(); 
-        pc.printf("Range: %f\n      PID:   %f", range, pid_return);
+        pc.printf("Range: %f\n      PID:   %f\r\n", range, pid_return);
         
         if(pid_return > 0){
             motors.setMotor0Speed(MAX_SPEED - pid_return);//left
@@ -99,39 +111,48 @@
 
 /* MODIFIED WALL_FOLLOW FOR NAVIGATION */
 
-void wall_follow2(int &currentLocation)
+void wall_follow2(int *currentLocation)
 {
     int SeeWaveGap = false;
+    int count=0;
+    
     while(1){
         
-        pid1.setInputLimits(4.0, 20.0);
-        pid1.setOutputLimits( -0.6, 0.6);
-        pid1.setSetPoint(20.0);
+        pid1.setInputLimits(0.0, 6.0);
+        pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
+        pid1.setSetPoint(6.0);
         
         rangeFinder.startMeas();
-        wait_ms(100);
+        wait_ms(20);
         if ( (rangeFinder.getMeas(range) == RANGE_MEAS_VALID) && (range < 100.0) && (range > 3.0))
         {
-            //bt.printf("Range = %f\n", range);
+            bt.printf("Range = %f\n", range);
         } 
         
         /*************CHECK FOR WAVE OPENING*****************/
-        /* If after 100 ms the ultrasonic still sees 20+ cm */
+        /* If after 60 ms the ultrasonic still sees 20+ cm */
         /*      then robot is at wave opening               */
         
-        
-        if(range > 20 && !SeeWaveGap){
+        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;
-        } else {
-            // AT WAVE OPENING!!!!
-            break;
         }
+           
         
             
         pid1.setProcessValue(range);
         pid_return = pid1.compute(); 
-        bt.printf("Range: %f\n      PID:   %f", range, pid_return);
+        bt.printf("Range: %f\n      PID:   %f\r\n", range, pid_return);
         
         if(pid_return > 0){
             motors.setMotor0Speed(MAX_SPEED - pid_return);
@@ -155,9 +176,9 @@
     while(1){
         
         
-        pid1.setInputLimits(4.0, 20.0);
-        pid1.setOutputLimits( -0.6, 0.6);
-        pid1.setSetPoint(20.0);
+        pid1.setInputLimits(5.75, 6);
+        pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
+        pid1.setSetPoint(6.0);
         
         rangeFinder.startMeas();
         wait_ms(100);
@@ -198,3 +219,23 @@
         }
      }
 }
+
+void rightTurn(void)
+{
+    leftEncoder.reset();
+    //rightEncoder.reset();
+    motors.setMotor0Speed(-0.4*127);
+    motors.setMotor1Speed(0.4*127);
+    while(leftEncoder.getPulses()<1400);
+    motors.stopBothMotors();
+}
+
+void leftTurn(void)
+{
+    leftEncoder.reset();
+    //rightEncoder.reset();
+    motors.setMotor0Speed(0.4*127);
+    motors.setMotor1Speed(-0.4*127);
+    while(leftEncoder.getPulses()>-1500);
+    motors.stopBothMotors();
+}