Paolina Povolotskaya
ksdflsjdfljas
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of
theRobot
by 
Thomas Ashworth
Diff: main.cpp
- Revision:
- 13:529323807361
- Parent:
- 12:284be46593ae
- Child:
- 14:784acd735b8c
--- a/main.cpp Wed Apr 02 00:30:30 2014 +0000
+++ b/main.cpp Wed Apr 02 03:30:48 2014 +0000
@@ -129,7 +129,6 @@
void setServoPulse(uint8_t n, int angle);
void setServoPulseNo_delay(uint8_t n, int angle);
void servoPosition(int set);
-void setGripper(int open);
int fire_checker(int rig);
@@ -217,6 +216,8 @@
int y_coord;
int area;
int shape;
+int shape_alignX_done = 0;
+
/* Variables for distance sensor*/
int distLaser;
@@ -246,118 +247,90 @@
//ServoOutputDisable = 0;
-
-
-
-
-
/*******************
WHILE LOOP FOR TESTING
********************/
//while(1) {
-/*
- pc.scanf("%d %d", &servoNum, &servoAngle);
- if(servoAngle > 175) {
- servoAngle = 175;
- }
- if(servoNum > 5 ) {
- servoNum = 0;
- servoAngle = 90;
- }
- setServoPulse(servoNum, servoAngle);
- distLaser = getDistance();
- pc.printf("Distance %d", distLaser);
-*/
- /*
- while(pc.getc() != 'g');
- servoPosition(TOOL_2);
- wait(3);
- //shape_detected = shapeDetection();
- //clearBounds();
- ImageToArray(GREYSCALE);
- printImageToFile(GREYSCALE);
- while(pc.getc() != 'g');
- servoPosition(TOOL_3);
- wait(3);
- shape_detected = shapeDetection();
- printImageToFile(BINARY);
- while(pc.getc() != 'g');
- servoPosition(TOOL_3);
- wait(3);
- shape_detected = shapeDetection();
- while(1){};*/
+ /*
+ pc.scanf("%d %d", &servoNum, &servoAngle);
+ if(servoAngle > 175) {
+ servoAngle = 175;
+ }
+ if(servoNum > 5 ) {
+ servoNum = 0;
+ servoAngle = 90;
+ }
+ setServoPulse(servoNum, servoAngle);
+ distLaser = getDistance();
+ pc.printf("Distance %d", distLaser);
+ */
-
- //shape_detected = shapeDetection();
- //distLaser = getDistance();
- //pc.printf("Distance %d", distLaser);
- //ledtoggle();
- /*
- int shape_alignX_done = 0;
- int shape_alignY_done = 0;
+ /*
+ int shape_alignX_done = 0;
+ int shape_alignY_done = 0;
- //pc.scanf("%d", &posNum);
+ //pc.scanf("%d", &posNum);
- while(pc.getc() != 'g');
- servoPosition(TOOL_1);
- while(pc.getc() != 'g');
- //shape_detected = shapeDetection();
+ while(pc.getc() != 'g');
+ servoPosition(TOOL_1);
+ while(pc.getc() != 'g');
+ //shape_detected = shapeDetection();
- ImageToArray(GREYSCALE);
- //clearBounds();
- printImageToFile(GREYSCALE);
- while(1);
+ ImageToArray(GREYSCALE);
+ //clearBounds();
+ printImageToFile(GREYSCALE);
+ while(1);
- while(shape_alignY_done == 0) {
- shape_detected = shapeDetection();
- pc.printf("\nY movement\n");
- if(get_com_y() >= 80) {
- setServoPulse(1, Arm_Table[TOOL_1].base_arm-=2);
- setServoPulse(2, Arm_Table[TOOL_1].big_arm-=1);
- //setServoPulse(3, Arm_Table[TOOL_1].claw_arm+=2);
- } else if(get_com_y() > 70 && get_com_y() < 90) {
- setServoPulse(1, Arm_Table[TOOL_1].base_arm-=1);
- setServoPulse(2, Arm_Table[TOOL_1].big_arm-=1);
- //setServoPulse(3, Arm_Table[TOOL_1].claw_arm+=1);
- } else if(get_com_y() <= 35 ) {
- setServoPulse(1, Arm_Table[TOOL_1].base_arm+=2);
- setServoPulse(2, Arm_Table[TOOL_1].big_arm+=1);
- //setServoPulse(3, Arm_Table[TOOL_1].claw_arm-=2);
- } else if(get_com_y() < 50 && get_com_y() > 20) {
- setServoPulse(1, Arm_Table[TOOL_1].base_arm+=1);
- setServoPulse(2, Arm_Table[TOOL_1].big_arm+=1);
- //setServoPulse(3, Arm_Table[TOOL_1].claw_arm-=1);
- } else {
- shape_alignY_done = 1;
- }
- }
+ while(shape_alignY_done == 0) {
+ shape_detected = shapeDetection();
+ pc.printf("\nY movement\n");
+ if(get_com_y() >= 80) {
+ setServoPulse(1, Arm_Table[TOOL_1].base_arm-=2);
+ setServoPulse(2, Arm_Table[TOOL_1].big_arm-=1);
+ //setServoPulse(3, Arm_Table[TOOL_1].claw_arm+=2);
+ } else if(get_com_y() > 70 && get_com_y() < 90) {
+ setServoPulse(1, Arm_Table[TOOL_1].base_arm-=1);
+ setServoPulse(2, Arm_Table[TOOL_1].big_arm-=1);
+ //setServoPulse(3, Arm_Table[TOOL_1].claw_arm+=1);
+ } else if(get_com_y() <= 35 ) {
+ setServoPulse(1, Arm_Table[TOOL_1].base_arm+=2);
+ setServoPulse(2, Arm_Table[TOOL_1].big_arm+=1);
+ //setServoPulse(3, Arm_Table[TOOL_1].claw_arm-=2);
+ } else if(get_com_y() < 50 && get_com_y() > 20) {
+ setServoPulse(1, Arm_Table[TOOL_1].base_arm+=1);
+ setServoPulse(2, Arm_Table[TOOL_1].big_arm+=1);
+ //setServoPulse(3, Arm_Table[TOOL_1].claw_arm-=1);
+ } else {
+ shape_alignY_done = 1;
+ }
+ }
- while( shape_alignX_done == 0){
- shape_detected = shapeDetection();
- pc.printf("\nX movement\n");
- if(get_com_x() > 95) {
- Arm_Table[TOOL_1].base_rotate+=1;
- setServoPulse(0, Arm_Table[TOOL_1].base_rotate);
- setServoPulse(0, Arm_Table[TOOL_1].base_rotate-1);
- setServoPulse(0, Arm_Table[TOOL_1].base_rotate+1);
+ while( shape_alignX_done == 0){
+ shape_detected = shapeDetection();
+ pc.printf("\nX movement\n");
+ if(get_com_x() > 95) {
+ Arm_Table[TOOL_1].base_rotate+=1;
+ setServoPulse(0, Arm_Table[TOOL_1].base_rotate);
+ setServoPulse(0, Arm_Table[TOOL_1].base_rotate-1);
+ setServoPulse(0, Arm_Table[TOOL_1].base_rotate+1);
- } else if(get_com_x() < 65) {
- Arm_Table[TOOL_1].base_rotate-=1;
- setServoPulse(0, Arm_Table[TOOL_1].base_rotate);
- setServoPulse(0, Arm_Table[TOOL_1].base_rotate-1);
- setServoPulse(0, Arm_Table[TOOL_1].base_rotate+1);
+ } else if(get_com_x() < 65) {
+ Arm_Table[TOOL_1].base_rotate-=1;
+ setServoPulse(0, Arm_Table[TOOL_1].base_rotate);
+ setServoPulse(0, Arm_Table[TOOL_1].base_rotate-1);
+ setServoPulse(0, Arm_Table[TOOL_1].base_rotate+1);
- } else {
- shape_alignX_done = 1;
- }
+ } else {
+ shape_alignX_done = 1;
+ }
- }
- printImageToFile(BINARY);*/
+ }
+ printImageToFile(BINARY);*/
- // }
+ // }
@@ -378,11 +351,12 @@
**************************************************/
case START :
myled1 = 1;
- while(pc.getc() != 'g');
+ while(pc.getc() != 'g'); //waits for the letter g before starting program
myled1 = 0;
state = OILRIG1_POS;
break;
+
/**************************************************
* STAGE 1
*
@@ -391,57 +365,42 @@
**************************************************/
case OILRIG1_POS: //aims arm at square oil rig
- setServoPulse(3, Arm_Table[OIL_RIG1].claw_arm);
- setServoPulse(2, Arm_Table[OIL_RIG1].big_arm);
- setServoPulse(1, Arm_Table[OIL_RIG1].base_arm);
- 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);
- //lrf.putc('E'); // lighting calculation
- //wait(15);
+ servoPosition(OIL_RIG1);
+ wait(3); //wait for servo to settle before laser distance
fire = fire_checker(OIL_RIG1); //determines if oil rig is on fire
- pc.printf("FIRE: SQUARE %d", fire);
//determines what tool is needed
if (fire == 1) {
+ pc.printf("FIRE FOUND!!!!!!!!\n\r");
tool_needed = SQUARE;
state = GOTO_TOOLS1;
} else {
+ pc.printf("XXXXXX FIRE NOT FOUND XXXXXX");
state = OILRIG2_POS;
}
-
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);
-
+ servoPosition(DRIVE_POSITION_NOTOOL);
wait(3); //wait for servos to settle
- to_tools_section2(location, current);
+ to_tools_section2(location, current); // moves to second rig
servoPosition(OIL_RIG2); //position arm to point at first oilrig
- wait(2);
- //lrf.putc('E'); // lighting calculation
- //wait(15);
+ wait(3);
+
fire = fire_checker(OIL_RIG2);
- pc.printf("FIRE: TRIANGLE %d", fire);
if (fire == 1) {
+ pc.printf("FIRE FOUND!!!!!!!!");
tool_needed = TRIANGLE;
state = GOTO_TOOLS2;
} else {
+ pc.printf("XXXXXX FIRE NOT FOUND XXXXXX");
tool_needed = CIRCLE;
state = GOTO_TOOLS2;
}
- pc.printf("tool needed: %d", tool_needed);
break;
/**************************************************
@@ -452,29 +411,16 @@
**************************************************/
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
+ servoPosition(DRIVE_POSITION_NOTOOL);
+ wait(3); //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(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
+ servoPosition(DRIVE_POSITION_NOTOOL);
+ wait(3); //wait for servos to settle
slightMove(FORWARD,3100);
current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
@@ -482,6 +428,8 @@
state = IDENTIFY_TOOLS;
break;
+
+
/**************************************************
* STAGE 3
*
@@ -493,1076 +441,1054 @@
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();
+
+ //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_TOOL2;
- } else {
- servoPosition(TOOL_1);
- wait(1); //wait for servos to settle
- //centerCamWithTool();
+ while( shape_alignX_done == 0) {
shape_detected = shapeDetection();
- if (shape_detected == tool_needed) {
- state = AQUIRE_TOOL1;
+ pc.printf("X - Adjust to center tool\n\r");
+ if(get_com_x() > 95) {
+ Arm_Table[TOOL_1].base_rotate+=1;
+
+ } else if(get_com_x() < 65) {
+ Arm_Table[TOOL_1].base_rotate-=1;
+
} else {
- servoPosition(TOOL_3);
- wait(1); //wait for servos to settle
- centerCamWithTool();
- state = AQUIRE_TOOL3;
+ shape_alignX_done = 1;
}
- }
- while(1);
- break;
- case AQUIRE_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:
- 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:
- servoPosition(PU_TOOL_3_STAB);
- wait(2);
- servoPosition(DRIVE_POSITION_TOOL); //arm position for driving with the tool
- state = NAVIGATE_WAVES_ROW1;
- break;
+ //either goes to aquire the tool or look at the next shape
+ if(shape_detected == tool_needed) {
+ state = AQUIRE_TOOL2;
+ } else {
+ servoPosition(TOOL_1);
+ wait(3); //wait for servos to settle
+
+ shape_alignX_done = 0;
+ while( shape_alignX_done == 0) {
+
+ shape_detected = shapeDetection();
+ pc.printf("X - Adjust to center tool\n\r");
+ if(get_com_x() > 95) {
+ Arm_Table[TOOL_1].base_rotate+=1;
+
+ } else if(get_com_x() < 65) {
+ Arm_Table[TOOL_1].base_rotate-=1;
+
+ } else {
+ shape_alignX_done = 1;
+ }
+
+ if (shape_detected == tool_needed) {
+ state = AQUIRE_TOOL;
+ } else {
+ servoPosition(TOOL_3);
+ wait(3); //wait for servos to settle
+ shape_detected = shapeDetection();
+ pc.printf("X - Adjust to center tool\n\r");
+ if(get_com_x() > 95) {
+ Arm_Table[TOOL_1].base_rotate+=1;
+
+ } else if(get_com_x() < 65) {
+ Arm_Table[TOOL_1].base_rotate-=1;
+
+ } else {
+ shape_alignX_done = 1;
+ }
+
+ state = AQUIRE_TOOL3;
+ }
+ }
+
+ while(1);
+ break;
+
+ case AQUIRE_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:
+ 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:
+ servoPosition(PU_TOOL_3_STAB);
+ wait(2);
+
+
+ servoPosition(DRIVE_POSITION_TOOL); //arm position for driving with the tool
+ state = NAVIGATE_WAVES_ROW1;
+ break;
- /**************************************************
- * STAGE 4
- *
- * - Navigate through the ocean
- *
- **************************************************/
+ /**************************************************
+ * STAGE 4
+ *
+ * - Navigate through the ocean
+ *
+ **************************************************/
- case NAVIGATE_WAVES_ROW1:
- //*********************//
- //******* TODO ********//
- //*********************//
- // CODE TO NAVIGATE ROW1
- state = NAVIGATE_WAVES_ROW2;
- break;
+ case NAVIGATE_WAVES_ROW1:
+ //*********************//
+ //******* TODO ********//
+ //*********************//
+ // CODE TO NAVIGATE ROW1
+ state = NAVIGATE_WAVES_ROW2;
+ break;
- case NAVIGATE_WAVES_ROW2:
- //*********************//
- //******* TODO ********//
- //*********************//
- // CODE TO NAVIGATE ROW2
- state = NAVIGATE_WAVES_ROW3;
- break;
+ case NAVIGATE_WAVES_ROW2:
+ //*********************//
+ //******* TODO ********//
+ //*********************//
+ // CODE TO NAVIGATE ROW2
+ state = NAVIGATE_WAVES_ROW3;
+ break;
- case NAVIGATE_WAVES_ROW3:
- //*********************//
- //******* TODO ********//
- //*********************//
- // CODE TO NAVIGATE ROW3
+ case NAVIGATE_WAVES_ROW3:
+ //*********************//
+ //******* TODO ********//
+ //*********************//
+ // CODE TO NAVIGATE ROW3
- //goes to appropriate rig
- if(shape_detected == 1) {
- state = NAVIGATE_TO_SQUARE_RIG;
- } else if(shape_detected == 2) {
- state = NAVIGATE_TO_TRIANGLE_RIG;
- } else {
- state = NAVIGATE_TO_CIRCLE_RIG;
- }
- break;
+ //goes to appropriate rig
+ if(shape_detected == 1) {
+ state = NAVIGATE_TO_SQUARE_RIG;
+ } else if(shape_detected == 2) {
+ state = NAVIGATE_TO_TRIANGLE_RIG;
+ } else {
+ state = NAVIGATE_TO_CIRCLE_RIG;
+ }
+ break;
- /**************************************************
- * STAGE 5
- *
- * - Travel to appropriate rig
- *
- **************************************************/
- case NAVIGATE_TO_SQUARE_RIG:
- //NAVIGATION CODE HERE
- state = RIG_ALIGN;
- break;
- case NAVIGATE_TO_TRIANGLE_RIG:
- //NAVIGATION CODE HERE
- state = RIG_ALIGN;
- break;
- case NAVIGATE_TO_CIRCLE_RIG:
- //NAVIGATION CODE HERE
- state = RIG_ALIGN;
- break;
+ /**************************************************
+ * STAGE 5
+ *
+ * - Travel to appropriate rig
+ *
+ **************************************************/
+ case NAVIGATE_TO_SQUARE_RIG:
+ //NAVIGATION CODE HERE
+ state = RIG_ALIGN;
+ break;
+ case NAVIGATE_TO_TRIANGLE_RIG:
+ //NAVIGATION CODE HERE
+ state = RIG_ALIGN;
+ break;
+ case NAVIGATE_TO_CIRCLE_RIG:
+ //NAVIGATION CODE HERE
+ state = RIG_ALIGN;
+ break;
- /**************************************************
- * STAGE 6
- *
- * - Align with appropriate rig
- *
- **************************************************/
- case RIG_ALIGN:
+ /**************************************************
+ * STAGE 6
+ *
+ * - Align with appropriate rig
+ *
+ **************************************************/
+ case RIG_ALIGN:
- //*********************//
- //******* TODO ********//
- //*********************//
- // CODE TO ALIGN ROBOT WITH RIG
+ //*********************//
+ //******* TODO ********//
+ //*********************//
+ // CODE TO ALIGN ROBOT WITH RIG
- servoPosition(ORIENT_TOOL);
- wait(1); //wait for servos to settle
- state = INSERT_TOOL;
- break;
+ servoPosition(ORIENT_TOOL);
+ wait(1); //wait for servos to settle
+ state = INSERT_TOOL;
+ break;
- /**************************************************
- * STAGE 7
- *
- * - Insert Tool
- * - Extenguish fire
- * - win contest
- *
- **************************************************/
+ /**************************************************
+ * STAGE 7
+ *
+ * - Insert Tool
+ * - Extenguish fire
+ * - win contest
+ *
+ **************************************************/
- case INSERT_TOOL:
- //*********************//
- //******* TODO ********//
- //*********************//
- // CODE TO INSERT TOOL
- break;
+ case INSERT_TOOL:
+ //*********************//
+ //******* TODO ********//
+ //*********************//
+ // CODE TO INSERT TOOL
+ break;
- /**************************************************
- * STAGE 8
- *
- * - END COMPETITION
- *
- **************************************************/
- case END:
- servoPosition(STORE_POSITION);
- myled1 = 1;
- wait(.2);
- myled2 = 1;
- wait(.2);
- myled3 = 1;
- wait(.2);
- myled4 = 1;
- wait(.2);
- break;
- default:
+ /**************************************************
+ * STAGE 8
+ *
+ * - END COMPETITION
+ *
+ **************************************************/
+ case END:
+ servoPosition(STORE_POSITION);
+ myled1 = 1;
+ wait(.2);
+ myled2 = 1;
+ wait(.2);
+ myled3 = 1;
+ wait(.2);
+ myled4 = 1;
+ wait(.2);
+ break;
+ default:
- break;
- }
- }
+ break;
+ }
+ }
-}
+ }
-/************
-
-Servo Functions
-
-**************/
-
-void setServoPulse(uint8_t n, int angle)
-{
- int pulse;
- pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE);
- float pulselength = 20000; // 20,000 us per second
- int i = currentPosition[n];
- pc.printf("ServoNumber: %d Begining Pulse: %d\n\r",n,currentPosition[n]);
- int pulse2;
- if(currentPosition[n] < pulse) {
- for(i; i < pulse; i++) {
- pulse2 = 4094 * i / pulselength;
- pwm.setPWM(n, 0, pulse2);
- wait_ms(3);
- }
- } else if (currentPosition[n] > pulse) {
- for(i; i > pulse; i--) {
- pulse2 = 4094 * i / pulselength;
- pwm.setPWM(n, 0, pulse2);
- wait_ms(3);
- }
- }
- currentPosition[n] = i;
- pc.printf("\nEND: pulse: %d, angle: %d\n\n", i, angle);
-}
+ /************
-void initServoDriver(void)
-{
- pwm.begin();
- //pwm.setPWMFreq(100); //This dosen't work well because of uncertain clock speed. Use setPrescale().
- pwm.setPrescale(140); //This value is decided for 20ms interval.
- pwm.setI2Cfreq(400000); //400kHz
-
-}
+ Servo Functions
-void servoBegin(void)
-{
- pc.printf("Setting Initial Position\n\r");
- setServoPulseNo_delay(2, 0);
- setServoPulseNo_delay(3, 162);
- setServoPulseNo_delay(1, 10);
- setServoPulseNo_delay(0, 85);
- setServoPulseNo_delay(4, 175);
- setServoPulseNo_delay(5, 0);
- setGripper(1);
-}
-
-void setServoPulseNo_delay(uint8_t n, int angle)
-{
- int pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE);
- float pulselength = 20000; // 20,000 us per second
- currentPosition[n] = pulse;
- //pc.printf("ServoNumber: %d Pulsewidth: %d Angle: %d \n\r",n,pulse, angle);
- pulse = 4094 * pulse / pulselength;
- pwm.setPWM(n, 0, pulse);
-
-}
+ **************/
-void setGripper(int open)
-{
- if (open) {
- pc.printf("Gripper Open\r");
- setServoPulseNo_delay(6, 10);
- } else {
- pc.printf("Gripper Closed\n\r");
- setServoPulseNo_delay(6, 170);
- }
-}
-
-void servoPosition(int set)
-{
- //moves to current position
- setServoPulse(3, Arm_Table[set].claw_arm);
- setServoPulse(1, Arm_Table[set].base_arm);
- setServoPulse(2, Arm_Table[set].big_arm);
- setServoPulse(0, Arm_Table[set].base_rotate);
- setServoPulse(4, Arm_Table[set].claw_rotate);
- setServoPulse(5, Arm_Table[set].claw_open);
-}
-
-
-int fire_checker(int rig)
-{
- switch (rig) {
-
- 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)
- && (distLaser > OILRIG1_MIN)) {
- fire_detected++;
- } else {
- fire_not_detected++;
+ void setServoPulse(uint8_t n, int angle) {
+ int pulse;
+ pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE);
+ float pulselength = 20000; // 20,000 us per second
+ int i = currentPosition[n];
+ pc.printf("ServoNumber: %d Begining Pulse: %d\n\r",n,currentPosition[n]);
+ int pulse2;
+ if(currentPosition[n] < pulse) {
+ for(i; i < pulse; i++) {
+ pulse2 = 4094 * i / pulselength;
+ pwm.setPWM(n, 0, pulse2);
+ wait_ms(3);
}
- }
- break;
- case 2:
- for (int i = 0; i<5; i++) {
- distLaser = getDistance();
- pc.printf("L DISTANCE: %d \n\r", distLaser);
- if ((distLaser < OILRIG2_MAX)
- && (distLaser > OILRIG2_MIN)) {
- fire_detected++;
- } else {
- fire_not_detected++;
- }
- }
- break;
- case 3:
- for (int i = 0; i<5; i++) {
- distLaser = getDistance();
- if ((distLaser < OILRIG3_MAX)
- && (distLaser > OILRIG3_MIN)) {
- fire_detected++;
- } else {
- fire_not_detected++;
- }
- }
- break;
-
- default:
- for (int i = 0; i<5; i++) {
- distLaser = getDistance();
- if ((distLaser < OILRIG1_MAX)
- && (distLaser > OILRIG1_MIN)) {
- fire_detected++;
- } else {
- fire_not_detected++;
+ } else if (currentPosition[n] > pulse) {
+ for(i; i > pulse; i--) {
+ pulse2 = 4094 * i / pulselength;
+ pwm.setPWM(n, 0, pulse2);
+ wait_ms(3);
}
}
- break;
- }
+ currentPosition[n] = i;
+ pc.printf("END: pulse: %d, angle: %d\n\r", i, angle);
+ }
- if (fire_detected > fire_not_detected) {
- return 1;
- } else {
- return 0;
- }
-}
+ void initServoDriver(void) {
+ pwm.begin();
+ //pwm.setPWMFreq(100); //This dosen't work well because of uncertain clock speed. Use setPrescale().
+ pwm.setPrescale(140); //This value is decided for 20ms interval.
+ pwm.setI2Cfreq(400000); //400kHz
+
+ }
-void errFunction(void)
-{
- //Nothing
-}
-void wall_follow(int side, int direction, int section)
-{
- float location, set=6;
- int dir=1;
+ void servoBegin(void) {
+ pc.printf("Setting Initial Servo Position\n\r");
+ servoPosition(STORE_POSITION);
+ }
- pid1.reset();
+ void setServoPulseNo_delay(uint8_t n, int angle) {
+ int pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE);
+ float pulselength = 20000; // 20,000 us per second
+ currentPosition[n] = pulse;
+ //pc.printf("ServoNumber: %d Pulsewidth: %d Angle: %d \n\r",n,pulse, angle);
+ pulse = 4094 * pulse / pulselength;
+ pwm.setPWM(n, 0, pulse);
+
+ }
+
+
- if(direction == BACKWARD) dir=-1;
- if(section == TOOLS)set= 10;
+ void servoPosition(int set) {
+ //moves to current position
+ setServoPulse(3, Arm_Table[set].claw_arm);
+ setServoPulse(2, Arm_Table[set].big_arm);
+ setServoPulse(1, Arm_Table[set].base_arm);
+ setServoPulse(0, Arm_Table[set].base_rotate);
+ setServoPulse(4, Arm_Table[set].claw_rotate);
+ setServoPulse(5, Arm_Table[set].claw_open);
+ }
- 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;
+ int fire_checker(int rig) {
+ switch (rig) {
- 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);
+ case 1:
+ for (int i = 0; i<10; i++) {
+ distLaser = getDistance();
+ pc.printf("L DISTANCE: %d \n\r", distLaser);
+ if ((distLaser < OILRIG1_MAX)
+ && (distLaser > OILRIG1_MIN)) {
+ fire_detected++;
+ } else {
+ fire_not_detected++;
+ }
+ }
+ break;
+ case 2:
+ for (int i = 0; i<10; i++) {
+ distLaser = getDistance();
+ pc.printf("L DISTANCE: %d \n\r", distLaser);
+ if ((distLaser < OILRIG2_MAX)
+ && (distLaser > OILRIG2_MIN)) {
+ fire_detected++;
+ } else {
+ fire_not_detected++;
+ }
+ }
+ break;
+
+ }
+
+ if (fire_detected > 0) {
+ return 1;
+ } else {
+ return 0;
+ }
}
- 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 {
+ void errFunction(void) {
+ //Nothing
+ }
+ void wall_follow(int side, int direction, int section) {
+ float location, set=6;
+ int dir=1;
- pid1.setProcessValue(range);
- pid_return = pid1.compute();
+ pid1.reset();
+
+ if(direction == BACKWARD) dir=-1;
+ if(section == TOOLS)set= 10;
- 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) {
+ 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) {
- motors.setMotor0Speed(dir*MAX_SPEED);//right
- motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
+ rangeFinderLeft.startMeas();
+ wait_ms(20);
+ rangeFinderLeft.getMeas(range);
} else {
- motors.setMotor1Speed(dir*MAX_SPEED);//left
- motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
+ rangeFinderRight.startMeas();
+ wait_ms(20);
+ rangeFinderRight.getMeas(range);
+ pc.printf("%d\r\n",range);
}
- } 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=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;
+ 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.reset();
-
- if(direction == BACKWARD) {
- dir=-1;
- limit = 100;
- }
- if(section == TOOLS) {
- set= 6;
- limit = 86;
- }
- if(section == RETURN) lowlim=15;
+ pid1.setProcessValue(range);
+ pid_return = pid1.compute();
- leftEncoder.reset();
- rightEncoder.reset();
-
- //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);
+ 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
+ }
+ }
+ }
- pid1.setInputLimits(0.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);
+ //STOP
+ motors.setMotor0Speed(dir*-0.3*127); //right
+ motors.setMotor1Speed(dir*-0.3*127); //left
+ wait_ms(10);
+ motors.stopBothMotors(0);
}
- if(section == RIGS) {
- rangeFinderLeft.startMeas();
- wait_ms(20);
- rangeFinderLeft.getMeas(range2);
+ /* MODIFIED WALL_FOLLOW FOR NAVIGATION */
+
+ 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;
+ 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;
+ limit = 100;
+ }
+ if(section == TOOLS) {
+ set= 6;
+ limit = 86;
+ }
+ if(section == RETURN) lowlim=15;
+
+ leftEncoder.reset();
+ rightEncoder.reset();
+
+ //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);
+ pid1.setSetPoint(set);
+
+ if(side) {
+ rangeFinderLeft.startMeas();
+ wait_ms(20);
+ rangeFinderLeft.getMeas(range);
+ } else {
+ rangeFinderRight.startMeas();
+ wait_ms(20);
+ rangeFinderRight.getMeas(range);
+ }
+
+ if(section == RIGS) {
+ rangeFinderLeft.startMeas();
+ wait_ms(20);
+ 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(0);
- break;
+ 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 ) {
+ 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);
+
+ pc.printf("wavegap\r\n");
+ // AT WAVE OPENING!!!!
+ break;
+ }
+ }
+ } else {
+ 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);
+ }
}
}
+
+ //STOP
+ motors.setMotor0Speed(dir*-0.3*127); //right
+ motors.setMotor1Speed(dir*-0.3*127); //left
+ wait_ms(5);
+ motors.stopBothMotors(0);
}
- //pc.printf("wall follow 2 range %f\r\n",range);
- //pc.printf("loc+location = %f\r\n", loc+location);
- if(range > 20 ) {
- if(section == RIGS || section == RETURN) {
- motors.setMotor0Speed(dir*0.25*127); //right
- motors.setMotor1Speed(dir*0.25*127); //left
+ void alignWithWall(int section) {
+ 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(0);
+
+ //go backwards toward wall
+ leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor0Speed(-0.25*127); //right
+ motors.setMotor1Speed(-0.25*127); //left
+ while(abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300);
+ motors.stopBothMotors(0);
+
+ // turn left towards wall
+ leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor0Speed(MAX_SPEED); //right
+ motors.setMotor1Speed(-MAX_SPEED); //left
+ 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
+
+ } 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);
+
+ //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 {
- if(!SeeWaveGap) {
- SeeWaveGap=true;
+ pc.printf("in mid section align\r\n");
+ // turn right towards wall
+ rightTurn();
+ // turning left towards wall
+ motors.setMotor0Speed(0.9*MAX_SPEED); //right
+ motors.setMotor1Speed(-0.9*MAX_SPEED); //left
+ }
+
+ usValue = 0;
+ while(1) {
+ if(section == RIGS) {
+ rangeFinderRight.startMeas();
+ wait_ms(20);
+ rangeFinderRight.getMeas(range);
} else {
- //STOP
- motors.setMotor0Speed(dir*-0.25*127); //right
- motors.setMotor1Speed(dir*-0.25*127); //left
- wait_ms(5);
- motors.stopBothMotors(0);
-
- pc.printf("wavegap\r\n");
- // AT WAVE OPENING!!!!
+ 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;
}
}
- } else {
- SeeWaveGap = false;
- pid1.setProcessValue(range);
- pid_return = pid1.compute();
- //pc.printf("Range: %f\n PID: %f\r\n", range, pid_return);
+ motors.stopBothMotors(0);
+ }
+
+ void rightTurn(void) {
+ motors.begin();
+ leftEncoder.reset();
+ 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);
+ }
+
+ void leftTurn(void) {
+ motors.begin();
+ leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor0Speed(0.5*127);// right
+ motors.setMotor1Speed(-0.5*127);// left
+ while(abs(leftEncoder.getPulses())<1100 || rightEncoder.getPulses()<1100);
+ motors.stopBothMotors(0);
+ }
+
+ void slightleft(void) {
+
+ leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor0Speed(0.5*127);// right
+ motors.setMotor1Speed(-0.5*127);// left
+ 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);
+ }
- 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
+ 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();
+ // slight backwards
+ leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor0Speed(-0.25*127); //right
+ motors.setMotor1Speed(-0.25*127); //left
+ while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses()) < 50);
+ motors.stopBothMotors(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((abs(leftEncoder.getPulses()) < 800 || abs(rightEncoder.getPulses()) < 800) /*&& preLeft!=0*/ && IR.getIRDistance() >15 ) {
+ /*preLeft=leftEncoder.getPulses();
+ preRight=rightEncoder.getPulses();
+ wait_ms(200);
+ 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();
+
+ preLeft=preRight=5000 ;
+ leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor0Speed(.25*127); //right
+ 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)) {
+
+ if(IR.getIRDistance() > 38) break;
+
+ preLeft=leftEncoder.getPulses();
+ preRight=rightEncoder.getPulses();
+ wait_ms(200);
}
- } 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 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 {
- motors.setMotor0Speed(dir*MAX_SPEED);
- motors.setMotor1Speed(dir*MAX_SPEED);
+ 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;
}
- }
- }
- //STOP
- motors.setMotor0Speed(dir*-0.3*127); //right
- motors.setMotor1Speed(dir*-0.3*127); //left
- wait_ms(5);
- motors.stopBothMotors(0);
-}
+ 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);
+ wait_ms(20);
+ motors.begin();
+
+ }
+
+ void to_tools_section1(float* location, float ¤t) {
+ slightMove(FORWARD,6500);
+ current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
+
+ }
+
+ void to_tools_section2(float* location, float ¤t) {
+ slightMove(FORWARD,3200);
+ current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
+
+ }
+
+ void from_tools_section(float* location, float ¤t) {
-void alignWithWall(int section)
-{
- 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(0);
-
- //go backwards toward wall
- leftEncoder.reset();
- rightEncoder.reset();
- motors.setMotor0Speed(-0.25*127); //right
- motors.setMotor1Speed(-0.25*127); //left
- while(abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300);
- motors.stopBothMotors(0);
-
- // turn left towards wall
- leftEncoder.reset();
- rightEncoder.reset();
- motors.setMotor0Speed(MAX_SPEED); //right
- motors.setMotor1Speed(-MAX_SPEED); //left
- 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
-
- } else if( section == RIGS) {
- // check distance to wall
- rangeFinderRight.startMeas();
- wait_ms(20);
- rangeFinderRight.getMeas(range);
-
- if(range < 4 || range > 20) return;
+ alignWithWall(TOOLS);
+ pc.printf("align\r\n");
+ wait_ms(100);
- // 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);
-
- //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);
+ //wall_follow2(LEFT,FORWARD,MID, current);
+ //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
- // 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();
- // turning left towards wall
- motors.setMotor0Speed(0.9*MAX_SPEED); //right
- motors.setMotor1Speed(-0.9*MAX_SPEED); //left
- }
-
- usValue = 0;
- while(1) {
- 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(0);
-}
-
-void rightTurn(void)
-{
- motors.begin();
- leftEncoder.reset();
- 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);
-}
-
-void leftTurn(void)
-{
- motors.begin();
- leftEncoder.reset();
- rightEncoder.reset();
- motors.setMotor0Speed(0.5*127);// right
- motors.setMotor1Speed(-0.5*127);// left
- while(abs(leftEncoder.getPulses())<1100 || rightEncoder.getPulses()<1100);
- motors.stopBothMotors(0);
-}
-
-void slightleft(void)
-{
-
- leftEncoder.reset();
- rightEncoder.reset();
- motors.setMotor0Speed(0.5*127);// right
- motors.setMotor1Speed(-0.5*127);// left
- 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();
- // slight backwards
- leftEncoder.reset();
- rightEncoder.reset();
- motors.setMotor0Speed(-0.25*127); //right
- motors.setMotor1Speed(-0.25*127); //left
- while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses()) < 50);
- motors.stopBothMotors(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((abs(leftEncoder.getPulses()) < 800 || abs(rightEncoder.getPulses()) < 800) /*&& preLeft!=0*/ && IR.getIRDistance() >15 ) {
- /*preLeft=leftEncoder.getPulses();
- preRight=rightEncoder.getPulses();
- wait_ms(200);
- 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
+ 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
+ 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);
- 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();
+ wait_ms(100);
+ leftTurn();
+ overBump(TOOLS);
+ } else {
+ pc.printf("else greater than 20\r\n");
+ location[0]= current;
+ leftTurn();
+ overBump(TOOLS);
+ }
- preLeft=preRight=5000 ;
- leftEncoder.reset();
- rightEncoder.reset();
- motors.setMotor0Speed(.25*127); //right
- 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)) {
-
- if(IR.getIRDistance() > 38) break;
+ pc.printf("First Wavegap = %f\r\n",location[0]);
- 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()) < 100 || abs(rightEncoder.getPulses()) < 100);
+ void mid_section(float* location, float ¤t, int* direction) {
+ motors.begin();
- leftEncoder.reset();
- rightEncoder.reset();
+ 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,0);
+ current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+ pc.printf("after wf2 current = %f\r\n",current);
+
+ wait_ms(500);
+ rangeFinderLeft.startMeas();
+ wait_ms(20);
+ rangeFinderLeft.getMeas(range);
- 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;
+ if(range > 20 ) {
+ direction[0]= RIGHT;
+ location[1]= current;
+ slightMove(FORWARD,75);
+ //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+ } else {
+ direction[0]= LEFT;
+ 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();
+
+ wait_ms(100);
+
+ overBump(MID);
+
}
- 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;
- }
+ void mid_section2(float* location, float ¤t, int* direction) {
+ motors.begin();
- 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);
- wait_ms(20);
- motors.begin();
-
-}
+ pc.printf("mid section 2\r\n");
-void to_tools_section2(float* location, float ¤t)
-{
- slightMove(FORWARD,3200);
- current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
-
-}
-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;
-
-}
+ if(IR.getIRDistance() > 38) {
+ direction[1]= STRAIGHT;
+ overBump(RIGS);
+ return;
+ }
-void from_tools_section(float* location, float ¤t)
-{
-
+ alignWithWall(MID);
+ pc.printf("midsection 2 alignt with wall mid \r\n");
- alignWithWall(TOOLS);
- pc.printf("align\r\n");
- wait_ms(100);
+ wall_follow2(LEFT,FORWARD,MID, current,0);
+ current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
- //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);
+ wait_ms(500);
- 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
- 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);
-
- 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]);
-
-}
+ pc.printf("midseection 2 after wf2 %f",current);
+ rangeFinderLeft.startMeas();
+ wait_ms(20);
+ rangeFinderLeft.getMeas(range);
-void mid_section(float* location, float ¤t, int* direction)
-{
- motors.begin();
-
- 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,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(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);
+ } 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);
+ }
- wait_ms(500);
- rangeFinderLeft.startMeas();
- wait_ms(20);
- rangeFinderLeft.getMeas(range);
-
- if(range > 20 ) {
- direction[0]= RIGHT;
- location[1]= current;
- slightMove(FORWARD,75);
- //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
- } else {
- direction[0]= LEFT;
- 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);
+ leftTurn();
+ overBump(RIGS);
+ pc.printf("overbump rigs\r\n");
}
- }
-
- pc.printf("wavegap2 = %f\r\n",location[1]);
- leftTurn();
-
- wait_ms(100);
-
- overBump(MID);
-
-}
-
-void mid_section2(float* location, float ¤t, int* direction)
-{
- motors.begin();
+ void rig_section(float* location, float ¤t, int* direction, int rig) {
+ float loc;
- pc.printf("mid section 2\r\n");
-
- if(IR.getIRDistance() > 38) {
- direction[1]= STRAIGHT;
- overBump(RIGS);
- return;
- }
+ if(rig == 1) loc= 15;
+ else if(rig == 2) loc= 45;
+ else loc = 75;
- alignWithWall(MID);
- 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);
+ rightTurn();
+ slightright();
- pc.printf("midseection 2 after wf2 %f",current);
- rangeFinderLeft.startMeas();
- wait_ms(20);
- rangeFinderLeft.getMeas(range);
+ 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);
+ }
+ }
- 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);
- } 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);
- }
+ void tools_section_return(float* location, float ¤t) {
+ if(location[0] > 16) {
+ leftTurn();
+ wall_follow2(LEFT, BACKWARD, RETURN, location[0], 0);
+ }
+ motors.stopBothMotors(0);
- 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 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_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) {
+ 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 mid_section2_return(float* location, float ¤t, int* direction)
-{
- 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);
-}
+ 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);
+ }