Diff: main.cpp

Revision:

27:5540aa3c828c

Parent:

26:7257bd16bc67

Child:

28:2bb6b0fe39d0

--- a/main.cpp	Mon Apr 14 12:17:10 2014 +0000
+++ b/main.cpp	Tue Apr 15 17:13:35 2014 +0000
@@ -118,13 +118,14 @@
 extern Serial lrf;                         //Laser Range Finder    lrf(p13,p14)
 //Hardware Initialization
 //Serial bt(p13,p14);
-HCSR04 rangeFinderLeft( PIN_TRIGGERL, PIN_ECHOL );
+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);
+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 IR(p20);
+Sharp IRFront(p19);
+Sharp IRBack(p20);
 //InterruptIn encoder(p29);
  
  
@@ -144,20 +145,19 @@
  
  
 //Navigation Functions
-void wall_follow(int side, int direction, int section);
 void wall_follow2(int side, int direction, int section, float location, int rig);
 void leftTurn(void);
 void slightleft(void);
 void slightright(void);
 void rightTurn(void);
 void slightMove(int direction, float pulses);
+void tools_section_return(float* location, float &current);
 void to_tools_section1(float* location, float &current);
 void to_tools_section2(float* location, float &current);
 void from_tools_section(float* location, float &current);
 void mid_section(float* location, float &current, int* direction);
 void mid_section2(float* location, float &current, int* direction);
 void rig_section(float* location, float &current, int* direction, int rig);
-void tools_section_return(float* location, float &current);
 void mid_section_return(float* location, float &current, int* direction);
 void mid_section2_return(float* location, float &current, int* direction);
 void rig_section_return(float* location, float &current, int* direction);
@@ -165,7 +165,7 @@
 void alignWithWall(int section);
 void UntilWall(int dir);
  
- 
+void testSensors(void); 
 float normd(int* pop, int count, int threshold);
 int Xadjust(int tool);
  
@@ -178,7 +178,7 @@
 int fire = 0;
 int tool_needed = 1;
 int shape_detected = 0;
-float range, range2, pid_return;
+float range, range2, range3, pid_return;
 int num, input;
  
  
@@ -273,6 +273,8 @@
  
     pc.baud(115200);
     
+    //testSensors();
+    
     //Laser Range Finder Initialization
     lrf_baudCalibration();
  
@@ -837,14 +839,7 @@
     setServoPulse(4, Arm_Table[set].claw_rotate);
     setServoPulse(5, Arm_Table[set].claw_open);
 }
