For IEEE Robotics
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Diff: main.cpp
- Revision:
- 8:77a57909aa15
- Parent:
- 7:8fb4204f9600
- Child:
- 9:1b29cd9ed1ba
diff -r 8fb4204f9600 -r 77a57909aa15 main.cpp --- a/main.cpp Wed Mar 19 17:08:25 2014 +0000 +++ b/main.cpp Thu Mar 27 23:16:02 2014 +0000 @@ -1,16 +1,35 @@ #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" -DigitalOut myled1(LED1); -DigitalOut myled2(LED2); -DigitalOut myled3(LED3); -DigitalOut myled4(LED4); - -Serial pc(USBTX,USBRX); //USB Comm -Adafruit_PWMServoDriver pwm(p28,p27); //Servo Control PWM -DigitalOut ServoOutputDisable(p8); //Servo Control Output Enable/Disable -extern Serial lrf; //Laser Range Finder lrf(p13,p14) +/* 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 @@ -32,6 +51,7 @@ #define END 16 + //Servo Static Positions #define STORE_POSITION 0 #define OIL_RIG1 1 @@ -53,18 +73,45 @@ //******* TODO ********// //*********************// //Oil Rig distance thresholds -#define OILRIG1_MAX 1200 -#define OILRIG1_MIN 1200 -#define OILRIG2_MAX 1200 -#define OILRIG2_MIN 1200 -#define OILRIG3_MAX 1200 -#define OILRIG3_MIN 1200 +#define OILRIG1_MAX 3000 +#define OILRIG1_MIN 1000 +#define OILRIG2_MAX 3000 +#define OILRIG2_MIN 1000 +#define OILRIG3_MAX 3000 +#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 ****************/ @@ -76,6 +123,25 @@ 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 *************/ @@ -83,6 +149,8 @@ int fire = 0; int tool_needed = 0; int shape_detected = 0; +float range, range2, pid_return; + /************ Variables for Servos @@ -109,15 +177,17 @@ // POSITION ODER: // base_rotate, base_arm, int big_arm, int claw_arm, int claw_rotate, int claw_open - {STORE_POSITION, 90, 0, 0, 0, 90, 90}, // storing position - {OIL_RIG1, 90, 60, 75, 60, 90, 90}, // point laser at oilrig1 - {OIL_RIG2, 120, 60, 75, 60, 90, 90}, // point laser at oilrig2 - {OIL_RIG3, 135, 60, 75, 60, 90, 90}, // point laser at oilrig2 - {DRIVE_POSITION_NOTOOL, 135, 60, 75, 60, 90, 90}, // Drive through course - {TOOL_1, 135, 60, 75, 60, 90, 90}, // Look over first tool - {TOOL_2, 135, 60, 75, 60, 90, 90}, // Look over second tool - {TOOL_3, 135, 60, 75, 60, 90, 90}, // Look over third tool - {DRIVE_POSITION_TOOL, 135, 60, 75, 60, 90, 90}, // Drive with tool loaded +//increase in number 5 rotates gripper + + {STORE_POSITION, 85, 5, 5, 175, 100, 0}, // storing position + {OIL_RIG1, 163, 90, 90, 48, 100, 0}, // point laser at oilrig1 + {OIL_RIG2, 144, 90, 90, 47, 100, 0}, // point laser at oilrig2 + {OIL_RIG3, 130, 90, 90, 50, 100, 0}, // point laser at oilrig2 + {DRIVE_POSITION_NOTOOL, 55, 70, 102, 74, 0, 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 }; @@ -129,7 +199,7 @@ int shape; /* Variables for distance sensor*/ -int dist; +int distLaser; int fire_detected = 0; int fire_not_detected = 0; @@ -140,9 +210,14 @@ /***************** INITIALIZATIONS *******************/ + float location[3], current=0; + int direction[3]; + double distance; + pc.baud(115200); //Laser Range Finder Initialization lrf_baudCalibration(); + motors.begin(); //Servo initialization initServoDriver(); @@ -153,11 +228,25 @@ WHILE LOOP FOR TESTING ********************/ while(1) { - //pc.scanf("%d %d", &servoNum, &servoAngle); - //setServoPulse(servoNum, servoAngle); + pc.scanf("%d %d", &servoNum, &servoAngle); + if(servoAngle > 175) { + servoAngle = 175; + } + if(servoNum > 5 ) { + servoNum = 0; + servoAngle = 90; + } + setServoPulse(servoNum, servoAngle); + + //ledtoggle(); - pc.scanf("%d", &posNum); - servoPosition(posNum); + //pc.scanf("%d", &posNum); + //servoPosition(posNum); + //wait(3); + //shape_detected = shapeDetection(); + //distLaser = getDistance(); + //pc.printf("Distance %d", distLaser); + } /******************************** @@ -165,7 +254,7 @@ *********************************/ while(1) { switch (state) { - + /************************************************** * STAGE 0 * @@ -188,7 +277,7 @@ **************************************************/ case OILRIG1_POS: //aims arm at square oil rig servoPosition(OIL_RIG1); //position arm to point at first oilrig - wait(1); //wait for servos to settle + wait(2); //wait for servos to settle fire = fire_checker(OIL_RIG1); //determines if oil rig is on fire //determines what tool is needed @@ -223,14 +312,13 @@ servoPosition(DRIVE_POSITION_NOTOOL); //drive position without a tool wait(1); //wait for servos to settle - //*********************// - //******* TODO ********// - //*********************// - //CODE TO NAVIGATE TO TOOLS +//****************************************************// + + to_tools_section(location, current); state = IDENTIFY_TOOLS; break; - + while(1) {} /************************************************** * STAGE 3 * @@ -451,12 +539,17 @@ void servoBegin(void) { pc.printf("Setting Initial Position\n\r"); - setServoPulseNo_delay(0, 90); - setServoPulseNo_delay(1, 0); - setServoPulseNo_delay(2, 177); - setServoPulseNo_delay(3, 0); - setServoPulseNo_delay(4, 0); - setServoPulseNo_delay(5, 90); + setServoPulseNo_delay(3, 175); + wait(2); + setServoPulseNo_delay(2, 0); + wait(2); + setServoPulseNo_delay(1, 10); + wait(2); + setServoPulseNo_delay(0, 85); + wait(1); + setServoPulseNo_delay(4, 100); + wait(1); + setServoPulseNo_delay(5, 0); setGripper(1); } @@ -465,7 +558,7 @@ 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); + //pc.printf("ServoNumber: %d Pulsewidth: %d Angle: %d \n\r",n,pulse, angle); pulse = 4094 * pulse / pulselength; pwm.setPWM(n, 0, pulse); @@ -485,23 +578,24 @@ void servoPosition(int set) { //moves to current position - setServoPulse(0, Arm_Table[set].base_rotate); + setServoPulse(3, Arm_Table[set].claw_arm); setServoPulse(1, Arm_Table[set].base_arm); setServoPulse(2, Arm_Table[set].big_arm); - setServoPulse(3, Arm_Table[set].claw_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++) { - dist = getDistance(); - if ((dist < OILRIG1_MAX) - || (dist > OILRIG1_MIN)) { + distLaser = getDistance(); + if ((distLaser < OILRIG1_MAX) + || (distLaser > OILRIG1_MIN)) { fire_detected++; } else { fire_not_detected++; @@ -509,9 +603,9 @@ } case 2: for (int i = 0; i<5; i++) { - dist = getDistance(); - if ((dist < OILRIG2_MAX) - || (dist > OILRIG2_MIN)) { + distLaser = getDistance(); + if ((distLaser < OILRIG2_MAX) + || (distLaser > OILRIG2_MIN)) { fire_detected++; } else { fire_not_detected++; @@ -519,9 +613,9 @@ } case 3: for (int i = 0; i<5; i++) { - dist = getDistance(); - if ((dist < OILRIG3_MAX) - || (dist > OILRIG3_MIN)) { + distLaser = getDistance(); + if ((distLaser < OILRIG3_MAX) + || (distLaser > OILRIG3_MIN)) { fire_detected++; } else { fire_not_detected++; @@ -529,9 +623,9 @@ } default: for (int i = 0; i<5; i++) { - dist = getDistance(); - if ((dist < OILRIG1_MAX) - || (dist > OILRIG1_MIN)) { + distLaser = getDistance(); + if ((distLaser < OILRIG1_MAX) + || (distLaser > OILRIG1_MIN)) { fire_detected++; } else { fire_not_detected++; @@ -546,5 +640,485 @@ } } +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< 73) { + 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.getDistance() >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.getDistance() >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.getDistance() > 20 ) { + //pc.printf("IR %f\r\n", IR.getDistance()); + //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(); + + //Tool aquiring + //wait(2); + // After tool is aquired + +} + +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.getDistance() > 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.getDistance() > 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); + } + + +