Paolina Povolotskaya
/
NavigationTest
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: HCSR04 PID PololuQik2 QEI mbed-rtos Sharp
Revision 13:9bad7f74833a, committed 2014-03-30
- Comitter:
- Fairy_Paolina
- Date:
- Sun Mar 30 00:29:49 2014 +0000
- Parent:
- 12:168cb595f98e
- Child:
- 14:3c8c4efe4786
- Commit message:
- revise;
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Sat Mar 29 22:20:44 2014 +0000
+++ b/main.cpp Sun Mar 30 00:29:49 2014 +0000
@@ -49,8 +49,7 @@
//Functions
void wall_follow(int side, int direction, int section);
-void wall_follow2(int side, int direction, int section, float location);
-void wall_follow3(int ¤tLocation, int &WaveOpening);
+void wall_follow2(int side, int direction, int section, float location, int rig);
void leftTurn(void);
void slightleft(void);
void rightTurn(void);
@@ -82,7 +81,7 @@
tools_section(location, current);
mid_section(location, current, direction);
mid_section2(location, current, direction);
- rig_section(location, current, direction, 1);
+ rig_section(location, current, direction, 3);
/*while(1) {
pc.printf("IR %f\r\n", IR.getDistance());
/*rangeFinderLeft.startMeas();
@@ -186,10 +185,14 @@
/* MODIFIED WALL_FOLLOW FOR NAVIGATION */
-void wall_follow2(int side, int direction, int section, float location)
+void wall_follow2(int side, int direction, int section, float location, int rig)
{
- int dir=1, limit=89;
- float set=6, loc=0;
+ int dir=1, limit=90;
+ float set=6, loc=0, Rigloc=0;
+
+ if(rig == 1) Rigloc= 16;
+ else if(rig == 2) Rigloc= 45;
+ else if(rig== 3) Rigloc = 70;
pid1.reset();
@@ -205,7 +208,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);
@@ -229,12 +232,19 @@
}
if(section == RIGS){
- rangeFinderRight.startMeas();
+ rangeFinderLeft.startMeas();
wait_ms(20);
- rangeFinderRight.getMeas(range);
- if(range< 20){
- motors.stopBothMotors();
- break;
+ rangeFinderLeft.getMeas(range2);
+
+ if(range2< 20){
+ if( abs(dir*loc + location - Rigloc) < 10){
+ //STOP
+ motors.setMotor0Speed(dir*-0.25*127); //right
+ motors.setMotor1Speed(dir*-0.25*127); //left
+ wait_ms(5);
+ motors.stopBothMotors();
+ break;
+ }
}
}
@@ -242,10 +252,21 @@
//pc.printf("wall follow 2 range %f\r\n",range);
//pc.printf("loc+location = %f\r\n", loc+location);
if(range > 20 ) {
- motors.stopBothMotors();
- pc.printf("wavegap\r\n");
- // AT WAVE OPENING!!!!
- break;
+ if(section == RIGS){
+ motors.setMotor0Speed(dir*0.25*127); //right
+ motors.setMotor1Speed(dir*0.25*127); //left
+ }
+ else{
+ //STOP
+ motors.setMotor0Speed(dir*-0.25*127); //right
+ motors.setMotor1Speed(dir*-0.25*127); //left
+ wait_ms(5);
+ motors.stopBothMotors();
+
+ pc.printf("wavegap\r\n");
+ // AT WAVE OPENING!!!!
+ break;
+ }
} else {
pid1.setProcessValue(range);
@@ -274,6 +295,7 @@
}
}}
+ //STOP
motors.setMotor0Speed(dir*-0.25*127); //right
motors.setMotor1Speed(dir*-0.25*127); //left
wait_ms(5);
@@ -396,11 +418,9 @@
motors.setMotor1Speed(dir*0.25*127); //left
while(abs(leftEncoder.getPulses()) < pulses || abs(rightEncoder.getPulses()) < pulses);
- leftEncoder.reset();
- rightEncoder.reset();
motors.setMotor0Speed(dir*-0.25*127); //right
motors.setMotor1Speed(dir*-0.25*127); //left
- while(abs(leftEncoder.getPulses()) < 20 || abs(rightEncoder.getPulses()) < 20);
+ wait_ms(10);
motors.stopBothMotors();
}
@@ -560,7 +580,7 @@
rangeFinderLeft.getMeas(range);
if(range < 20) {
- wall_follow2(LEFT,BACKWARD,TOOLS, current);
+ 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];
@@ -606,7 +626,7 @@
wait_ms(100);
pc.printf("mid section current = %f\r\n",current);
- wall_follow2(LEFT,FORWARD,MID, current);
+ wall_follow2(LEFT,FORWARD,MID, current,0);
current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
pc.printf("after wf2 current = %f\r\n",current);
@@ -618,14 +638,17 @@
if(range > 20 ) {
direction[0]= RIGHT;
location[1]= current;
- slightMove(FORWARD,100);
+ //slightMove(FORWARD,200);
+ current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
} else {
direction[0]= LEFT;
- wall_follow2(LEFT,BACKWARD,MID,current);
+ wall_follow2(LEFT,BACKWARD,MID,current,0);
location[1]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
current= location[1];
+
if(location[1] < 18){
slightMove(FORWARD, 50);
+ current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
}
}
@@ -654,7 +677,7 @@
alignWithWall(MID);
pc.printf("midsection 2 alignt with wall mid \r\n");
- wall_follow2(LEFT,FORWARD,MID, current);
+ wall_follow2(LEFT,FORWARD,MID, current,0);
current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
pc.printf("midseection 2 after wf2");
@@ -666,12 +689,13 @@
direction[1]= RIGHT;
location[2]= current;
slightMove(FORWARD,500);
+ current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
} else {
direction[1]= LEFT;
- wall_follow2(LEFT,BACKWARD,MID,current);
+ wall_follow2(LEFT,BACKWARD,MID,current,0);
location[2]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
current=location[2];
- slightMove(BACKWARD,100);
+ //slightMove(FORWARD,500);
}
leftTurn();
@@ -683,33 +707,20 @@
{
float loc;
- if(rig == 1) loc= 16;
+ if(rig == 1) loc= 15;
else if(rig == 2) loc= 45;
- else loc = 70;
+ else loc = 75;
- alignWithWall(RIGS);
+ rightTurn();
if(current > loc){
- UntilWall(BACKWARD);
- current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
- wall_follow2(RIGHT, BACKWARD, RIGS, current);
+ pc.printf("RIG section %f\r\n",current);
+ wall_follow2(RIGHT, BACKWARD, RIGS, current, rig);
current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
-
- if((current- loc)>10){
- wall_follow2(RIGHT, BACKWARD, RIGS, current);
- current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
- }
}
else{
- UntilWall(FORWARD);
- wall_follow2(RIGHT, FORWARD, RIGS, current);
- current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
-
- if((current- loc)<-10){
- wall_follow2(RIGHT, FORWARD, RIGS, current);
- current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
- }
+ pc.printf("RIG section %f\r\n",current);
+ wall_follow2(RIGHT, FORWARD, RIGS, current, rig);
+ current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
}
-
-
}
\ No newline at end of file