Thomas Ashworth
For IEEE Robotics
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Diff: main.cpp
- Revision:
- 12:284be46593ae
- Parent:
- 11:8d2455e383ce
- Child:
- 13:529323807361
--- a/main.cpp Tue Apr 01 02:00:01 2014 +0000
+++ b/main.cpp Wed Apr 02 00:30:30 2014 +0000
@@ -23,19 +23,21 @@
#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 FIRST_WAVE (0)
+#define MID2 (3)
+#define RETURN (4)
#define FAR (1)
//States
#define START 0
#define OILRIG1_POS 1
#define OILRIG2_POS 2
-#define GOTO_TOOLS 3
+#define GOTO_TOOLS1 3
#define IDENTIFY_TOOLS 4
#define AQUIRE_TOOL1 5
#define AQUIRE_TOOL2 6
@@ -49,6 +51,7 @@
#define RIG_ALIGN 14
#define INSERT_TOOL 15
#define END 16
+#define GOTO_TOOLS2 17
@@ -131,21 +134,26 @@
//Navigation Functions
-float 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_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 us_distance(void);
-void to_tools_section(float* location, float ¤t);
+void slightMove(int direction, float pulses);
+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);
void overBump(int section);
void alignWithWall(int section);
-void ledtoggle(void);
+void UntilWall(int dir);
/************
@@ -186,13 +194,13 @@
//increase in number 5 rotates gripper
{STORE_POSITION, 85, 5, 0, 165, 175, 0}, // storing position
- {OIL_RIG1, 162, 20, 60, 55, 100, 0}, // point laser at oilrig1
- {OIL_RIG2, 145, 20, 60, 55, 100, 0}, // point laser at oilrig2
- {OIL_RIG3, 130, 90, 90, 57, 100, 0}, // point laser at oilrig2
- {DRIVE_POSITION_NOTOOL, 85, 5, 0, 175, 175, 0}, // Drive through course
- {TOOL_1, 95, 40, 70, 128, 175, 0}, // Look over first tool
- {TOOL_2, 77, 40, 70, 128, 175, 0}, // Look over second tool
- {TOOL_3, 57, 40, 70, 128, 175, 0}, // Look over third tool
+ {OIL_RIG1, 160, 20, 60, 45, 175, 0}, // point laser at oilrig1
+ {OIL_RIG2, 163, 20, 60, 45, 175, 0}, // point laser at oilrig2
+ {OIL_RIG3, 130, 90, 90, 52, 175, 0}, // point laser at oilrig2
+ {DRIVE_POSITION_NOTOOL, 85, 5, 0, 165, 175, 0}, // Drive through course
+ {TOOL_1, 94, 50, 80, 109, 175, 0}, // Look over first tool
+ {TOOL_2, 77, 50, 80, 110, 175, 0}, // Look over second tool
+ {TOOL_3, 57, 50, 80, 109, 175, 0}, // Look over third tool
{DRIVE_POSITION_TOOL, 55, 70, 102, 74, 0, 0}, // Drive with tool loaded
{ORIENT_TOOL, 135, 60, 75, 60, 90, 90}, // position tool to be inserted
{PU_TOOL_1, 98, 50, 82, 118, 90, 0}, // STATIC Pickup tool POSITION
@@ -222,7 +230,7 @@
/*****************
INITIALIZATIONS
*******************/
- float location[3], current=6;
+ float location[3], current=4;
int direction[3];
double distance;
@@ -246,8 +254,8 @@
/*******************
WHILE LOOP FOR TESTING
********************/
- while(1) {
-
+ //while(1) {
+/*
pc.scanf("%d %d", &servoNum, &servoAngle);
if(servoAngle > 175) {
servoAngle = 175;
@@ -259,7 +267,7 @@
setServoPulse(servoNum, servoAngle);
distLaser = getDistance();
pc.printf("Distance %d", distLaser);
-
+*/
/*
while(pc.getc() != 'g');
servoPosition(TOOL_2);
@@ -349,7 +357,7 @@
}
printImageToFile(BINARY);*/
- }
+ // }
@@ -370,7 +378,7 @@
**************************************************/
case START :
myled1 = 1;
- wait(1);
+ while(pc.getc() != 'g');
myled1 = 0;
state = OILRIG1_POS;
break;
@@ -389,15 +397,17 @@
setServoPulse(0, Arm_Table[OIL_RIG1].base_rotate);
setServoPulse(4, Arm_Table[OIL_RIG1].claw_rotate);
setServoPulse(5, Arm_Table[OIL_RIG1].claw_open);
-
- wait(3); //wait for servos to settle
+ wait(3);
+ //lrf.putc('E'); // lighting calculation
+ //wait(15);
fire = fire_checker(OIL_RIG1); //determines if oil rig is on fire
- pc.printf("FIRE: %d", fire);
+
+ pc.printf("FIRE: SQUARE %d", fire);
//determines what tool is needed
if (fire == 1) {
tool_needed = SQUARE;
- state = GOTO_TOOLS;
+ state = GOTO_TOOLS1;
} else {
state = OILRIG2_POS;
}
@@ -405,16 +415,31 @@
break;
case OILRIG2_POS:
+
+
+ setServoPulse(2, Arm_Table[DRIVE_POSITION_NOTOOL].big_arm);
+ setServoPulse(3, Arm_Table[DRIVE_POSITION_NOTOOL].claw_arm);
+ setServoPulse(1, Arm_Table[DRIVE_POSITION_NOTOOL].base_arm);
+ setServoPulse(0, Arm_Table[DRIVE_POSITION_NOTOOL].base_rotate);
+ setServoPulse(4, Arm_Table[DRIVE_POSITION_NOTOOL].claw_rotate);
+ setServoPulse(5, Arm_Table[DRIVE_POSITION_NOTOOL].claw_open);
+
+ wait(3); //wait for servos to settle
+
+ to_tools_section2(location, current);
+
servoPosition(OIL_RIG2); //position arm to point at first oilrig
- wait(1); //wait for servos to settle
+ wait(2);
+ //lrf.putc('E'); // lighting calculation
+ //wait(15);
fire = fire_checker(OIL_RIG2);
- pc.printf("FIRE: %d", fire);
+ pc.printf("FIRE: TRIANGLE %d", fire);
if (fire == 1) {
tool_needed = TRIANGLE;
- state = GOTO_TOOLS;
+ state = GOTO_TOOLS2;
} else {
tool_needed = CIRCLE;
- state = GOTO_TOOLS;
+ state = GOTO_TOOLS2;
}
pc.printf("tool needed: %d", tool_needed);
break;
@@ -425,10 +450,25 @@
* - TRAVEL TO TOOLS
*
**************************************************/
- case GOTO_TOOLS:
+ case GOTO_TOOLS1:
+
+ setServoPulse(2, Arm_Table[DRIVE_POSITION_NOTOOL].big_arm);
+ setServoPulse(3, Arm_Table[DRIVE_POSITION_NOTOOL].claw_arm);
+ setServoPulse(1, Arm_Table[DRIVE_POSITION_NOTOOL].base_arm);
+ setServoPulse(0, Arm_Table[DRIVE_POSITION_NOTOOL].base_rotate);
+ setServoPulse(4, Arm_Table[DRIVE_POSITION_NOTOOL].claw_rotate);
+ setServoPulse(5, Arm_Table[DRIVE_POSITION_NOTOOL].claw_open);
+ wait(5); //wait for servos to settle
+ to_tools_section1(location, current);
+
+ state = IDENTIFY_TOOLS;
+ break;
+
+ case GOTO_TOOLS2:
+
+ setServoPulse(2, Arm_Table[DRIVE_POSITION_NOTOOL].big_arm);
setServoPulse(3, Arm_Table[DRIVE_POSITION_NOTOOL].claw_arm);
- setServoPulse(2, Arm_Table[DRIVE_POSITION_NOTOOL].big_arm);
setServoPulse(1, Arm_Table[DRIVE_POSITION_NOTOOL].base_arm);
setServoPulse(0, Arm_Table[DRIVE_POSITION_NOTOOL].base_rotate);
setServoPulse(4, Arm_Table[DRIVE_POSITION_NOTOOL].claw_rotate);
@@ -436,12 +476,12 @@
wait(5); //wait for servos to settle
- to_tools_section(location, current);
+ slightMove(FORWARD,3100);
+ current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
state = IDENTIFY_TOOLS;
- pc.printf("YES!!!!!");
- while(1);
break;
+
/**************************************************
* STAGE 3
*
@@ -450,21 +490,24 @@
*
**************************************************/
case IDENTIFY_TOOLS:
- servoPosition(TOOL_1); //arm/camera looks over tool
- wait(1); //wait for servos to settle
- centerCamWithTool(); //centers camera over tool
+
+ servoPosition(TOOL_2); //arm/camera looks over tool
+ wait(5); //wait for servos to settle
+ //centerCamWithTool(); //centers camera over tool
shape_detected = shapeDetection(); //determines the shape
+ clearBounds();
+ //printImageToFile(BINARY);
//either goes to aquire the tool or look at the next shape
if(shape_detected == tool_needed) {
- state = AQUIRE_TOOL1;
+ state = AQUIRE_TOOL2;
} else {
- servoPosition(TOOL_2);
+ servoPosition(TOOL_1);
wait(1); //wait for servos to settle
- centerCamWithTool();
+ //centerCamWithTool();
shape_detected = shapeDetection();
if (shape_detected == tool_needed) {
- state = AQUIRE_TOOL2;
+ state = AQUIRE_TOOL1;
} else {
servoPosition(TOOL_3);
wait(1); //wait for servos to settle
@@ -472,28 +515,29 @@
state = AQUIRE_TOOL3;
}
}
+ while(1);
break;
case AQUIRE_TOOL1:
- //*********************//
- //******* TODO ********//
- //*********************//
- // CODE TO GRAB TOOL1
+
+ servoPosition(PU_TOOL_1_STAB);
+ wait(2);
+
servoPosition(DRIVE_POSITION_TOOL); //arm position for driving with the tool
state = NAVIGATE_WAVES_ROW1;
break;
case AQUIRE_TOOL2:
- //*********************//
- //******* TODO ********//
- //*********************//
- // CODE TO GRAB TOOL2
+ servoPosition(PU_TOOL_2_STAB);
+ wait(2);
+
servoPosition(DRIVE_POSITION_TOOL); //arm position for driving with the tool
state = NAVIGATE_WAVES_ROW1;
break;
+
case AQUIRE_TOOL3:
- //*********************//
- //******* TODO ********//
- //*********************//
- // CODE TO GRAB TOOL3
+ servoPosition(PU_TOOL_3_STAB);
+ wait(2);
+
+
servoPosition(DRIVE_POSITION_TOOL); //arm position for driving with the tool
state = NAVIGATE_WAVES_ROW1;
break;
@@ -711,6 +755,7 @@
case 1:
for (int i = 0; i<5; i++) {
+ pc.printf("1");
distLaser = getDistance();
pc.printf("L DISTANCE: %d \n\r", distLaser);
if ((distLaser < OILRIG1_MAX)
@@ -769,20 +814,9 @@
{
//Nothing
}
-
-void us_distance(void)
+void wall_follow(int side, int direction, int section)
{
- pc.printf("Ultra Sonic\n\r");
- rangeFinderLeft.startMeas();
- wait_us(20);
- if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID)) {
- pc.printf("Range = %f\n\r", range);
- }
-}
-
-float wall_follow(int side, int direction, int section)
-{
- float location, wavegap=0, set=5;
+ float location, set=6;
int dir=1;
pid1.reset();
@@ -795,7 +829,7 @@
location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
- while(location< 70) {
+ while(location< 66.5) {
location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
pid1.setInputLimits(0, set);
@@ -803,21 +837,20 @@
pid1.setSetPoint(set);
if(side) {
rangeFinderLeft.startMeas();
- wait_ms(38);
+ wait_ms(20);
rangeFinderLeft.getMeas(range);
} else {
rangeFinderRight.startMeas();
- wait_ms(38);
+ wait_ms(20);
rangeFinderRight.getMeas(range);
pc.printf("%d\r\n",range);
}
- if(range > 20) {
- wavegap=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
+ if(range > 15) {
//pc.printf("wavegap %f\r\n",wavegap);
// AT WAVE OPENING!!!!
- motors.setMotor1Speed(dir*0.4*127);//left
- motors.setMotor0Speed(dir*0.4*127);//right
+ motors.setMotor1Speed(dir*0.25*127);//left
+ motors.setMotor0Speed(dir*0.25*127);//right
} else {
pid1.setProcessValue(range);
@@ -845,26 +878,50 @@
}
}
}
- return wavegap;
+
+ //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)
+void wall_follow2(int side, int direction, int section, float location, int rig)
{
- int SeeWaveGap = false, dir=1;
- float set=5, loc=0;
+ int dir=1, limit=86, lowlim=5;
+ float set=6, loc=0, Rigloc=0;
+ bool SeeWaveGap = false;
+
+ if(rig == 1) Rigloc= 16;
+ else if(rig == 2) Rigloc= 45;
+ else if(rig== 3) Rigloc = 70;
pid1.reset();
- if(direction == BACKWARD) dir=-1;
- if(section == TOOLS)set= 5;
+ if(direction == BACKWARD) {
+ dir=-1;
+ limit = 100;
+ }
+ if(section == TOOLS) {
+ set= 6;
+ limit = 86;
+ }
+ if(section == RETURN) lowlim=15;
leftEncoder.reset();
rightEncoder.reset();
- while(dir*loc + location <= 78) {
+ //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);
+
+ while((dir*loc + location <= limit) && (dir*loc + location >= lowlim)) {
+
loc=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
+ pc.printf("loc %f \r\n", loc);
pid1.setInputLimits(0.0, set);
pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
@@ -872,54 +929,87 @@
if(side) {
rangeFinderLeft.startMeas();
- wait_ms(38);
+ wait_ms(20);
rangeFinderLeft.getMeas(range);
} else {
rangeFinderRight.startMeas();
- wait_ms(38);
+ wait_ms(20);
rangeFinderRight.getMeas(range);
}
+ if(section == RIGS) {
+ rangeFinderLeft.startMeas();
+ wait_ms(20);
+ rangeFinderLeft.getMeas(range2);
- /*************CHECK FOR WAVE OPENING*****************/
- /* If after 20 ms the ultrasonic still sees 20+ cm */
- /* then robot is at wave opening */
+ 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(0);
+ break;
+ }
+ }
+ }
+
//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;
- }
-
- pid1.setProcessValue(range);
- pid_return = pid1.compute();
- //pc.printf("Range: %f\n PID: %f\r\n", range, pid_return);
+ if(range > 20 ) {
+ if(section == RIGS || section == RETURN) {
+ motors.setMotor0Speed(dir*0.25*127); //right
+ motors.setMotor1Speed(dir*0.25*127); //left
+ } else {
+ if(!SeeWaveGap) {
+ SeeWaveGap=true;
+ } else {
+ //STOP
+ motors.setMotor0Speed(dir*-0.25*127); //right
+ motors.setMotor1Speed(dir*-0.25*127); //left
+ wait_ms(5);
+ motors.stopBothMotors(0);
- 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
+ pc.printf("wavegap\r\n");
+ // AT WAVE OPENING!!!!
+ break;
+ }
}
} else {
- motors.setMotor0Speed(dir*MAX_SPEED);
- motors.setMotor1Speed(dir*MAX_SPEED);
+ SeeWaveGap = false;
+ pid1.setProcessValue(range);
+ 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
+ } 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);
+ motors.setMotor1Speed(dir*MAX_SPEED);
+ }
}
}
- motors.stopBothMotors();
+
+ //STOP
+ motors.setMotor0Speed(dir*-0.3*127); //right
+ motors.setMotor1Speed(dir*-0.3*127); //left
+ wait_ms(5);
+ motors.stopBothMotors(0);
}
@@ -928,53 +1018,100 @@
float usValue = 0;
if(section == TOOLS) {
+ pc.printf("tools section align\r\n");
// turn at an angle
leftEncoder.reset();
rightEncoder.reset();
motors.setMotor0Speed(-1.2*MAX_SPEED); //right
motors.setMotor1Speed(0.4*MAX_SPEED); //left
while(rightEncoder.getPulses()>-1000);
- motors.stopBothMotors();
+ motors.stopBothMotors(0);
//go backwards toward wall
leftEncoder.reset();
rightEncoder.reset();
- motors.setMotor0Speed(-MAX_SPEED); //right
- motors.setMotor1Speed(-MAX_SPEED); //left
+ motors.setMotor0Speed(-0.25*127); //right
+ motors.setMotor1Speed(-0.25*127); //left
while(abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300);
-
- motors.stopBothMotors();
+ motors.stopBothMotors(0);
// turn left towards wall
leftEncoder.reset();
rightEncoder.reset();
motors.setMotor0Speed(MAX_SPEED); //right
motors.setMotor1Speed(-MAX_SPEED); //left
- while(rightEncoder.getPulses() < 10 || abs(leftEncoder.getPulses()) < 10);
+ while(rightEncoder.getPulses() < 20 || abs(leftEncoder.getPulses()) < 20);
+
+ motors.stopBothMotors(0);
+
+ // turning left
+ motors.setMotor0Speed(0.9*MAX_SPEED); //right
+ motors.setMotor1Speed(-0.9*MAX_SPEED); //left
- motors.stopBothMotors();
+ } else if( section == RIGS) {
+ // check distance to wall
+ rangeFinderRight.startMeas();
+ wait_ms(20);
+ rangeFinderRight.getMeas(range);
+
+ if(range < 4 || range > 20) return;
+
+ // turn at an angle
+ leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor1Speed(-1.2*MAX_SPEED); //left
+ motors.setMotor0Speed(0.4*MAX_SPEED); //right
+ while(abs(leftEncoder.getPulses())<1000);
+ motors.stopBothMotors(0);
- motors.setMotor0Speed(0.7*MAX_SPEED); //right
- motors.setMotor1Speed(-0.7*MAX_SPEED); //left
+ //go backwards toward wall
+ leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor0Speed(-0.25*127); //right
+ motors.setMotor1Speed(-0.25*127); //left
+ while(abs(leftEncoder.getPulses()) < 150 || abs(rightEncoder.getPulses()) < 150);
+ motors.stopBothMotors(0);
+
+ // turn left towards wall
+ leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor0Speed(-MAX_SPEED); //right
+ motors.setMotor1Speed(MAX_SPEED); //left
+ while(abs(rightEncoder.getPulses()) < 20 || abs(leftEncoder.getPulses()) < 20);
+
+ motors.stopBothMotors(0);
+
+ // turning left
+ motors.setMotor0Speed(-0.9*MAX_SPEED); //right
+ motors.setMotor1Speed(0.9*MAX_SPEED); //left
} else {
+ pc.printf("in mid section align\r\n");
+ // turn right towards wall
rightTurn();
- motors.setMotor0Speed(-0.7*MAX_SPEED); //right
- motors.setMotor1Speed(0.7*MAX_SPEED); //left
+ // turning left towards wall
+ motors.setMotor0Speed(0.9*MAX_SPEED); //right
+ motors.setMotor1Speed(-0.9*MAX_SPEED); //left
}
usValue = 0;
while(1) {
- rangeFinderLeft.startMeas();
- wait_ms(20);
- rangeFinderLeft.getMeas(range);
- //pc.printf("Range %f \t OldValue %f\n\r",range, usValue);
+ if(section == RIGS) {
+ rangeFinderRight.startMeas();
+ wait_ms(20);
+ rangeFinderRight.getMeas(range);
+ } else {
+ rangeFinderLeft.startMeas();
+ wait_ms(20);
+ rangeFinderLeft.getMeas(range);
+ }
+ pc.printf("Range %f \t OldValue %f\n\r",range, usValue);
if(range > usValue && usValue != 0 && range < 25) {
break;
} else {
usValue = range;
}
}
- motors.stopBothMotors();
+ motors.stopBothMotors(0);
}
void rightTurn(void)
@@ -984,28 +1121,21 @@
rightEncoder.reset();
motors.setMotor0Speed(-0.5*127);//right
motors.setMotor1Speed(0.5*127);//left
- while(leftEncoder.getPulses()<900 || rightEncoder.getPulses()>-900);
- motors.stopBothMotors();
+ while(abs(leftEncoder.getPulses())<950 || abs(rightEncoder.getPulses())<950);
+ motors.stopBothMotors(0);
}
void leftTurn(void)
{
- /*
- leftEncoder.reset();
- rightEncoder.reset();
- motors.setMotor0Speed(0.4*MAX_SPEED); //right
- motors.setMotor1Speed(-MAX_SPEED); //left
- while(abs(leftEncoder.getPulses())<2500);
- motors.stopBothMotors();
- */
motors.begin();
leftEncoder.reset();
rightEncoder.reset();
motors.setMotor0Speed(0.5*127);// right
motors.setMotor1Speed(-0.5*127);// left
- while(abs(leftEncoder.getPulses())<1000 || rightEncoder.getPulses()<1000);
- motors.stopBothMotors();
+ while(abs(leftEncoder.getPulses())<1100 || rightEncoder.getPulses()<1100);
+ motors.stopBothMotors(0);
}
+
void slightleft(void)
{
@@ -1013,80 +1143,123 @@
rightEncoder.reset();
motors.setMotor0Speed(0.5*127);// right
motors.setMotor1Speed(-0.5*127);// left
- while(abs(leftEncoder.getPulses())<50 || rightEncoder.getPulses()<50);
- motors.stopBothMotors();
+ while(abs(leftEncoder.getPulses())<90 || rightEncoder.getPulses()<90);
+ motors.stopBothMotors(0);
+}
+
+void slightright(void)
+{
+
+ leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor0Speed(-0.4*127);// right
+ motors.setMotor1Speed(0.4*127);// left
+ while(abs(leftEncoder.getPulses())<90 || abs(rightEncoder.getPulses())<90);
+ motors.stopBothMotors(0);
}
+void slightMove(int direction, float pulses)
+{
+ int dir=1;
+
+ if(direction == BACKWARD) dir= -1;
+
+ leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor0Speed(dir*0.25*127); //right
+ motors.setMotor1Speed(dir*0.25*127); //left
+ while(abs(leftEncoder.getPulses()) < pulses || abs(rightEncoder.getPulses()) < pulses);
+
+ motors.setMotor0Speed(dir*-0.25*127); //right
+ motors.setMotor1Speed(dir*-0.25*127); //left
+ wait_ms(10);
+ motors.stopBothMotors(127);
+}
+
+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(0);
+}
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();
-
+ // slight backwards
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.getIRDistance() >20 && preLeft!=0) {
- preLeft=leftEncoder.getPulses();
- preRight=rightEncoder.getPulses();
- wait_ms(100);
- //pc.printf(" first while left %d right %d \r\n", preLeft, preRight);
- if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
- }
+ motors.setMotor0Speed(-0.25*127); //right
+ motors.setMotor1Speed(-0.25*127); //left
+ while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses()) < 50);
+ motors.stopBothMotors(0);
- motors.stopBothMotors();
- motors.begin();
- wait(2);
- /*
- motors.stopBothMotors();
- motors.setMotor0Speed(0.15*127); //right
- motors.setMotor1Speed(0.15*127); //left
- preLeft=preRight=5000 ;
- leftEncoder.reset();
- rightEncoder.reset();
- */
-// while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.getIRDistance() >20 && preLeft!=0){
- /* preLeft=leftEncoder.getPulses();
- preRight=rightEncoder.getPulses();
- pc.printf("second while left %d right %d \r\n", preLeft, preRight);
- wait_ms(200);
- if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
- }*/
+ pc.printf("slight backwards\r\n");
+ wait_ms(200);
leftEncoder.reset();
rightEncoder.reset();
motors.setMotor0Speed(0.3*127); //right
motors.setMotor1Speed(0.3*127); //left
-
- while(!out) {
- preLeft=leftEncoder.getPulses();
+ while((abs(leftEncoder.getPulses()) < 800 || abs(rightEncoder.getPulses()) < 800) /*&& preLeft!=0*/ && IR.getIRDistance() >15 ) {
+ /*preLeft=leftEncoder.getPulses();
preRight=rightEncoder.getPulses();
-
- rangeFinderLeft.startMeas();
- rangeFinderRight.startMeas();
- wait_ms(20);
- rangeFinderLeft.getMeas(range);
- rangeFinderRight.getMeas(range2);
- if(range < 10 || range2 < 10) out=1;
-
- if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) {
- motors.setMotor0Speed(0.4*127); //right
- motors.setMotor1Speed(0.4*127); //left
- }
- if(abs(leftEncoder.getPulses()) <1000 || abs(leftEncoder.getPulses())<1000) out=1;
+ wait_ms(200);
+ if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;*/
}
- motors.stopBothMotors();
- wait(2);
+ pc.printf("forward \r\n");
+ wait_ms(200);
+ /*
+ motors.stopBothMotors(0);
+ motors.begin();
+
+ leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor0Speed(0.3*127); //right
+ motors.setMotor1Speed(0.3*127); //left
+
+ while(!out) {
+ preLeft=leftEncoder.getPulses();
+ preRight=rightEncoder.getPulses();
+
+ rangeFinderLeft.startMeas();
+ rangeFinderRight.startMeas();
+ wait_ms(20);
+ rangeFinderLeft.getMeas(range);
+ rangeFinderRight.getMeas(range2);
+ if(range < 10 || range2 < 10) out=1;
+
+ if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) {
+ motors.setMotor0Speed(0.4*127); //right
+ motors.setMotor1Speed(0.4*127); //left
+ wait_ms(50);
+ out=1;
+ }
+ if(abs(leftEncoder.getPulses()) >1000 || abs(leftEncoder.getPulses())>1000) out=1;
+ }
+ */
+
+ motors.stopBothMotors(0);
motors.begin();
preLeft=preRight=5000 ;
@@ -1095,159 +1268,303 @@
motors.setMotor0Speed(.25*127); //right
motors.setMotor1Speed(.25*127); //left
- if(section == TOOLS || section == MID) {
- while(IR.getIRDistance() > 20 ) {
- //pc.printf("IR %f\r\n", IR.getIRDistance());
- //pc.printf("third while left %d right %d \r\n", preLeft, preRight);
+ if(section == TOOLS) {
+ while(IR.getIRDistance() > 10 && (abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) {
+
+ if(IR.getIRDistance() > 38) break;
+
+ preLeft=leftEncoder.getPulses();
+ preRight=rightEncoder.getPulses();
+ wait_ms(200);
+ }
+ } else if(section == MID || section == MID2) {
+ if(section == MID2) while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 400 || abs(rightEncoder.getPulses()) < 400));
+ while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 400 || abs(rightEncoder.getPulses()) < 400) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) {
+
+ if(IR.getIRDistance() > 38) break;
+
+ preLeft=leftEncoder.getPulses();
+ preRight=rightEncoder.getPulses();
+ wait_ms(200);
}
- } else while((abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses())< 200));
+
+ } else {
+ while(abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100);
+
+ leftEncoder.reset();
+ rightEncoder.reset();
+
+ motors.setMotor0Speed(-.15*127); //right
+ motors.setMotor1Speed(-.15*127); //left
+ while((abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100) && preLeft!=0 ) {
+ preLeft = leftEncoder.getPulses();
+ preRight = rightEncoder.getPulses();
+ wait_ms(200);
+ if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
+ }
+
+ leftEncoder.reset();
+ rightEncoder.reset();
+
+ motors.setMotor0Speed(0.25*127); //right
+ motors.setMotor1Speed(0.25*127); //left
+ while((abs(leftEncoder.getPulses()) < 10 || abs(rightEncoder.getPulses()) < 10));
+
+ motors.stopBothMotors(0);
+
+ return;
+ }
+
+ leftEncoder.reset();
+ rightEncoder.reset();
motors.setMotor0Speed(-.25*127); //right
motors.setMotor1Speed(-.25*127); //left
- wait_ms(10);
- motors.stopBothMotors();
- wait(2);
+ while((abs(leftEncoder.getPulses()) < 10 || abs(rightEncoder.getPulses()) < 10));
+
+ motors.stopBothMotors(0);
+ wait_ms(20);
motors.begin();
}
-void to_tools_section(float* location, float ¤t)
+void to_tools_section2(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;
- pc.printf("current %f \r\n",current);
+ slightMove(FORWARD,3200);
+ current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
- motors.stopBothMotors();
-
- wait(2);
-
- motors.setMotor0Speed(-.2*127); //right
- motors.setMotor1Speed(-.2*127); //left
- wait_ms(5);
- motors.stopBothMotors();
+}
+void to_tools_section1(float* location, float ¤t)
+{
+ slightMove(FORWARD,6450);
+ current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
}
void from_tools_section(float* location, float ¤t)
{
+
+
alignWithWall(TOOLS);
-
+ pc.printf("align\r\n");
wait_ms(100);
- wall_follow2(LEFT,FORWARD,MID, current);
- current= 78;
+ //wall_follow2(LEFT,FORWARD,MID, current);
+ //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
rangeFinderLeft.startMeas();
wait_ms(20);
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];
+ 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
leftEncoder.reset();
rightEncoder.reset();
motors.setMotor0Speed(MAX_SPEED); //right
motors.setMotor1Speed(MAX_SPEED); //left
- while(abs(leftEncoder.getPulses()) < 40 || abs(rightEncoder.getPulses())< 40);
- motors.stopBothMotors();
+ while(abs(leftEncoder.getPulses()) < 10 || abs(rightEncoder.getPulses())< 10);
+ motors.stopBothMotors(0);
- wait_ms(500);
- location[0]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
- current= location[0];
+ wait_ms(100);
leftTurn();
- slightleft();
overBump(TOOLS);
} else {
- location[0]= 77;
+ pc.printf("else greater than 20\r\n");
+ location[0]= current;
leftTurn();
- wait_ms(20);
- overBump(FIRST_WAVE);
+ overBump(TOOLS);
}
pc.printf("First Wavegap = %f\r\n",location[0]);
+
}
+
void mid_section(float* location, float ¤t, int* direction)
{
-
motors.begin();
- if(IR.getIRDistance() > 20) return;
-
+ if(IR.getIRDistance() > 38) {
+ direction[0]= STRAIGHT;
+ overBump(MID);
+ return;
+ }
+ pc.printf("before align with wall \r\n");
alignWithWall(MID);
+ wait_ms(100);
pc.printf("mid section current = %f\r\n",current);
- wall_follow2(LEFT,FORWARD,MID, current);
- current=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+ 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);
- if(current != 0) {
+ wait_ms(500);
+ rangeFinderLeft.startMeas();
+ wait_ms(20);
+ rangeFinderLeft.getMeas(range);
+
+ if(range > 20 ) {
direction[0]= RIGHT;
- current+= location[0];
location[1]= current;
+ slightMove(FORWARD,75);
+ //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
} else {
- current=location[0];
direction[0]= LEFT;
- wall_follow2(LEFT,BACKWARD,MID,current);
- location[1]= location[0]- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+ 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);
+ }
+
}
pc.printf("wavegap2 = %f\r\n",location[1]);
leftTurn();
- overBump(TOOLS);
- // go forward
- leftEncoder.reset();
- rightEncoder.reset();
- motors.setMotor0Speed(0.2*127); //right
- motors.setMotor1Speed(0.2*127); //left
- while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300);
- motors.stopBothMotors();
+
+ wait_ms(100);
+
+ overBump(MID);
}
void mid_section2(float* location, float ¤t, int* direction)
{
-
motors.begin();
- if(IR.getIRDistance() > 20) return;
+ pc.printf("mid section 2\r\n");
+
+ if(IR.getIRDistance() > 38) {
+ direction[1]= STRAIGHT;
+ overBump(RIGS);
+ return;
+ }
alignWithWall(MID);
- wall_follow2(LEFT,FORWARD,MID, current);
- current=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+ pc.printf("midsection 2 alignt with wall mid \r\n");
+
+ wall_follow2(LEFT,FORWARD,MID, current,0);
+ current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+
+ wait_ms(500);
- if(current != 0) {
+ pc.printf("midseection 2 after wf2 %f",current);
+ rangeFinderLeft.startMeas();
+ wait_ms(20);
+ rangeFinderLeft.getMeas(range);
+
+ if(range > 20 ) {
direction[1]= RIGHT;
- current+= location[1];
location[2]= current;
+ slightMove(FORWARD,75);
+ //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
} else {
- current=location[1];
direction[1]= LEFT;
- wall_follow2(LEFT,BACKWARD,MID,current);
- location[2]= location[1]- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+ 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(FORWARD,500);
}
leftTurn();
overBump(RIGS);
+ pc.printf("overbump rigs\r\n");
}
void rig_section(float* location, float ¤t, int* direction, int rig)
{
+ float loc;
+ if(rig == 1) loc= 15;
+ else if(rig == 2) loc= 45;
+ else loc = 75;
+
+ rightTurn();
+ slightright();
+
+ if(current > loc) {
+ 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);
+ } else {
+ 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);
+ }
+}
+
+void tools_section_return(float* location, float ¤t)
+{
+ if(location[0] > 16) {
+ leftTurn();
+ wall_follow2(LEFT, BACKWARD, RETURN, location[0], 0);
+ }
+ motors.stopBothMotors(0);
}
-void ledtoggle(void)
+void mid_section_return(float* location, float ¤t, int* direction)
+{
+ if(direction[0] == RIGHT) {
+ leftTurn();
+ alignWithWall(MID);
+ wall_follow2(LEFT, BACKWARD, MID, current,0);
+ current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+ rightTurn();
+ } else if(direction[0] == LEFT) {
+ leftTurn();
+ wall_follow2(RIGHT, FORWARD, MID, current,0);
+ current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+ rightTurn();
+ }
+ //ELSE and GO FORWARD
+ overBump(RIGS);
+}
+
+void mid_section2_return(float* location, float ¤t, int* direction)
{
- pwm.setPWM(9, 0, 4094);
- wait(2);
- pwm.setPWM(9, 0, 0);
+ if(direction[1] == RIGHT) {
+ leftTurn();
+ wall_follow2(LEFT, BACKWARD, MID, current,0);
+ current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+ rightTurn();
+ } else if(direction[1] == LEFT) {
+ leftTurn();
+ wall_follow2(RIGHT, FORWARD, MID, current,0);
+ current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+ rightTurn();
+ }
+ //ELSE and GO FORWARD
+ overBump(MID);
+}
+
+void rig_section_return(float* location, float ¤t, int* direction)
+{
+ alignWithWall(RIGS);
+ if(location[2] > current) {
+ wall_follow2(RIGHT, FORWARD, MID, current,0);
+ current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+ } else {
+ wall_follow2(RIGHT, BACKWARD, MID, current,0);
+ current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+ }
+ rightTurn();
+ overBump(MID2);
}
+
+