- 
- 
- 
- 
- 
- 
- 
- 
+
 int fire_checker(int rig)
 {
     switch (rig) {
@@ -890,86 +885,12 @@
 }
  
  
- 
- 
-void wall_follow(int side, int direction, int section)
-{
-    float location, set=4;
-    int dir=1;
- 
-    pid1.reset();
- 
-    if(direction == BACKWARD) dir=-1;
-    if(section == TOOLS)set= 10;
- 
-    leftEncoder.reset();
-    rightEncoder.reset();
- 
-    location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
- 
-    while(location< 66.5) {
-        location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
- 
-        pid1.setInputLimits(0, set);
-        pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
-        pid1.setSetPoint(set);
-        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);
-        }
- 
-        if(range > 15) {
-            //pc.printf("wavegap %f\r\n",wavegap);
-            // AT WAVE OPENING!!!!
-            motors.setMotor1Speed(dir*0.25*127);//left
-            motors.setMotor0Speed(dir*0.25*127);//right
-        } else {
- 
-            pid1.setProcessValue(range);
-            pid_return = pid1.compute();
- 
-            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.setMotor0Speed(dir*MAX_SPEED);//right
-                motors.setMotor1Speed(dir*MAX_SPEED);//left
-            }
-        }
-    }
- 
-    //STOP
-    motors.setMotor0Speed(dir*-0.3*127); //right
-    motors.setMotor1Speed(dir*-0.3*127); //left
-    wait_ms(10);
-    motors.stopBothMotors(0);
-}
- 
 /* MODIFIED WALL_FOLLOW FOR NAVIGATION */
  
 void wall_follow2(int side, int direction, int section, float location, int rig)
 {
     int dir=1, limit=80, lowlim=4;
-    float set=9, loc=0, Rigloc=0, location_cal=0;
+    float set=28, loc=0, Rigloc=0, location_cal=0;
     bool SeeWaveGap = false;
  
     if(rig == 1) Rigloc= 16;
@@ -981,7 +902,7 @@
     if(section == TOOLS) {
         limit = 100;
         lowlim=10;
-    }else if(section == RIGS) set = 6;
+    }else if(section == RIGS) set = 18;
     else if(section == MID2) limit =85;
  
     if(direction == BACKWARD) {
@@ -994,7 +915,7 @@
     leftEncoder.reset();
     rightEncoder.reset();
  
-    pc.printf("before %f\r\n", location);
+    //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);
@@ -1013,9 +934,8 @@
         pid1.setSetPoint(set);
  
         if(side) {
-            rangeFinderLeft.startMeas();
-            wait_ms(20);
-            rangeFinderLeft.getMeas(range);
+            range = IRFront.getIRDistance();
+            range3= IRBack.getIRDistance();
         } else {
             rangeFinderRight.startMeas();
             wait_ms(20);
@@ -1024,13 +944,13 @@
  
         if(section == RIGS) {
             if(side == LEFT){
+                range2 = IRFront.getIRDistance();
+                range3= IRBack.getIRDistance();
+
+            }else{
                 rangeFinderRight.startMeas();
                 wait_ms(20);
                 rangeFinderRight.getMeas(range2);
-            }else{
-                rangeFinderLeft.startMeas();
-                wait_ms(20);
-                rangeFinderLeft.getMeas(range2);
             }
  
             if(range2< 15) {
@@ -1045,28 +965,19 @@
  
         //pc.printf("wall follow 2 range %f\r\n",range);
         //pc.printf("loc+location = %f\r\n", loc+location);
-        if(range > 15 ) {
-            if(section == RIGS || section == TOOLS) {
-                motors.setMotor0Speed(dir*0.25*127); //right
-                motors.setMotor1Speed(dir*0.25*127); //left
-            } else {
-                if(!SeeWaveGap) {
-                    wait_ms(75);
-                    SeeWaveGap=true;
-                } else {
-                    //STOP
-                    motors.stopBothMotors(127);
- 
-                    //pc.printf("wavegap\r\n");
-                    // AT WAVE OPENING!!!!
-                    break;
-                }
+        if(range > 35 && range3 > 35) {
+            if(section != RETURN) {
+                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);
+            //pc.printf("Range: %f\n      PID:   %f\r\n", range, pid_return);
  
             if(pid_return > 0) {
                 if(side) {
@@ -1180,11 +1091,8 @@
     } else if(section == MID2) {
 
         // check distance to wall
-        rangeFinderLeft.startMeas();
-        wait_ms(20);
-        rangeFinderLeft.getMeas(range);
 
-        if(range > 4) {
+        if(IRFront.getIRDistance() > 4) {
 
             leftEncoder.reset();
             rightEncoder.reset();
@@ -1230,9 +1138,7 @@
                 wait_ms(20);
                 rangeFinderRight.getMeas(range);
             } else {
-                rangeFinderLeft.startMeas();
-                wait_ms(20);
-                rangeFinderLeft.getMeas(range);
+                range = IRFront.getIRDistance();
             }
             //pc.printf("Range %f \t OldValue %f\n\r",range, usValue);
             if(range > usValue && usValue != 0 && range < 25) {
@@ -1253,6 +1159,37 @@
     motors.setMotor1Speed(0.5*127);//left
     while(abs(leftEncoder.getPulses())<800 || abs(rightEncoder.getPulses())<800);
     motors.stopBothMotors(127);
+    wait_ms(300);
+    
+    if(IRFront.getIRDistance() > 35 && IRBack.getIRDistance() > 35) return;
+    // align with wave
+    while(IRFront.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 ){
+                pc.printf("back sensor sees gap\r\n");
+        motors.setMotor0Speed(0.1*127);//right
+        motors.setMotor1Speed(0.1*127);//left        
+    }
+    motors.stopBothMotors(127);
+    wait_ms(300);
+    
+    while(IRFront.getIRDistance() - IRBack.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){
+                                pc.printf("turn right\r\n");
+            motors.setMotor0Speed(-0.3*127);//right
+            motors.setMotor1Speed(0.3*127);// left            
+    }
+    
+    motors.stopBothMotors(127);
+    wait_ms(300);
+    
 }
  
 void leftTurn(void)
@@ -1264,6 +1201,38 @@
     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;
+    // align with wave
+    while(IRFront.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 ){
+                pc.printf("back sensor sees gap\r\n");
+        motors.setMotor0Speed(0.1*127);//right
+        motors.setMotor1Speed(0.1*127);//left        
+    }
+    motors.stopBothMotors(127);
+    wait_ms(300);
+    
+    while(IRFront.getIRDistance() - IRBack.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){
+                                pc.printf("turn right\r\n");
+            motors.setMotor0Speed(-0.3*127);//right
+            motors.setMotor1Speed(0.3*127);// left            
+    }
+    
+    motors.stopBothMotors(127);
+    wait_ms(300);   
+    
 }
  
 void slightleft(void)
@@ -1326,6 +1295,9 @@
  
 void overBump(int section)
 {
+    float avg=0;
+    int i;
+    
     // first set
     motors.setMotor1Speed(0.1*127);//left
     motors.setMotor0Speed(0.1*127);//right
@@ -1333,7 +1305,6 @@
     while(motors.GetMotor0Current()<5 || motors.GetMotor1Current() <5 );
 
     while(motors.GetMotor0Current()>5 || motors.GetMotor1Current() >5 ) {
-        pc.printf("current %f\r\n",IR.getIRDistance());
         pc.printf("left %d \t right %d \r\n", motors.GetMotor0Current(), motors.GetMotor1Current());
         motors.setMotor1Speed(0.3*127);//left
         motors.setMotor0Speed(0.3*127);//right
@@ -1352,17 +1323,29 @@
     // second set
     wait_ms(200);
     motors.stopBothMotors(127);
-    wait_ms(300);
+    wait_ms(500);
     if(section!= RIGS) {
+        range = 0;
+        
+        do{
+            rangeFinderFront.startMeas();
+            wait_ms(1);
+            while(rangeFinderFront.getMeas(range) != RANGE_MEAS_INVALID);
 
-        while(IR.getIRDistance() >30) {
-            pc.printf("%f\r\n",IR.getIRDistance());
+            pc.printf("Ultrasonic front sensor %f\r\n", range);
+            if(range < 9){
+                motors.stopBothMotors(127);
+                wait_ms(200);
+                break;
+            }
+
             motors.setMotor1Speed(0.3*127);//left
             motors.setMotor0Speed(0.3*127);//right
             wait_ms(220);
             motors.stopBothMotors(127);
-            wait_ms(200);
-        }
+            wait_ms(500);
+            
+        }while(range > 9); 
         
         motors.setMotor1Speed(0.1*127);//left
         motors.setMotor0Speed(0.1*127);//right
@@ -1421,26 +1404,11 @@
 void from_tools_section(float* location, float &current)
 {
  
-    //alignWithWall(TOOLS);
-    //current-=4;
- 
-    //slightMove(FORWARD,150);
-    //current+=1;
-    //pc.printf("align\r\n");
-    //wait_ms(200);
- 
-    //wall_follow2(LEFT,FORWARD,MID, current);
- 
-    slightMove(BACKWARD,500);
-    current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
- 
-    rangeFinderLeft.startMeas();
-    wait_ms(20);
-    rangeFinderLeft.getMeas(range);
- 
-    if(range < 15) {
-        wall_follow2(LEFT,BACKWARD,MID, current,0);
-        //pc.printf("wall follow\r\n");
+    pc.printf("IR front %f \t back %f \r\n", IRFront.getIRDistance(), IRBack.getIRDistance());
+    
+    if(IRFront.getIRDistance() < 37 || IRBack.getIRDistance() < 37) {
+        wall_follow2(LEFT,BACKWARD,TOOLS, current,0);
+       pc.printf("IR front %f \t back %f \r\n", IRFront.getIRDistance(), IRBack.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);
@@ -1452,75 +1420,25 @@
         slightleft();
         overBump(TOOLS);
     } else {
+        pc.printf("IR front %f \t back %f \r\n", IRFront.getIRDistance(), IRBack.getIRDistance());
         //pc.printf("else greater than 20\r\n");
         location[0]= current;
         leftTurn();
         overBump(TOOLS);
     }
- 
-    //pc.printf("First Wavegap = %f\r\n",location[0]);
- 
-}
-void tools_section(float* location, float &current)
-{
-    wall_follow(LEFT,FORWARD, TOOLS);
-    // current position in reference to the starting position
-    current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
- 
-    //////////////////////////////// determine tool
-    wait(2);
-    ///////////////////////////////////////////////////////////////////////////////////////
-    // Move Forward
-    slightMove(FORWARD, 100);
-    current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
- 
-    //////////////////////////////////////////Tool aquiring
-    wait(2);
-    //////////////////////////////////////////////////////////////////// After tool is aquired
- 
-    //alignWithWall(TOOLS);
-    //pc.printf("align\r\n");
-    //wait_ms(100);
- 
-    //wall_follow2(LEFT,FORWARD,MID, current);
-    //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+    pc.printf("First Wavegap = %f\r\n",location[0]);
  
-    rangeFinderLeft.startMeas();
-    wait_ms(20);
-    rangeFinderLeft.getMeas(range);
- 
-    if(range < 20) {
-        wall_follow2(LEFT,BACKWARD,TOOLS, current,0);
-        //pc.printf("wall follow\r\n");
-        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);
-        // go backwards
-        leftEncoder.reset();
-        rightEncoder.reset();
-        motors.setMotor0Speed(-MAX_SPEED); //right
-        motors.setMotor1Speed(-MAX_SPEED); //left
-        while(abs(leftEncoder.getPulses()) < 120 || abs(rightEncoder.getPulses())< 120);
-        // hard stop
- 
-        motors.stopBothMotors(127);
- 
-        wait_ms(100);
-        leftTurn();
-        overBump(TOOLS);
-    } else {
-        //pc.printf("else greater than 20\r\n");
-        location[0]= current;
-        leftTurn();
-        overBump(TOOLS);
-    }
- 
-    //pc.printf("First Wavegap = %f\r\n",location[0]);
 }
  
 void mid_section(float* location, float &current, int* direction)
-{
-    if(IR.getIRDistance() > 38) {
+{   
+    wait_ms(500);
+    
+    rangeFinderFront.startMeas();
+    wait_ms(20);
+    rangeFinderFront.getMeas(range);
+    
+    if(range > 20) {
         leftEncoder.reset();
         rightEncoder.reset();
         motors.setMotor0Speed(0.25*127); //right
@@ -1547,11 +1465,8 @@
     //pc.printf("after wf2 current = %f\r\n",current);
  
     wait_ms(500);
-    rangeFinderLeft.startMeas();
-    wait_ms(20);
-    rangeFinderLeft.getMeas(range);
  
-    if(range > 20 ) {
+    if(IRFront.getIRDistance() > 37 && IRBack.getIRDistance() > 37) {
         wait(1);
         direction[0]= RIGHT;
         location[1]= current;
@@ -1591,8 +1506,13 @@
 void mid_section2(float* location, float &current, int* direction)
 {
     //pc.printf("mid section 2\r\n");
- 
-    if(IR.getIRDistance() > 38) {
+    wait_ms(500);
+    
+    rangeFinderFront.startMeas();
+    wait_ms(20);
+    rangeFinderFront.getMeas(range);
+    
+    if(range > 20) {
         leftEncoder.reset();
         rightEncoder.reset();
         motors.setMotor0Speed(0.25*127); //right
@@ -1617,11 +1537,8 @@
     wait_ms(500);
  
     //pc.printf("midseection 2 after wf2 %f",current);
-    rangeFinderLeft.startMeas();
-    wait_ms(20);
-    rangeFinderLeft.getMeas(range);
  
-    if(range > 20 ) {
+    if(IRFront.getIRDistance() > 37 && IRBack.getIRDistance() > 37) {
         direction[1]= RIGHT;
         location[2]= current;
         slightMove(FORWARD,150);
@@ -1831,4 +1748,19 @@
     return mean;
     //return (1/(std*sqrt(2*PI)))*exp(-pow(((float)threshold-mean),2)/(2*pow(std,2)));
  
+}
+
+void testSensors(void){
+    float range, range2;
+    
+    while(1){
+        rangeFinderFront.startMeas();
+        rangeFinderRight.startMeas();
+        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