Justin Carr
Yeah
Dependencies: HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of
NavigationTest
by 
Paolina Povolotskaya
Diff: main.cpp
- Revision:
- 12:081b8fc654af
- Parent:
- 11:12ce7600f2f9
- Child:
- 13:b6480275c445
--- a/main.cpp Thu Mar 27 21:09:55 2014 +0000
+++ b/main.cpp Thu Mar 27 22:48:00 2014 +0000
@@ -20,11 +20,13 @@
#define PPR (4331/4)
#define LEFT (1)
#define RIGHT (0)
+#define STRAIGHT (2)
#define FORWARD (1)
#define BACKWARD (0)
#define TOOLS (0)
#define MID (1)
#define RIGS (2)
+#define RETURN (3)
#define FIRST_WAVE (0)
#define FAR (1)
@@ -59,8 +61,14 @@
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, int rig);
void overBump(int section);
void alignWithWall(int section);
+void UntilWall(int dir);
+
//Variables
@@ -79,15 +87,15 @@
tools_section(location, current);
mid_section(location, current, direction);
//mid_section2(location, current, direction);
- /* while(1) {
- //bt.printf("IR %f\r\n", US.getDistance());
- rangeFinderLeft.startMeas();
- wait_ms(20);
- rangeFinderLeft.getMeas(range);
- bt.printf("Range = %f\n\r", range);
+ /* while(1) {
+ //bt.printf("IR %f\r\n", US.getDistance());
+ rangeFinderLeft.startMeas();
+ wait_ms(20);
+ rangeFinderLeft.getMeas(range);
+ bt.printf("Range = %f\n\r", range);
- wait_ms(200);
- }*/
+ wait_ms(200);
+ }*/
//leftTurn();
@@ -127,6 +135,11 @@
location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
while(location< 73) {
+
+ if(section == RETURN) {
+ if(location <10) break;
+ }
+
location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
pid1.setInputLimits(0, set);
@@ -183,18 +196,18 @@
void wall_follow2(int side, int direction, int section, float location)
{
- bool SeeWaveGap = false, dir=1, limit;
+ bool SeeWaveGap = false, dir=1, limit=83;
float set=5, loc=0;
pid1.reset();
if(direction == BACKWARD) dir=-1;
- if(section == TOOLS){
+ if(section == TOOLS) {
set= 5;
limit = 78;
}
- if(section == TOOLS) limit = 83;
-
+
+
leftEncoder.reset();
rightEncoder.reset();
@@ -215,46 +228,57 @@
rangeFinderRight.getMeas(range);
}
+ if(section == RIGS) {
+ rangeFinderRight.startMeas();
+ wait_ms(38);
+ rangeFinderRight.getMeas(range);
+ if(range< 20) {
+ motors.stopBothMotors();
+ break;
+ }
+ }
+
//bt.printf("wall follow 2 range %f\r\n",range);
//bt.printf("loc+location = %f\r\n", loc+location);
if(range > 20 && !SeeWaveGap) {
- motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
- motors.setMotor1Speed(dir*MAX_SPEED);//left
- wait_ms(30);
- SeeWaveGap = true;
- } else if(range > 20 && SeeWaveGap){
- motors.stopBothMotors();
+ motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
+ motors.setMotor1Speed(dir*MAX_SPEED);//left
+ wait_ms(30);
+ SeeWaveGap = true;
+ } else if(range > 20 && SeeWaveGap) {
+ motors.stopBothMotors();
bt.printf("wavegap\r\n");
// AT WAVE OPENING!!!!
break;
} else {
- pid1.setProcessValue(range);
- pid_return = pid1.compute();
- //bt.printf("Range: %f\n PID: %f\r\n", range, pid_return);
+ pid1.setProcessValue(range);
+ pid_return = pid1.compute();
+ //bt.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);
}
- }}
+ }
motors.stopBothMotors();
}
@@ -343,8 +367,9 @@
motors.stopBothMotors();
}
-void slightleft(void){
-
+void slightleft(void)
+{
+
leftEncoder.reset();
rightEncoder.reset();
motors.setMotor0Speed(0.5*127);// right
@@ -353,32 +378,55 @@
motors.stopBothMotors();
}
+void UntilWall(int dir)
+{
+
+ if(dir == BACKWARD) dir=-1;
+
+ leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor0Speed(dir*0.2*127); //right
+ motors.setMotor1Speed(dir*0.2*127); //left
+
+ range = 30;
+
+ while(range > 20) {
+ rangeFinderRight.startMeas();
+ wait_ms(20);
+ rangeFinderRight.getMeas(range);
+ }
+
+ motors.setMotor0Speed(dir*-0.2*127); //right
+ motors.setMotor1Speed(dir*-0.2*127); //left
+ wait_ms(5);
+ motors.stopBothMotors();
+}
void overBump(int section)
{
int preLeft=5000, preRight=5000, out=0;
-
+
motors.begin();
-
+
leftEncoder.reset();
rightEncoder.reset();
motors.setMotor0Speed(-0.2*127); //right
motors.setMotor1Speed(-0.2*127); //left
while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses())< 50);
motors.stopBothMotors();
-
+
leftEncoder.reset();
rightEncoder.reset();
motors.setMotor0Speed(0.2*127); //right
motors.setMotor1Speed(0.2*127); //left
- while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.getDistance() >20 && preLeft!=0){
- preLeft=leftEncoder.getPulses();
- preRight=rightEncoder.getPulses();
- wait_ms(100);
- //bt.printf(" first while left %d right %d \r\n", preLeft, preRight);
- if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
+ while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.getDistance() >20 && preLeft!=0) {
+ preLeft=leftEncoder.getPulses();
+ preRight=rightEncoder.getPulses();
+ wait_ms(100);
+ //bt.printf(" first while left %d right %d \r\n", preLeft, preRight);
+ if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
}
-
+
motors.stopBothMotors();
motors.begin();
wait(2);
@@ -397,7 +445,7 @@
wait_ms(200);
if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
}*/
-
+
leftEncoder.reset();
rightEncoder.reset();
motors.setMotor0Speed(0.3*127); //right
@@ -418,13 +466,13 @@
motors.setMotor0Speed(0.4*127); //right
motors.setMotor1Speed(0.4*127); //left
}
- if(abs(leftEncoder.getPulses()) <1000 || abs(leftEncoder.getPulses())<1000) out=1;
+ if(abs(leftEncoder.getPulses()) >1000 || abs(leftEncoder.getPulses())>1000) out=1;
}
motors.stopBothMotors();
wait(2);
motors.begin();
-
+
preLeft=preRight=5000 ;
leftEncoder.reset();
rightEncoder.reset();
@@ -454,35 +502,49 @@
current=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
bt.printf("current %f \r\n",current);
+
+ motors.setMotor0Speed(-.2*127); //right
+ motors.setMotor1Speed(-.2*127); //left
+ wait_ms(5);
motors.stopBothMotors();
+ motors.setMotor0Speed(.2*127); //right
+ motors.setMotor1Speed(.2*127); //left
+ while(IR.getDistance()>6);
+
+ motors.setMotor0Speed(-.2*127); //right
+ motors.setMotor1Speed(-.2*127); //left
+ wait_ms(5);
+ motors.stopBothMotors();
+
+
//Tool aquiring
wait(2);
// After tool is aquired
alignWithWall(TOOLS);
-
+
wait_ms(100);
-
+
wall_follow2(LEFT,FORWARD,MID, current);
current= 78;
-
+
rangeFinderLeft.startMeas();
wait_ms(20);
rangeFinderLeft.getMeas(range);
if(range < 20) {
wall_follow2(LEFT,BACKWARD,TOOLS, current);
-
+
location[0]= current - ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
current= location[0];
-
+
leftEncoder.reset();
rightEncoder.reset();
motors.setMotor0Speed(-MAX_SPEED); //right
motors.setMotor1Speed(-MAX_SPEED); //left
while(abs(leftEncoder.getPulses()) < 120 || abs(rightEncoder.getPulses())< 120);
-
+
leftEncoder.reset();
rightEncoder.reset();
motors.setMotor0Speed(MAX_SPEED); //right
@@ -508,9 +570,12 @@
{
motors.begin();
-
- if(IR.getDistance() > 20) return;
-
+
+ if(IR.getDistance() > 20) {
+ direction[0]= STRAIGHT;
+ return;
+ }
+
alignWithWall(MID);
bt.printf("mid section current = %f\r\n",current);
@@ -546,9 +611,12 @@
{
motors.begin();
-
- if(IR.getDistance() > 20) return;
-
+
+ if(IR.getDistance() > 20) {
+ direction[0]= STRAIGHT;
+ return;
+ }
+
alignWithWall(MID);
wall_follow2(LEFT,FORWARD,MID, current);
current=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
@@ -570,6 +638,94 @@
void rig_section(float* location, float ¤t, int* direction, int rig)
{
+ float loc;
+
+ if(rig == 1) loc= 16;
+ else if(rig == 2) loc= 37;
+ else loc = 58;
+
+ 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);
+ 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);
+ }
+ }
+}
+
+void tools_section_return(float* location, float ¤t)
+{
+ if(location[0] > 16) {
+ rightTurn();
+ leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor0Speed(MAX_SPEED); //right
+ motors.setMotor1Speed(MAX_SPEED); //left
+ while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300);
+ wall_follow(RIGHT, FORWARD, RETURN);
+ }
+ motors.stopBothMotors();
+
+}
+
+void mid_section_return(float* location, float ¤t, int* direction)
+{
+ if(direction[1] == RIGHT) {
+ rightTurn();
+ alignWithWall(MID);
+ wall_follow2(LEFT, FORWARD, MID, current);
+ leftTurn();
+ } else if(direction[1] == LEFT) {
+ leftTurn();
+ wall_follow2(RIGHT, FORWARD, MID, current);
+ rightTurn();
+ } else {
+ //GO FORWARD
+ }
+ overBump(RIGS);
+}
+
+void mid_section2_return(float* location, float ¤t, int* direction)
+{
+ if(direction[2] == RIGHT) {
+ rightTurn();
+ wall_follow2(LEFT, FORWARD, MID, current);
+ leftTurn();
+ } else if(direction[2] == LEFT) {
+ leftTurn();
+ wall_follow2(RIGHT, FORWARD, MID, current);
+ rightTurn();
+ } else {
+ //GO FORWARD
+ }
+ overBump(MID);
+}
+
+void rig_section_return(float* location, float ¤t, int* direction, int rig)
+{
+ if(location[2] > current) {
+ wall_follow2(RIGHT, FORWARD, RIGS, current);
+ } else {
+ wall_follow2(RIGHT, BACKWARD, RIGS, current);
+ }
+ rightTurn();
+ overBump(MID);
}
\ No newline at end of file