nav fixed
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of theRobot by
main.cpp
- Committer:
- tashworth
- Date:
- 2014-03-29
- Revision:
- 10:1a1d52207f59
- Parent:
- 9:1b29cd9ed1ba
- Child:
- 11:8d2455e383ce
File content as of revision 10:1a1d52207f59:
#include "mbed.h" #include "Adafruit_PWMServoDriver.h" #include "ShapeDetect.h" #include "rtos.h" #include "PID.h" #include "PololuQik2.h" #include "QEI.h" #include "HCSR04.h" #include "stdio.h" #include "LPC17xx.h" #include "Sharp.h" /* Navigation Definitions */ #define PIN_TRIGGERL (p12) #define PIN_ECHOL (p11) #define PIN_TRIGGERR (p29) #define PIN_ECHOR (p30) #define PULSE_PER_REV (1192) #define WHEEL_CIRCUM (12.56637) #define DIST_PER_PULSE (0.01054225722682) #define MTRS_TO_INCH (39.3701) #define MAX_SPEED (0.3*127) #define PPR (4331/4) #define LEFT (1) #define RIGHT (0) #define FORWARD (1) #define BACKWARD (0) #define TOOLS (0) #define MID (1) #define RIGS (2) #define FIRST_WAVE (0) #define FAR (1) //States #define START 0 #define OILRIG1_POS 1 #define OILRIG2_POS 2 #define GOTO_TOOLS 3 #define IDENTIFY_TOOLS 4 #define AQUIRE_TOOL1 5 #define AQUIRE_TOOL2 6 #define AQUIRE_TOOL3 7 #define NAVIGATE_WAVES_ROW1 8 #define NAVIGATE_WAVES_ROW2 9 #define NAVIGATE_WAVES_ROW3 10 #define NAVIGATE_TO_SQUARE_RIG 11 #define NAVIGATE_TO_TRIANGLE_RIG 12 #define NAVIGATE_TO_CIRCLE_RIG 13 #define RIG_ALIGN 14 #define INSERT_TOOL 15 #define END 16 //Servo Static Positions #define STORE_POSITION 0 #define OIL_RIG1 1 #define OIL_RIG2 2 #define OIL_RIG3 3 #define DRIVE_POSITION_NOTOOL 4 #define TOOL_1 5 #define TOOL_2 6 #define TOOL_3 7 #define DRIVE_POSITION_TOOL 8 #define ORIENT_TOOL 9 //Rig definitions #define SQUARE 1 #define TRIANGLE 2 #define CIRCLE 3 //*********************// //******* TODO ********// //*********************// //Oil Rig distance thresholds #define OILRIG1_MAX 3000 #define OILRIG1_MIN 1000 #define OILRIG2_MAX 5000 #define OILRIG2_MIN 1000 #define OILRIG3_MAX 5000 #define OILRIG3_MIN 1000 //for servo normalization #define MIN_SERVO_PULSE 900 #define MAX_SERVO_PULSE 2100 #define SERVO_MAX_ANGLE 180 DigitalOut myled1(LED1); DigitalOut myled2(LED2); DigitalOut myled3(LED3); DigitalOut myled4(LED4); void errFunction(void); bool cRc; Serial pc(USBTX,USBRX); //USB Comm Adafruit_PWMServoDriver pwm(p28,p27); //pwm(SDA,SCL) - Servo Control PWM DigitalOut ServoOutputDisable(p8); //Servo Control Output Enable/Disable extern Serial lrf; //Laser Range Finder lrf(p13,p14) //Hardware Initialization //Serial bt(p13,p14); HCSR04 rangeFinderLeft( PIN_TRIGGERL, PIN_ECHOL ); HCSR04 rangeFinderRight( PIN_TRIGGERR, PIN_ECHOR ); PID pid1(15.0,0.0,4.0,0.02); PololuQik2 motors(p9, p10, p8, p15, errFunction, cRc); QEI rightEncoder(p17,p18,NC,PPR,QEI::X4_ENCODING); QEI leftEncoder(p16,p15,NC,PPR,QEI::X4_ENCODING); Sharp IR(p20); //InterruptIn encoder(p29); /*************** local servo functions ****************/ void servoBegin(void); void initServoDriver(void); 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); //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 leftTurn(void); void slightleft(void); void rightTurn(void); void us_distance(void); void to_tools_section(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 overBump(int section); void alignWithWall(int section); void ledtoggle(void); /************ Main Variables *************/ int state = START; int fire = 0; int tool_needed = 0; int shape_detected = 0; float range, range2, pid_return; /************ Variables for Servos *************/ int servoNum, servoAngle, outputDisabled, posNum, testVal; int currentPosition[7]; typedef struct { int arm_position_name; //for organization only (STORE, OILRIG1, OILRIG2...) int base_rotate; int base_arm; int big_arm; int claw_arm; int claw_rotate; int claw_open; } Coord; /******************** Static Arm Positions *********************/ Coord Arm_Table[] = { // POSITION ODER: // base_rotate, base_arm, int big_arm, int claw_arm, int claw_rotate, int claw_open //increase in number 5 rotates gripper {STORE_POSITION, 85, 10, 0, 175, 100, 0}, // storing position {OIL_RIG1, 161, 90, 90, 47, 100, 0}, // point laser at oilrig1 {OIL_RIG2, 140, 90, 90, 44, 100, 0}, // point laser at oilrig2 {OIL_RIG3, 130, 90, 90, 57, 100, 0}, // point laser at oilrig2 {DRIVE_POSITION_NOTOOL, 85, 10, 0, 175, 100, 0}, // Drive through course {TOOL_1, 95, 64, 97, 79, 0, 0}, // Look over first tool {TOOL_2, 75, 70, 102, 74, 0, 0}, // Look over second tool {TOOL_3, 55, 70, 102, 74, 0, 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 }; /* Variables for imageprocessing and distance */ int x_coord; int y_coord; int area; int shape; /* Variables for distance sensor*/ int distLaser; int fire_detected = 0; int fire_not_detected = 0; int main() { /***************** INITIALIZATIONS *******************/ float location[3], current=6; int direction[3]; double distance; pc.baud(115200); //Laser Range Finder Initialization lrf_baudCalibration(); motors.begin(); //Servo initialization initServoDriver(); servoBegin(); //initiates servos to start position //servoPosition(0); 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); //shape_detected = shapeDetection(); //distLaser = getDistance(); //pc.printf("Distance %d", distLaser); //ledtoggle(); //pc.scanf("%d", &posNum); //servoPosition(posNum); //wait(5); //shape_detected = shapeDetection(); distLaser = getDistance(); pc.printf("Distance %d", distLaser); } /******************************** MAIN WHILE LOOP FOR COMPETITION *********************************/ while(1) { switch (state) { /************************************************** * STAGE 0 * * - START OF THE COMETITION * **************************************************/ case START : myled1 = 1; wait(1); myled1 = 0; state = OILRIG1_POS; break; /************************************************** * STAGE 1 * * - DETERMINE OIL RIG ON FIRE * **************************************************/ case OILRIG1_POS: //aims arm at square oil rig setServoPulse(3, Arm_Table[OIL_RIG1].claw_arm); setServoPulse(2, 50); setServoPulse(1, Arm_Table[OIL_RIG1].base_arm); setServoPulse(2, Arm_Table[OIL_RIG1].big_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); //wait for servos to settle fire = fire_checker(OIL_RIG1); //determines if oil rig is on fire pc.printf("FIRE: %d", fire); //determines what tool is needed if (fire == 1) { tool_needed = SQUARE; state = GOTO_TOOLS; } else { state = OILRIG2_POS; } break; case OILRIG2_POS: servoPosition(OIL_RIG2); //position arm to point at first oilrig wait(1); //wait for servos to settle fire = fire_checker(OIL_RIG2); pc.printf("FIRE: %d", fire); if (fire == 1) { tool_needed = TRIANGLE; state = GOTO_TOOLS; } else { tool_needed = CIRCLE; state = GOTO_TOOLS; } pc.printf("tool needed: %d", tool_needed); break; /************************************************** * STAGE 2 * * - TRAVEL TO TOOLS * **************************************************/ case GOTO_TOOLS: 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); setServoPulse(5, Arm_Table[DRIVE_POSITION_NOTOOL].claw_open); wait(5); //wait for servos to settle to_tools_section(location, current); state = IDENTIFY_TOOLS; pc.printf("YES!!!!!"); while(1); break; /************************************************** * STAGE 3 * * - Determine order of tools * - Aquire appropriate tool * **************************************************/ case IDENTIFY_TOOLS: servoPosition(TOOL_1); //arm/camera looks over tool wait(1); //wait for servos to settle centerCamWithTool(); //centers camera over tool shape_detected = shapeDetection(); //determines the shape //either goes to aquire the tool or look at the next shape if(shape_detected == tool_needed) { state = AQUIRE_TOOL1; } else { servoPosition(TOOL_2); wait(1); //wait for servos to settle centerCamWithTool(); shape_detected = shapeDetection(); if (shape_detected == tool_needed) { state = AQUIRE_TOOL2; } else { servoPosition(TOOL_3); wait(1); //wait for servos to settle centerCamWithTool(); state = AQUIRE_TOOL3; } } break; case AQUIRE_TOOL1: //*********************// //******* TODO ********// //*********************// // CODE TO GRAB TOOL1 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(DRIVE_POSITION_TOOL); //arm position for driving with the tool state = NAVIGATE_WAVES_ROW1; break; case AQUIRE_TOOL3: //*********************// //******* TODO ********// //*********************// // CODE TO GRAB TOOL3 servoPosition(DRIVE_POSITION_TOOL); //arm position for driving with the tool state = NAVIGATE_WAVES_ROW1; break; /************************************************** * STAGE 4 * * - Navigate through the ocean * **************************************************/ 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_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; /************************************************** * 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: //*********************// //******* TODO ********// //*********************// // CODE TO ALIGN ROBOT WITH RIG servoPosition(ORIENT_TOOL); wait(1); //wait for servos to settle state = INSERT_TOOL; break; /************************************************** * STAGE 7 * * - Insert Tool * - Extenguish fire * - win contest * **************************************************/ 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: 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 } void servoBegin(void) { pc.printf("Setting Initial Position\n\r"); setServoPulseNo_delay(3, 175); setServoPulseNo_delay(2, 0); setServoPulseNo_delay(1, 2); setServoPulseNo_delay(0, 85); setServoPulseNo_delay(4, 100); 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++) { 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<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++; } } break; } if (fire_detected > fire_not_detected) { return 1; } else { return 0; } } void errFunction(void) { //Nothing } void us_distance(void) { 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; int dir=1; pid1.reset(); if(direction == BACKWARD) dir=-1; if(section == TOOLS)set= 10; leftEncoder.reset(); rightEncoder.reset(); location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; while(location< 70) { 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) { rangeFinderLeft.startMeas(); wait_ms(38); rangeFinderLeft.getMeas(range); } else { rangeFinderRight.startMeas(); wait_ms(38); 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; //pc.printf("wavegap %f\r\n",wavegap); // AT WAVE OPENING!!!! motors.setMotor1Speed(dir*0.4*127);//left motors.setMotor0Speed(dir*0.4*127);//right } else { pid1.setProcessValue(range); pid_return = pid1.compute(); 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 } } } return wavegap; } /* MODIFIED WALL_FOLLOW FOR NAVIGATION */ void wall_follow2(int side, int direction, int section, float location) { int SeeWaveGap = false, dir=1; float set=5, loc=0; pid1.reset(); if(direction == BACKWARD) dir=-1; if(section == TOOLS)set= 5; leftEncoder.reset(); rightEncoder.reset(); while(dir*loc + location <= 78) { loc=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; pid1.setInputLimits(0.0, set); pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED); pid1.setSetPoint(set); if(side) { rangeFinderLeft.startMeas(); wait_ms(38); rangeFinderLeft.getMeas(range); } else { rangeFinderRight.startMeas(); wait_ms(38); rangeFinderRight.getMeas(range); } /*************CHECK FOR WAVE OPENING*****************/ /* If after 20 ms the ultrasonic still sees 20+ cm */ /* then robot is at wave opening */ //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(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(); } void alignWithWall(int section) { float usValue = 0; if(section == TOOLS) { // 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(); //go backwards toward wall leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(-MAX_SPEED); //right motors.setMotor1Speed(-MAX_SPEED); //left while(abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300); motors.stopBothMotors(); // 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); motors.stopBothMotors(); motors.setMotor0Speed(0.7*MAX_SPEED); //right motors.setMotor1Speed(-0.7*MAX_SPEED); //left } else { rightTurn(); motors.setMotor0Speed(-0.7*MAX_SPEED); //right motors.setMotor1Speed(0.7*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(range > usValue && usValue != 0 && range < 25) { break; } else { usValue = range; } } motors.stopBothMotors(); } void rightTurn(void) { motors.begin(); leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(-0.5*127);//right motors.setMotor1Speed(0.5*127);//left while(leftEncoder.getPulses()<900 || rightEncoder.getPulses()>-900); motors.stopBothMotors(); } 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(); } void slightleft(void) { leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(0.5*127);// right motors.setMotor1Speed(-0.5*127);// left while(abs(leftEncoder.getPulses())<50 || rightEncoder.getPulses()<50); motors.stopBothMotors(); } void overBump(int section) { int preLeft=5000, preRight=5000, out=0; motors.begin(); leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(-0.2*127); //right motors.setMotor1Speed(-0.2*127); //left while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses())< 50); motors.stopBothMotors(); leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(0.2*127); //right motors.setMotor1Speed(0.2*127); //left while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.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.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; }*/ 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 } if(abs(leftEncoder.getPulses()) <1000 || abs(leftEncoder.getPulses())<1000) out=1; } motors.stopBothMotors(); wait(2); motors.begin(); preLeft=preRight=5000 ; leftEncoder.reset(); rightEncoder.reset(); 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); } } else while((abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses())< 200)); motors.setMotor0Speed(-.25*127); //right motors.setMotor1Speed(-.25*127); //left wait_ms(10); motors.stopBothMotors(); wait(2); motors.begin(); } void to_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; pc.printf("current %f \r\n",current); motors.stopBothMotors(); wait(2); motors.setMotor0Speed(-.2*127); //right motors.setMotor1Speed(-.2*127); //left wait_ms(5); motors.stopBothMotors(); } void from_tools_section(float* location, float ¤t) { alignWithWall(TOOLS); wait_ms(100); wall_follow2(LEFT,FORWARD,MID, current); current= 78; rangeFinderLeft.startMeas(); wait_ms(20); rangeFinderLeft.getMeas(range); if(range < 20) { wall_follow2(LEFT,BACKWARD,TOOLS, current); leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(-MAX_SPEED); //right motors.setMotor1Speed(-MAX_SPEED); //left while(abs(leftEncoder.getPulses()) < 120 || abs(rightEncoder.getPulses())< 120); leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(MAX_SPEED); //right motors.setMotor1Speed(MAX_SPEED); //left while(abs(leftEncoder.getPulses()) < 40 || abs(rightEncoder.getPulses())< 40); motors.stopBothMotors(); wait_ms(500); location[0]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); current= location[0]; leftTurn(); slightleft(); overBump(TOOLS); } else { location[0]= 77; leftTurn(); wait_ms(20); overBump(FIRST_WAVE); } 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; alignWithWall(MID); 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); pc.printf("after wf2 current = %f\r\n",current); if(current != 0) { direction[0]= RIGHT; current+= location[0]; location[1]= current; } 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); } 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(); } void mid_section2(float* location, float ¤t, int* direction) { motors.begin(); if(IR.getIRDistance() > 20) return; alignWithWall(MID); wall_follow2(LEFT,FORWARD,MID, current); current=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); if(current != 0) { direction[1]= RIGHT; current+= location[1]; location[2]= current; } 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); } leftTurn(); overBump(RIGS); } void rig_section(float* location, float ¤t, int* direction, int rig) { } void ledtoggle(void) { pwm.setPWM(9, 0, 4094); wait(2); pwm.setPWM(9, 0, 0); }