uyvug
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of theRobot by
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 ¤t); void to_tools_section1(float* location, float ¤t); void to_tools_section2(float* location, float ¤t); void from_tools_section(float* location, float ¤t); void mid_section(float* location, float ¤t, int* direction); void mid_section2(float* location, float ¤t, int* direction); void rig_section(float* location, float ¤t, int* direction, int rig); -void tools_section_return(float* location, float ¤t); void mid_section_return(float* location, float ¤t, int* direction); void mid_section2_return(float* location, float ¤t, int* direction); void rig_section_return(float* location, float ¤t, 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 ¤t) { - //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 ¤t) -{ - 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 ¤t, 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 ¤t, 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