Thomas Ashworth
For IEEE Robotics
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Diff: main.cpp
- Revision:
- 17:a5bb85ee205d
- Parent:
- 16:8bb212df81b7
- Child:
- 18:a0ea7ecaf4fe
--- a/main.cpp Wed Apr 02 23:54:05 2014 +0000
+++ b/main.cpp Thu Apr 03 22:05:57 2014 +0000
@@ -78,9 +78,7 @@
#define TRIANGLE 2
#define CIRCLE 3
-//*********************//
-//******* TODO ********//
-//*********************//
+
//Oil Rig distance thresholds
#define OILRIG1_MAX 2200
#define OILRIG1_MIN 1000
@@ -94,6 +92,9 @@
#define MAX_SERVO_PULSE 2100
#define SERVO_MAX_ANGLE 180
+#define X_CENTER 80
+#define Y_CENTER 60
+
DigitalOut myled1(LED1);
DigitalOut myled2(LED2);
@@ -128,6 +129,7 @@
void servoBegin(void);
void initServoDriver(void);
void setServoPulse(uint8_t n, int angle);
+void setServoPulse2(uint8_t n, float angle); //float precision
void setServoPulseNo_delay(uint8_t n, int angle);
void servoPosition(int set);
int fire_checker(int rig);
@@ -195,19 +197,19 @@
//increase in number 5 rotates gripper
- {STORE_POSITION, 85, 10, 0, 165, 90, 0}, // storing position
+ {STORE_POSITION, 85, 10, 0, 165, 175, 0}, // storing position
{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
+ {DRIVE_POSITION_NOTOOL, 85, 10, 0, 165, 175, 0}, // Drive through course
+ {TOOL_1, 96, 50, 80, 109, 90, 0}, // Look over first tool
+ {TOOL_2, 79, 50, 80, 110, 90, 0}, // Look over second tool
+ {TOOL_3, 59, 50, 80, 109, 90, 0}, // Look over third tool
+ {DRIVE_POSITION_TOOL, 85, 10, 0, 165, 90, 105}, // Drive with tool loaded
{ORIENT_TOOL, 135, 60, 75, 60, 90, 90}, // position tool to be inserted
- {PU_TOOL_1, 96, 46, 78, 104, 90, 0}, // STATIC Pickup tool POSITION
- {PU_TOOL_2, 74, 47, 80, 105, 90, 0}, // STATIC Pickup tool POSITION
- {PU_TOOL_3, 57, 44, 76, 104, 90, 0}, // STATIC Pickup tool POSITION
+ {PU_TOOL_1, 96, 46, 78, 102, 170, 0}, // STATIC Pickup tool POSITION
+ {PU_TOOL_2, 74, 47, 80, 104, 170, 0}, // STATIC Pickup tool POSITION
+ {PU_TOOL_3, 57, 44, 76, 102, 170, 0}, // STATIC Pickup tool POSITION
{PU_TOOL_1_STAB, 98, 50, 90, 118, 90, 0}, // STAB TOOL 1
{PU_TOOL_2_STAB, 78, 50, 90, 108, 90, 0}, // STAB TOOL 2
{PU_TOOL_3_STAB, 53, 50, 90, 118, 90, 0}, // STAB TOOL 3
@@ -221,6 +223,7 @@
int shape;
int shape_alignX_done = 0;
int shape_alignY_done = 0;
+float deltaX = 0;
/* Variables for distance sensor*/
@@ -251,11 +254,15 @@
int direction[3];
double distance;
+ int pu, num, input;
+
pc.baud(115200);
//Laser Range Finder Initialization
lrf_baudCalibration();
+
motors.begin();
+
startBtn.rise(&button_int);
//Servo initialization
@@ -263,8 +270,6 @@
servoBegin(); //initiates servos to start position
//ServoOutputDisable = 0;
-
-
/********************************
MAIN WHILE LOOP FOR COMPETITION
*********************************/
@@ -392,6 +397,46 @@
//printImageToFile(BINARY);
shape_alignX_done = 0;
shape_alignY_done = 0;
+ /*
+ while( shape_alignY_done == 0) {
+ wait(1);
+ shape_detected = shapeDetection();
+
+ pc.printf("Y - Adjust to center tool\n\r");
+
+ if(get_com_y() < 50) {
+ wait(1);
+ slightMove(FORWARD,25);
+ current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
+
+ } else if(get_com_y() > 70) {
+ wait(1);
+ slightMove(BACKWARD,25);
+ current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
+
+ } else {
+ shape_alignY_done = 1;
+ }
+ }*/
+
+
+ for(int i = 0; i < 4; i++) {
+ shape_detected = shapeDetection();
+ wait(2);
+ if(get_com_x() > X_CENTER ) {
+ deltaX = get_com_x()-X_CENTER;
+ setServoPulse2(0,Arm_Table[TOOL_2].base_rotate += (deltaX/60.0) );
+ Arm_Table[PU_TOOL_2].base_rotate = Arm_Table[TOOL_2].base_rotate;
+ }
+ if(get_com_x() < X_CENTER) {
+ deltaX = get_com_x()-X_CENTER;
+ setServoPulse2(0,Arm_Table[TOOL_2].base_rotate += (deltaX/60.0) );
+ Arm_Table[PU_TOOL_2].base_rotate = Arm_Table[TOOL_2].base_rotate;
+ }
+ }
+
+
+ /*
while( shape_alignX_done == 0) {
wait(1);
@@ -407,14 +452,15 @@
} else {
shape_alignX_done = 1;
}
- }
+ }*/
+ //printImageToFile(BINARY);
//either goes to aquire the tool or look at the next shape
if(shape_detected == tool_needed) {
state = AQUIRE_TOOL2;
break;
} else {
- slightMove(FORWARD,25);
+ slightMove(FORWARD,40);
current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
@@ -422,19 +468,19 @@
wait(5); //wait for servos to settle
shape_alignX_done = 0;
shape_alignY_done = 0;
-
+ /*
while( shape_alignY_done == 0) {
wait(1);
shape_detected = shapeDetection();
pc.printf("Y - Adjust to center tool\n\r");
- if(get_com_y() < 40) {
+ if(get_com_y() < 50) {
wait(1);
slightMove(FORWARD,25);
current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
- } else if(get_com_y() > 80) {
+ } else if(get_com_y() > 70) {
wait(1);
slightMove(BACKWARD,25);
current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
@@ -442,23 +488,41 @@
} else {
shape_alignY_done = 1;
}
+ }*/
+
+
+ for(int i = 0; i < 4; i++) {
+ shape_detected = shapeDetection();
+ wait(2);
+ if (get_com_x() > X_CENTER) {
+ deltaX = get_com_x()-X_CENTER;
+ setServoPulse2(0,Arm_Table[TOOL_1].base_rotate += (deltaX/60.0) );
+ Arm_Table[PU_TOOL_1].base_rotate = Arm_Table[TOOL_1].base_rotate;
+ }
+ if (get_com_x() < X_CENTER) {
+ deltaX = get_com_x()-X_CENTER;
+ setServoPulse2(0,Arm_Table[TOOL_1].base_rotate += (deltaX/60.0) );
+ Arm_Table[PU_TOOL_1].base_rotate = Arm_Table[TOOL_1].base_rotate;
+ }
}
- while( shape_alignX_done == 0) {
- wait(1);
- shape_detected = shapeDetection();
- pc.printf("X - Adjust to center tool\n\r");
- if(get_com_x() > 100) {
- setServoPulse(0,Arm_Table[TOOL_1].base_rotate+=1);
+ /*
+ while( shape_alignX_done == 0) {
+ wait(1);
+ shape_detected = shapeDetection();
- } else if(get_com_x() < 60) {
- setServoPulse(0,Arm_Table[TOOL_1].base_rotate-=1);
+ pc.printf("X - Adjust to center tool\n\r");
+ if(get_com_x() > 100) {
+ setServoPulse(0,Arm_Table[TOOL_1].base_rotate+=1);
- } else {
- shape_alignX_done = 1;
- }
- }
+ } else if(get_com_x() < 60) {
+ setServoPulse(0,Arm_Table[TOOL_1].base_rotate-=1);
+
+ } else {
+ shape_alignX_done = 1;
+ }
+ }*/
if (shape_detected == tool_needed) {
state = AQUIRE_TOOL1;
@@ -489,52 +553,108 @@
setServoPulse(5, 105);
wait(.5);
setServoPulse(5, 00);
- setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm - 8);
+ setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm - 10);
wait(1);
setServoPulse(5, 105);
wait(1);
setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm);
+ wait(2);
- while(1);
+ setServoPulse(2, Arm_Table[DRIVE_POSITION_TOOL].big_arm);
+ setServoPulse(3, Arm_Table[DRIVE_POSITION_TOOL].claw_arm);
+ setServoPulse(1, Arm_Table[DRIVE_POSITION_TOOL].base_arm);
+ setServoPulse(0, Arm_Table[DRIVE_POSITION_TOOL].base_rotate);
+ setServoPulse(4, Arm_Table[DRIVE_POSITION_TOOL].claw_rotate);
+ setServoPulse(5, Arm_Table[DRIVE_POSITION_TOOL].claw_open);
- servoPosition(DRIVE_POSITION_TOOL); //arm position for driving with the tool
+
state = NAVIGATE_WAVES_ROW1;
break;
case AQUIRE_TOOL2:
servoPosition(PU_TOOL_2);
- setServoPulse(4, 170);
+ setServoPulse(4, 175);
wait(5);
setServoPulse(0, Arm_Table[PU_TOOL_2].base_rotate );
wait(1);
- setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 5);
+ setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 2);
+ wait(1);
+ setServoPulse(3, Arm_Table[PU_TOOL_2].claw_arm - 2);
wait(1);
setServoPulse(5, 105);
- wait(.5);
+ wait(2);
setServoPulse(5, 00);
setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 6);
- wait(.5);
+ wait(2);
setServoPulse(5, 105);
- wait(.5);
+ wait(2);
setServoPulse(5, 00);
- setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 8);
- wait(1);
+ setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 10);
+ wait(2);
setServoPulse(5, 105);
- wait(1);
+ wait(2);
setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm);
- while(1);
+
+ wait(2);
- servoPosition(DRIVE_POSITION_TOOL); //arm position for driving with the tool
+ setServoPulse(2, Arm_Table[DRIVE_POSITION_TOOL].big_arm);
+ setServoPulse(3, Arm_Table[DRIVE_POSITION_TOOL].claw_arm);
+ setServoPulse(1, Arm_Table[DRIVE_POSITION_TOOL].base_arm);
+ setServoPulse(0, Arm_Table[DRIVE_POSITION_TOOL].base_rotate);
+ setServoPulse(4, Arm_Table[DRIVE_POSITION_TOOL].claw_rotate);
+ setServoPulse(5, Arm_Table[DRIVE_POSITION_TOOL].claw_open);
+
+
state = NAVIGATE_WAVES_ROW1;
break;
case AQUIRE_TOOL3:
- servoPosition(PU_TOOL_3);
+ /*
+ while( shape_alignY_done == 0) {
+ wait(1);
+
+ servoPosition(PU_TOOL_3);
+ shape_detected = shapeDetection();
+ wait(2);
+
+ pc.printf("Y - Adjust to center tool\n\r");
+
+ if(get_com_y() < 50) {
+ wait(1);
+ slightMove(FORWARD,25);
+ current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
+
+ } else if(get_com_y() > 70) {
+ wait(1);
+ slightMove(BACKWARD,25);
+ current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
+
+ } else {
+ shape_alignY_done = 1;
+ }
+ }
+
+ */
+ for(int i = 0; i < 4; i++) {
+ shape_detected = shapeDetection();
+ wait(2);
+ if (get_com_x() > X_CENTER) {
+ deltaX = get_com_x()-X_CENTER;
+ setServoPulse2(0,Arm_Table[PU_TOOL_3].base_rotate += (deltaX/60.0) );
+ }
+ if (get_com_x() < X_CENTER) {
+ deltaX = get_com_x()-X_CENTER;
+ setServoPulse2(0,Arm_Table[PU_TOOL_3].base_rotate += (deltaX/60.0) );
+ }
+ }
+
setServoPulse(4, 130);
wait(5);
setServoPulse(0, Arm_Table[PU_TOOL_3].base_rotate );
wait(1);
- setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm - 5);
+ setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 2);
+ wait(1);
+ setServoPulse(3, Arm_Table[PU_TOOL_2].claw_arm - 3);
wait(1);
setServoPulse(5, 105);
wait(.5);
@@ -544,14 +664,21 @@
setServoPulse(5, 105);
wait(.5);
setServoPulse(5, 00);
- setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm - 8);
+ setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm - 10);
wait(1);
setServoPulse(5, 105);
wait(1);
setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm);
- while(1);
+
+ wait(2);
- servoPosition(DRIVE_POSITION_TOOL); //arm position for driving with the tool
+ setServoPulse(2, Arm_Table[DRIVE_POSITION_TOOL].big_arm);
+ setServoPulse(3, Arm_Table[DRIVE_POSITION_TOOL].claw_arm);
+ setServoPulse(1, Arm_Table[DRIVE_POSITION_TOOL].base_arm);
+ setServoPulse(0, Arm_Table[DRIVE_POSITION_TOOL].base_rotate);
+ setServoPulse(4, Arm_Table[DRIVE_POSITION_TOOL].claw_rotate);
+ setServoPulse(5, Arm_Table[DRIVE_POSITION_TOOL].claw_open);
+
state = NAVIGATE_WAVES_ROW1;
break;
@@ -564,33 +691,31 @@
**************************************************/
case NAVIGATE_WAVES_ROW1:
- //*********************//
- //******* TODO ********//
- //*********************//
- // CODE TO NAVIGATE ROW1
+ from_tools_section(location,current);
+
+ mid_section(location, current, direction);
+
state = NAVIGATE_WAVES_ROW2;
break;
case NAVIGATE_WAVES_ROW2:
- //*********************//
- //******* TODO ********//
- //*********************//
- // CODE TO NAVIGATE ROW2
+ mid_section2(location, current, direction);
state = NAVIGATE_WAVES_ROW3;
break;
case NAVIGATE_WAVES_ROW3:
- //*********************//
- //******* TODO ********//
- //*********************//
- // CODE TO NAVIGATE ROW3
- //goes to appropriate rig
if(shape_detected == 1) {
+ rig_section(location, current, direction, 1);
+ while(1);
state = NAVIGATE_TO_SQUARE_RIG;
} else if(shape_detected == 2) {
+ rig_section(location, current, direction, 2);
+ while(1);
state = NAVIGATE_TO_TRIANGLE_RIG;
} else {
+ rig_section(location, current, direction, 3);
+ while(1);
state = NAVIGATE_TO_CIRCLE_RIG;
}
break;
@@ -740,6 +865,13 @@
pwm.setPWM(n, 0, pulse);
}
+void setServoPulse2(uint8_t n, float angle)
+{
+ float pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE);
+ float pulselength = 20000; // 10,000 us per second
+ pulse = 4094 * pulse / pulselength;
+ pwm.setPWM(n, 0, pulse);
+}
@@ -874,7 +1006,7 @@
void wall_follow2(int side, int direction, int section, float location, int rig)
{
int dir=1, limit=86, lowlim=5;
- float set=6, loc=0, Rigloc=0;
+ float set=7, loc=0, Rigloc=0;
bool SeeWaveGap = false;
if(rig == 1) Rigloc= 16;
@@ -928,10 +1060,7 @@
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);
+ motors.stopBothMotors(127);
break;
}
}
@@ -949,10 +1078,7 @@
SeeWaveGap=true;
} else {
//STOP
- motors.setMotor0Speed(dir*-0.25*127); //right
- motors.setMotor1Speed(dir*-0.25*127); //left
- wait_ms(5);
- motors.stopBothMotors(0);
+ motors.stopBothMotors(127);
pc.printf("wavegap\r\n");
// AT WAVE OPENING!!!!
@@ -989,10 +1115,7 @@
}
//STOP
- motors.setMotor0Speed(dir*-0.3*127); //right
- motors.setMotor1Speed(dir*-0.3*127); //left
- wait_ms(5);
- motors.stopBothMotors(0);
+ motors.stopBothMotors(127);
}
@@ -1031,29 +1154,31 @@
motors.setMotor0Speed(0.9*MAX_SPEED); //right
motors.setMotor1Speed(-0.9*MAX_SPEED); //left
- } else if( section == RIGS) {
+ } else if(section == RIGS) {
// check distance to wall
rangeFinderRight.startMeas();
wait_ms(20);
rangeFinderRight.getMeas(range);
- if(range < 4 || range > 20) return;
+ if(range < 3 || 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);
+ while(abs(leftEncoder.getPulses())<500);
motors.stopBothMotors(0);
+ wait(2);
//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);
+ while(abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses()) < 200);
motors.stopBothMotors(0);
+ wait(2);
// turn left towards wall
leftEncoder.reset();
@@ -1063,11 +1188,12 @@
while(abs(rightEncoder.getPulses()) < 20 || abs(leftEncoder.getPulses()) < 20);
motors.stopBothMotors(0);
+ wait(2);
// turning left
motors.setMotor0Speed(-0.9*MAX_SPEED); //right
motors.setMotor1Speed(0.9*MAX_SPEED); //left
- } else {
+ } else {// MID
pc.printf("in mid section align\r\n");
// turn right towards wall
rightTurn();
@@ -1104,8 +1230,8 @@
rightEncoder.reset();
motors.setMotor0Speed(-0.5*127);//right
motors.setMotor1Speed(0.5*127);//left
- while(abs(leftEncoder.getPulses())<950 || abs(rightEncoder.getPulses())<950);
- motors.stopBothMotors(0);
+ while(abs(leftEncoder.getPulses())<1050 || abs(rightEncoder.getPulses())<1050);
+ motors.stopBothMotors(127);
}
void leftTurn(void)
@@ -1116,7 +1242,7 @@
motors.setMotor0Speed(0.5*127);// right
motors.setMotor1Speed(-0.5*127);// left
while(abs(leftEncoder.getPulses())<1100 || rightEncoder.getPulses()<1100);
- motors.stopBothMotors(0);
+ motors.stopBothMotors(127);
}
void slightleft(void)
@@ -1127,7 +1253,7 @@
motors.setMotor0Speed(0.5*127);// right
motors.setMotor1Speed(-0.5*127);// left
while(abs(leftEncoder.getPulses())<90 || rightEncoder.getPulses()<90);
- motors.stopBothMotors(0);
+ motors.stopBothMotors(127);
}
void slightright(void)
@@ -1137,8 +1263,8 @@
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);
+ while(abs(leftEncoder.getPulses())<50 || abs(rightEncoder.getPulses())<50);
+ motors.stopBothMotors(127);
}
void slightMove(int direction, float pulses)
@@ -1153,9 +1279,6 @@
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);
}
@@ -1177,10 +1300,7 @@
rangeFinderRight.getMeas(range);
}
- motors.setMotor0Speed(dir*-0.2*127); //right
- motors.setMotor1Speed(dir*-0.2*127); //left
- wait_ms(5);
- motors.stopBothMotors(0);
+ motors.stopBothMotors(127);
}
void overBump(int section)
@@ -1194,53 +1314,27 @@
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(127);
pc.printf("slight backwards\r\n");
wait_ms(200);
+ // Over bump
leftEncoder.reset();
rightEncoder.reset();
motors.setMotor0Speed(0.3*127); //right
motors.setMotor1Speed(0.3*127); //left
while((abs(leftEncoder.getPulses()) < 800 || abs(rightEncoder.getPulses()) < 800) /*&& preLeft!=0*/ && IR.getIRDistance() >15 ) {
- /*preLeft=leftEncoder.getPulses();
+ /*
+ preLeft=leftEncoder.getPulses();
preRight=rightEncoder.getPulses();
wait_ms(200);
- if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;*/
+ if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
+ */
}
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();
@@ -1252,7 +1346,7 @@
motors.setMotor1Speed(.25*127); //left
if(section == TOOLS) {
- while(IR.getIRDistance() > 10 && (abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) {
+ while(IR.getIRDistance() > 10 && (abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses()) < 200) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) {
if(IR.getIRDistance() > 38) break;
@@ -1262,7 +1356,7 @@
}
} 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)) {
+ while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) {
if(IR.getIRDistance() > 38) break;
@@ -1271,9 +1365,10 @@
wait_ms(200);
}
- } else {
- while(abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100);
+ } else {// RIGS
+ while(abs(leftEncoder.getPulses()) < 220 || abs(rightEncoder.getPulses()) < 220);
+ // go backwards to line up with bump
leftEncoder.reset();
rightEncoder.reset();
@@ -1285,35 +1380,19 @@
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);
+ motors.stopBothMotors(127);
return;
}
- leftEncoder.reset();
- rightEncoder.reset();
-
- motors.setMotor0Speed(-.25*127); //right
- motors.setMotor1Speed(-.25*127); //left
- while((abs(leftEncoder.getPulses()) < 10 || abs(rightEncoder.getPulses()) < 10));
-
- motors.stopBothMotors(0);
+ motors.stopBothMotors(127);
wait_ms(20);
motors.begin();
}
-
void to_tools_section1(float* location, float ¤t)
{
- slightMove(FORWARD,6600);
+ slightMove(FORWARD,6700);
current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
}
@@ -1328,7 +1407,6 @@
void from_tools_section(float* location, float ¤t)
{
-
alignWithWall(TOOLS);
pc.printf("align\r\n");
wait_ms(100);
@@ -1353,12 +1431,7 @@
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()) < 10 || abs(rightEncoder.getPulses())< 10);
- motors.stopBothMotors(0);
+ motors.stopBothMotors(127);
wait_ms(100);
leftTurn();
@@ -1373,6 +1446,61 @@
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);
+
+ //////////////////////////////////////////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);
+
+ 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)
{
@@ -1452,14 +1580,14 @@
if(range > 20 ) {
direction[1]= RIGHT;
location[2]= current;
- slightMove(FORWARD,75);
- //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+ slightMove(FORWARD,100);
+ current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
} else {
direction[1]= LEFT;
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);
+ //slightMove(FORWARD,100);
}
leftTurn();
@@ -1475,8 +1603,12 @@
else if(rig == 2) loc= 45;
else loc = 75;
+ // Slight forward for turn
+ slightMove(FORWARD,100);
+ wait_ms(100);
rightTurn();
slightright();
+ wait(5);
if(current > loc) {
pc.printf("RIG section %f\r\n",current);
@@ -1487,6 +1619,8 @@
wall_follow2(RIGHT, FORWARD, RIGS, current, rig);
current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
}
+
+ alignWithWall(RIGS);
}
void tools_section_return(float* location, float ¤t)
@@ -1536,7 +1670,6 @@
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);
@@ -1546,9 +1679,4 @@
}
rightTurn();
overBump(MID2);
-}
-
-
-
-
-
+}
\ No newline at end of file