Diff: main.cpp

Revision:

30:db07aea6d119

Parent:

29:22b243e288c8

Child:

31:99cf9c25b0f2

diff -r 22b243e288c8 -r db07aea6d119 main.cpp
--- a/main.cpp	Wed Apr 16 20:51:52 2014 +0000
+++ b/main.cpp	Thu Apr 17 20:34:17 2014 +0000
@@ -25,7 +25,6 @@
 #define DIST_PER_PULSE (0.01054225722682)
 #define MTRS_TO_INCH   (39.3701)
 #define MAX_SPEED      (0.3*127)
-#define MAX_SPEED1     (0.25*127)
 #define PPR            (4331/4)
 #define LEFT           (1)
 #define RIGHT          (0)
@@ -119,12 +118,16 @@
 //Serial bt(p13,p14);
 HCSR04 rangeFinderFront( 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 rightEncoder(p17,p18,NC,PPR,QEI::X4_ENCODING);
-QEI leftEncoder(p16,p15,NC,PPR,QEI::X4_ENCODING);
-Sharp IRFront(p19);
-Sharp IRBack(p20);
+PID pid1(25.0,0.0,0.0,0.0001);
+PololuQik2 motors(p9, p10, p8, p11, &errFunction, cRc);
+QEI rightEncoder(p22,p21,NC,PPR,QEI::X4_ENCODING);
+QEI leftEncoder(p23,p24,NC,PPR,QEI::X4_ENCODING);
+Sharp IRLeftFront(p18);
+Sharp IRLeftBack(p17);
+Sharp IRRightFront(p20);
+Sharp IRRightBack(p15);
+Sharp IRFrontL(p19);
+Sharp IRFrontR(p16);
 //InterruptIn encoder(p29);
 
 
@@ -271,14 +274,18 @@
 
     pc.baud(115200);
 
-    //testSensors();
-
     //Laser Range Finder Initialization
     lrf_baudCalibration();
 
     motors.begin();
 
     startBtn.rise(&button_int);
+ //////////////////////////// TEST SERVOS /////////////////////////
+    //testSensors();
+    
+    motors.begin();
+    //wall_follow2(LEFT, FORWARD, TOOLS, 10, 1);
+    //while(1);
 
     //Servo initialization
     initServoDriver();
@@ -321,9 +328,9 @@
                 case START :
                     myled1 = 1;
 
-                    //current=75;
-                    //state  = NAVIGATE_WAVES_ROW1;
-                    state = OILRIG1_POS;
+                    current=75;
+                    state  = NAVIGATE_WAVES_ROW1;
+                   //state = OILRIG1_POS;
                     break;
 
 
@@ -661,7 +668,7 @@
                     break;
 
                 case NAVIGATE_WAVES_ROW3:
-                    //shape_detected = 1;
+                    shape_detected = 1;
                     if(tool_needed == 1) {
                         rig_section(location, current, direction, 1);
                         //state = NAVIGATE_TO_SQUARE_RIG;
@@ -909,9 +916,8 @@
 
 void wall_follow2(int side, int direction, int section, float location, int rig)
 {
-    int dir=1, limit=80, lowlim=4;
+    int dir=1, limit=78, lowlim=4;
     float set=28, loc=0, Rigloc=0, location_cal=0;
-    bool SeeWaveGap = false;
 
     if(rig == 1) Rigloc= 16;
     else if(rig == 2) Rigloc= 45;
@@ -923,7 +929,7 @@
         limit = 100;
         lowlim=10;
     } else if(section == RIGS) set = 18;
-    else if(section == MID2) limit =85;
+    else if(section == MID2) limit =80;
 
     if(direction == BACKWARD) {
         dir=-1;
@@ -935,8 +941,6 @@
     leftEncoder.reset();
     rightEncoder.reset();
 
-    //pc.printf("before %f\r\n", location);
-
     //pc.printf("dir*loc+location %f\r\n",dir*loc + location );
     //pc.printf("limit %d \r\n", limit);
     if((section == RIGS || section == RETURN) && side == LEFT)location_cal= -1*dir*loc + location;
@@ -950,30 +954,28 @@
         else location_cal= dir*loc + location;
 
         pid1.setInputLimits(0.0, set);
-        pid1.setOutputLimits( -MAX_SPEED1, MAX_SPEED1);
+        pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
         pid1.setSetPoint(set);
 
-        if(side) {
-            range = IRFront.getIRDistance();
-            range3= IRBack.getIRDistance();
+        if(side== LEFT) {
+            range = IRLeftFront.getIRDistance();
+            range2= IRLeftBack.getIRDistance();
         } else {
-            rangeFinderRight.startMeas();
-            wait_ms(20);
-            rangeFinderRight.getMeas(range);
+            range = IRRightFront.getIRDistance();
+            range2= IRRightBack.getIRDistance();
         }
 
-        if(section == RIGS) {
+        if(section == RIGS) {// check for the rigs on opposite side
             if(side == LEFT) {
-                range2 = IRFront.getIRDistance();
-                range3= IRBack.getIRDistance();
+                range = IRRightFront.getIRDistance();
+                range2= IRRightBack.getIRDistance();
 
             } else {
-                rangeFinderRight.startMeas();
-                wait_ms(20);
-                rangeFinderRight.getMeas(range2);
+                range = IRLeftFront.getIRDistance();
+                range2= IRLeftBack.getIRDistance();
             }
 
-            if(range2< 15) {
+            if(range2 < 40 || range < 40 ) {
                 if( abs(location_cal - Rigloc) < 10) {
                     //STOP
                     motors.stopBothMotors(127);
@@ -981,43 +983,41 @@
                 }
             }
         }
-
-
-        //pc.printf("wall follow 2 range %f\r\n",range);
-        //pc.printf("loc+location = %f\r\n", loc+location);
-        if(range > 35 && range3 > 35) {
-            if(section != RETURN) {
+        
+        if(range > 40 && range2 > 40 && section != RIGS) {
                 pc.printf("RANGE %f \tRANGE3 %f\r\n",range,range3);
                 //STOP
                 motors.stopBothMotors(127);
                 break;
-            }
 
         } else {
-            SeeWaveGap = false;
-            pid1.setProcessValue(range);
-            pid_return = pid1.compute();
-            //pc.printf("Range: %f\n      PID:   %f\r\n", range, pid_return);
+            if(direction == FORWARD) pid1.setProcessValue(range);
+            else pid1.setProcessValue(range2);
+
+            if(abs(range - range2) < 10){ // does it see a wavegap? 
+                pid_return = pid1.compute();
+                //pc.printf("Range: %f\n      PID:   %f\r\n", range, pid_return);
 
-            if(pid_return > 0) {
-                if(side) {
-                    motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
-                    motors.setMotor1Speed(dir*MAX_SPEED);//left
+                if(pid_return > 0) {
+                    if(side) {
+                        motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
+                        motors.setMotor1Speed(dir*MAX_SPEED);//left
+                    } else {
+                        motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
+                        motors.setMotor0Speed(dir*MAX_SPEED);//right
+                    }
+                } else if(pid_return < 0) {
+                    if(side) {
+                        motors.setMotor0Speed(dir*MAX_SPEED);//right
+                        motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
+                    } else {
+                        motors.setMotor1Speed(dir*MAX_SPEED);//left
+                        motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
+                    }
                 } else {
-                    motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
-                    motors.setMotor0Speed(dir*MAX_SPEED);//right
+                    motors.setMotor0Speed(dir*MAX_SPEED);
+                    motors.setMotor1Speed(dir*MAX_SPEED);
                 }
-            } else if(pid_return < 0) {
-                if(side) {
-                    motors.setMotor0Speed(dir*MAX_SPEED);//right
-                    motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
-                } else {
-                    motors.setMotor1Speed(dir*MAX_SPEED);//left
-                    motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
-                }
-            } else {
-                motors.setMotor0Speed(dir*MAX_SPEED);
-                motors.setMotor1Speed(dir*MAX_SPEED);
             }
         }
     }
@@ -1112,7 +1112,7 @@
 
         // check distance to wall
 
-        if(IRFront.getIRDistance() > 4) {
+        if(IRLeftFront.getIRDistance() > 4) {
 
             leftEncoder.reset();
             rightEncoder.reset();
@@ -1157,7 +1157,7 @@
                 wait_ms(20);
                 rangeFinderRight.getMeas(range);
             } else {
-                range = IRFront.getIRDistance();
+                range = IRLeftFront.getIRDistance();
             }
             //pc.printf("Range %f \t OldValue %f\n\r",range, usValue);
             if(range > usValue && usValue != 0 && range < 25) {
@@ -1180,14 +1180,14 @@
     motors.stopBothMotors(127);
     wait_ms(300);
 
-    if(IRFront.getIRDistance() > 35 && IRBack.getIRDistance() > 35) return;
+    if(IRLeftFront.getIRDistance() > 35 && IRLeftBack.getIRDistance() > 35) return;
     // align with wave
-    while(IRFront.getIRDistance() > 35) {
+    while(IRLeftFront.getIRDistance() > 35) {
         pc.printf("front sensor sees gap\r\n");
         motors.setMotor0Speed(-0.1*127);//right
         motors.setMotor1Speed(-0.1*127);//left
     }
-    while(IRBack.getIRDistance() >35 ) {
+    while(IRLeftBack.getIRDistance() >35 ) {
         pc.printf("back sensor sees gap\r\n");
         motors.setMotor0Speed(0.1*127);//right
         motors.setMotor1Speed(0.1*127);//left
@@ -1195,12 +1195,12 @@
     motors.stopBothMotors(127);
     wait_ms(300);
 
-    while(IRFront.getIRDistance() - IRBack.getIRDistance() > 0.5) {
+    while(IRLeftFront.getIRDistance() - IRLeftBack.getIRDistance() > 0.5) {
         pc.printf("turn left\r\n");
         motors.setMotor0Speed(0.3*127);// right
         motors.setMotor1Speed(-0.3*127);// left
     }
-    while(IRFront.getIRDistance() - IRBack.getIRDistance() < -0.5) {
+    while(IRLeftFront.getIRDistance() - IRLeftBack.getIRDistance() < -0.5) {
         pc.printf("turn right\r\n");
         motors.setMotor0Speed(-0.3*127);//right
         motors.setMotor1Speed(0.3*127);// left
@@ -1220,17 +1220,16 @@
     motors.setMotor1Speed(-0.5*127);// left
     while(abs(leftEncoder.getPulses())<1075 || rightEncoder.getPulses()<1075);
     motors.stopBothMotors(127);
-
     wait_ms(300);
 
-    if(IRFront.getIRDistance() > 35 && IRBack.getIRDistance() > 35) return;
+    if(IRLeftFront.getIRDistance() > 35 && IRLeftBack.getIRDistance() > 35) return;
     // align with wave
-    while(IRFront.getIRDistance() > 35) {
+    while(IRLeftFront.getIRDistance() > 35) {
         pc.printf("front sensor sees gap\r\n");
         motors.setMotor0Speed(-0.1*127);//right
         motors.setMotor1Speed(-0.1*127);//left
     }
-    while(IRBack.getIRDistance() >35 ) {
+    while(IRLeftBack.getIRDistance() >35 ) {
         pc.printf("back sensor sees gap\r\n");
         motors.setMotor0Speed(0.1*127);//right
         motors.setMotor1Speed(0.1*127);//left
@@ -1238,12 +1237,12 @@
     motors.stopBothMotors(127);
     wait_ms(300);
 
-    while(IRFront.getIRDistance() - IRBack.getIRDistance() > 0.5) {
+    while(IRLeftFront.getIRDistance() - IRLeftBack.getIRDistance() > 0.5) {
         pc.printf("turn left\r\n");
         motors.setMotor0Speed(0.3*127);// right
         motors.setMotor1Speed(-0.3*127);// left
     }
-    while(IRFront.getIRDistance() - IRBack.getIRDistance() < -0.5) {
+    while(IRLeftFront.getIRDistance() - IRLeftBack.getIRDistance() < -0.5) {
         pc.printf("turn right\r\n");
         motors.setMotor0Speed(-0.3*127);//right
         motors.setMotor1Speed(0.3*127);// left
@@ -1256,12 +1255,14 @@
 
 void slightleft(void)
 {
-
+    motors.begin();
     leftEncoder.reset();
     rightEncoder.reset();
     motors.setMotor0Speed(0.5*127);// right
-    motors.setMotor1Speed(-0.5*127);// left
-    while(abs(leftEncoder.getPulses())<70 || rightEncoder.getPulses()<70);
+    motors.setMotor1Speed(0.5*127);// left
+    while(abs(leftEncoder.getPulses())<50 || abs(rightEncoder.getPulses())<50){
+        
+    }
     motors.stopBothMotors(127);
 }
 
@@ -1343,16 +1344,13 @@
     wait_ms(200);
     motors.stopBothMotors(127);
     wait_ms(500);
+    
     if(section!= RIGS) {
         range = 0;
 
         do {
-            rangeFinderFront.startMeas();
-            wait_ms(1);
-            while(rangeFinderFront.getMeas(range) != RANGE_MEAS_INVALID);
-
-            pc.printf("Ultrasonic front sensor %f\r\n", range);
-            if(range < 9) {
+            pc.printf("front left %f \t front right %f\r\n", IRFrontL.getIRDistance(),IRFrontR.getIRDistance());
+            if(IRFrontL.getIRDistance() < 20 && IRFrontR.getIRDistance() < 20) {
                 motors.stopBothMotors(127);
                 wait_ms(200);
                 break;
@@ -1364,7 +1362,7 @@
             motors.stopBothMotors(127);
             wait_ms(500);
 
-        } while(range > 9);
+        } while(IRFrontL.getIRDistance() > 20 && IRFrontR.getIRDistance() > 20);
 
         motors.setMotor1Speed(0.1*127);//left
         motors.setMotor0Speed(0.1*127);//right
@@ -1379,21 +1377,25 @@
 
 
         while(leftEncoder.getPulses()<500 || rightEncoder.getPulses()<500) {
+            pc.printf("left %d \t right %d \r\n", motors.GetMotor0Current(), motors.GetMotor1Current());
             motors.setMotor1Speed(0.1*127);//left
             motors.setMotor0Speed(0.1*127);//right
             wait_ms(220);
+            
             if(motors.GetMotor0Current()>5 || motors.GetMotor1Current() >5) {
-                motors.setMotor1Speed(0.3*127);//left
-                motors.setMotor0Speed(0.3*127);//right
+                pc.printf("fast left %d \t right %d \r\n", motors.GetMotor0Current(), motors.GetMotor1Current());
+                motors.setMotor1Speed(0.35*127);//left
+                motors.setMotor0Speed(0.35*127);//right
                 wait_ms(220);
                 motors.stopBothMotors(127);
-                wait_ms(200);
+                wait_ms(500);
                 leftEncoder.reset();
                 rightEncoder.reset();
             }
-            motors.stopBothMotors(127);
-            wait_ms(200);
+
         }
+        motors.stopBothMotors(127);
+        wait_ms(200);
     }
 
 
@@ -1423,11 +1425,11 @@
 void from_tools_section(float* location, float &current)
 {
 
-    pc.printf("IR front %f \t back %f \r\n", IRFront.getIRDistance(), IRBack.getIRDistance());
+    pc.printf("IR front %f \t back %f \r\n", IRLeftFront.getIRDistance(), IRLeftBack.getIRDistance());
 
-    if(IRFront.getIRDistance() < 37 || IRBack.getIRDistance() < 37) {
+    if(IRLeftFront.getIRDistance() < 40 || IRLeftBack.getIRDistance() < 40) {
         wall_follow2(LEFT,BACKWARD,TOOLS, current,0);
-        pc.printf("IR front %f \t back %f \r\n", IRFront.getIRDistance(), IRBack.getIRDistance());
+        pc.printf("IR front %f \t back %f \r\n", IRLeftFront.getIRDistance(), IRLeftBack.getIRDistance());
         location[0]= current - ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
         current= location[0];
         //pc.printf("current %f \r\n",current);
@@ -1436,10 +1438,9 @@
 
         wait_ms(100);
         leftTurn();
-        slightleft();
         overBump(TOOLS);
     } else {
-        pc.printf("IR front %f \t back %f \r\n", IRFront.getIRDistance(), IRBack.getIRDistance());
+        pc.printf("IR front %f \t back %f \r\n", IRLeftFront.getIRDistance(), IRLeftBack.getIRDistance());
         //pc.printf("else greater than 20\r\n");
         location[0]= current;
         leftTurn();
@@ -1453,11 +1454,7 @@
 {
     wait_ms(500);
 
-    rangeFinderFront.startMeas();
-    wait_ms(20);
-    rangeFinderFront.getMeas(range);
-
-    if(range > 20) {
+    if(IRFrontL.getIRDistance() > 40 && IRFrontR.getIRDistance() > 40) {
         leftEncoder.reset();
         rightEncoder.reset();
         motors.setMotor0Speed(0.25*127); //right
@@ -1485,7 +1482,7 @@
 
     wait_ms(500);
 
-    if(IRFront.getIRDistance() > 37 && IRBack.getIRDistance() > 37) {
+    if(IRLeftFront.getIRDistance() > 40 && IRLeftBack.getIRDistance() > 40) {
         wait(1);
         direction[0]= RIGHT;
         location[1]= current;
@@ -1526,11 +1523,7 @@
     //pc.printf("mid section 2\r\n");
     wait_ms(500);
 
-    rangeFinderFront.startMeas();
-    wait_ms(20);
-    rangeFinderFront.getMeas(range);
-
-    if(range > 20) {
+    if(IRFrontL.getIRDistance() > 40 && IRFrontR.getIRDistance() > 40) {
         leftEncoder.reset();
         rightEncoder.reset();
         motors.setMotor0Speed(0.25*127); //right
@@ -1545,7 +1538,6 @@
     wait_ms(100);
 
     rightTurn();
-    //slightright();
     wait_ms(100);
 
 
@@ -1556,7 +1548,7 @@
 
     //pc.printf("midseection 2 after wf2 %f",current);
 
-    if(IRFront.getIRDistance() > 37 && IRBack.getIRDistance() > 37) {
+    if(IRLeftFront.getIRDistance() > 40 && IRLeftBack.getIRDistance() > 40) {
         direction[1]= RIGHT;
         location[2]= current;
         slightMove(FORWARD,150);
@@ -1771,15 +1763,20 @@
 void testSensors(void)
 {
     float range, range2;
+    
 
     while(1) {
-        rangeFinderFront.startMeas();
-        rangeFinderRight.startMeas();
+        
+        //pc.printf("us front %f \t us right %f \t IR front %f \t IR back %f\r\n", range, range2, IRLeftFront.getIRDistance(), IRLeftBack.getIRDistance());
+        
+        pc.printf("IR LEFT front %f \t",IRLeftFront.getIRDistance());
+        pc.printf("IR left back %f \t",IRLeftBack.getIRDistance());
+        
+        pc.printf("IR right front %f \t",IRRightFront.getIRDistance());
+        pc.printf("IR right back %f \t",IRRightBack.getIRDistance());
+
+        pc.printf("IR front left %f \t",IRFrontL.getIRDistance());
+        pc.printf("IR front right %f \r\n",IRFrontR.getIRDistance());
         wait_ms(20);
-        rangeFinderFront.getMeas(range);
-        rangeFinderRight.getMeas(range2);
-
-        pc.printf("us front %f \t us right %f \t IR front %f \t IR back %f\r\n", range, range2, IRFront.getIRDistance(), IRBack.getIRDistance());
-        //pc.printf("IR front %f \t IR back %f\r\n", IRFront.getIRDistance(), IRBack.getIRDistance());
     }
 }
\ No newline at end of file