For IEEE Robotics
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Diff: main.cpp
- Revision:
- 20:55dcff40c5d9
- Parent:
- 19:d4d967a885dc
- Child:
- 21:0907e1f5e16c
--- a/main.cpp Fri Apr 04 01:09:49 2014 +0000 +++ b/main.cpp Sat Apr 05 07:26:15 2014 +0000 @@ -10,6 +10,11 @@ #include "LPC17xx.h" #include "Sharp.h" + +#define PI 3.14159 + + + /* Navigation Definitions */ #define PIN_TRIGGERL (p12) #define PIN_ECHOL (p11) @@ -20,6 +25,7 @@ #define DIST_PER_PULSE (0.01054225722682) #define MTRS_TO_INCH (39.3701) #define MAX_SPEED (0.3*127) +#define MAX_SPEED1 (0.25*127) #define PPR (4331/4) #define LEFT (1) #define RIGHT (0) @@ -33,6 +39,7 @@ #define RETURN (4) #define FAR (1) + //States #define START 0 #define OILRIG1_POS 1 @@ -107,7 +114,6 @@ 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); @@ -158,6 +164,10 @@ void alignWithWall(int section); void UntilWall(int dir); + +float normd(int* pop, int count, int threshold); +int Xadjust(int tool); + extern "C" void mbed_reset(); /************ @@ -165,7 +175,7 @@ *************/ int state = START; int fire = 0; -int tool_needed = 0; +int tool_needed = 1; int shape_detected = 0; float range, range2, pid_return; int num, input; @@ -199,8 +209,8 @@ //increase in number 5 rotates gripper {STORE_POSITION, 85, 10, 0, 165, 175, 0}, // storing position - {OIL_RIG1, 162, 20, 60, 50, 175, 0}, // point laser at oilrig1 - {OIL_RIG2, 165, 20, 60, 50, 175, 0}, // point laser at oilrig2 + {OIL_RIG1, 156, 20, 60, 47, 175, 0}, // point laser at oilrig1 + {OIL_RIG2, 162, 20, 60, 47, 175, 0}, // point laser at oilrig2 {OIL_RIG3, 130, 90, 90, 52, 175, 0}, // point laser at oilrig2 {DRIVE_POSITION_NOTOOL, 85, 10, 0, 165, 175, 0}, // Drive through course {TOOL_1, 101, 50, 80, 113, 90, 0}, // Look over first tool @@ -246,11 +256,6 @@ - - - - - int main() { @@ -282,6 +287,8 @@ MAIN WHILE LOOP FOR COMPETITION *********************************/ + + while(1) { if(button_start == 1) { @@ -380,7 +387,7 @@ setServoPulse(5, Arm_Table[DRIVE_POSITION_NOTOOL].claw_open); wait(3); //wait for servos to settle - slightMove(FORWARD,3100); + slightMove(FORWARD,3350); current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; state = IDENTIFY_TOOLS; @@ -397,7 +404,7 @@ **************************************************/ case IDENTIFY_TOOLS: - wait(5); + //wait(5); servoPosition(TOOL_2); //arm/camera looks over tool wait(5); //wait for servos to settle @@ -428,51 +435,17 @@ } }*/ - - for(int i = 0; i < 4; i++) { - shape_detected = shapeDetection(); - wait(2); - if(get_com_x() > X_CENTER ) { - deltaX = get_com_x()-X_CENTER; - setServoPulse2(0,Arm_Table[TOOL_2].base_rotate += (deltaX/60.0) ); - Arm_Table[PU_TOOL_2].base_rotate = Arm_Table[TOOL_2].base_rotate; - } - if(get_com_x() < X_CENTER) { - deltaX = get_com_x()-X_CENTER; - setServoPulse2(0,Arm_Table[TOOL_2].base_rotate += (deltaX/60.0) ); - Arm_Table[PU_TOOL_2].base_rotate = Arm_Table[TOOL_2].base_rotate; - } - } - - - /* - - while( shape_alignX_done == 0) { - wait(1); - shape_detected = shapeDetection(); - - pc.printf("X - Adjust to center tool\n\r"); - if(get_com_x() > 95) { - setServoPulse(0,Arm_Table[TOOL_2].base_rotate+=1); - - } else if(get_com_x() < 65) { - setServoPulse(0,Arm_Table[TOOL_2].base_rotate-=1); - - } else { - shape_alignX_done = 1; - } - }*/ - + // aveArea = sumArea/count; //printImageToFile(BINARY); //either goes to aquire the tool or look at the next shape - if(shape_detected == tool_needed) { + if(Xadjust(TOOL_2) == tool_needed) { + //printImageToFile(BINARY); state = AQUIRE_TOOL2; break; } else { - slightMove(FORWARD,40); + //printImageToFile(BINARY); + slightMove(FORWARD,60); current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; - - servoPosition(TOOL_1); wait(5); //wait for servos to settle shape_alignX_done = 0; @@ -499,41 +472,7 @@ } }*/ - - for(int i = 0; i < 4; i++) { - shape_detected = shapeDetection(); - wait(2); - if (get_com_x() > X_CENTER) { - deltaX = get_com_x()-X_CENTER; - setServoPulse2(0,Arm_Table[TOOL_1].base_rotate += (deltaX/60.0) ); - Arm_Table[PU_TOOL_1].base_rotate = Arm_Table[TOOL_1].base_rotate; - } - if (get_com_x() < X_CENTER) { - deltaX = get_com_x()-X_CENTER; - setServoPulse2(0,Arm_Table[TOOL_1].base_rotate += (deltaX/60.0) ); - Arm_Table[PU_TOOL_1].base_rotate = Arm_Table[TOOL_1].base_rotate; - } - } - - - /* - while( shape_alignX_done == 0) { - wait(1); - shape_detected = shapeDetection(); - - pc.printf("X - Adjust to center tool\n\r"); - if(get_com_x() > 100) { - setServoPulse(0,Arm_Table[TOOL_1].base_rotate+=1); - - } else if(get_com_x() < 60) { - setServoPulse(0,Arm_Table[TOOL_1].base_rotate-=1); - - } else { - shape_alignX_done = 1; - } - }*/ - - if (shape_detected == tool_needed) { + if (Xadjust(TOOL_1) == tool_needed) { state = AQUIRE_TOOL1; break; } else { @@ -550,13 +489,10 @@ servoPosition(PU_TOOL_1); setServoPulse(4, 175); wait(5); - setServoPulse(0, Arm_Table[PU_TOOL_1].base_rotate ); + setServoPulse(0, Arm_Table[PU_TOOL_1].base_rotate - 1); wait(1); setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm - 5); wait(1); - setServoPulse(5, 105); - wait(.5); - setServoPulse(5, 00); setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm - 6); wait(.5); setServoPulse(5, 105); @@ -584,15 +520,12 @@ servoPosition(PU_TOOL_2); setServoPulse(4, 175); wait(5); - setServoPulse(0, Arm_Table[PU_TOOL_2].base_rotate ); + setServoPulse(0, Arm_Table[PU_TOOL_2].base_rotate - 1); wait(1); setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 2); wait(1); setServoPulse(3, Arm_Table[PU_TOOL_2].claw_arm - 2); wait(1); - setServoPulse(5, 105); - wait(2); - setServoPulse(5, 00); setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 6); wait(2); setServoPulse(5, 105); @@ -644,18 +577,7 @@ } */ - for(int i = 0; i < 6; i++) { - shape_detected = shapeDetection(); - wait(2); - if (get_com_x() > X_CENTER) { - deltaX = get_com_x()-X_CENTER; - setServoPulse2(0,Arm_Table[PU_TOOL_3].base_rotate += (deltaX/60.0) ); - } - if (get_com_x() < X_CENTER) { - deltaX = get_com_x()-X_CENTER; - setServoPulse2(0,Arm_Table[PU_TOOL_3].base_rotate += (deltaX/60.0) ); - } - } + Xadjust(TOOL_3); setServoPulse(4, 175); wait(5); @@ -665,9 +587,6 @@ wait(1); setServoPulse(3, Arm_Table[PU_TOOL_2].claw_arm - 3); wait(1); - setServoPulse(5, 105); - wait(.5); - setServoPulse(5, 00); setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm - 6); wait(.5); setServoPulse(5, 105); @@ -824,7 +743,7 @@ 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]); + //pc.printf("ServoNumber: %d Begining Pulse: %d\n\r",n,currentPosition[n]); int pulse2; if(currentPosition[n] < pulse) { for(i; i < pulse; i++) { @@ -840,7 +759,7 @@ } } currentPosition[n] = i; - pc.printf("END: pulse: %d, angle: %d\n\r", i, angle); + //pc.printf("END: pulse: %d, angle: %d\n\r", i, angle); } void initServoDriver(void) @@ -896,6 +815,12 @@ } + + + + + + int fire_checker(int rig) { switch (rig) { @@ -936,11 +861,16 @@ void errFunction(void) { - //Nothing + pc.printf("\n\nERROR: %d", motors.getError() ); + } + + + + void wall_follow(int side, int direction, int section) { - float location, set=6; + float location, set=4; int dir=1; pid1.reset(); @@ -967,7 +897,7 @@ rangeFinderRight.startMeas(); wait_ms(20); rangeFinderRight.getMeas(range); - pc.printf("%d\r\n",range); + //pc.printf("%d\r\n",range); } if(range > 15) { @@ -1014,8 +944,8 @@ 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; + int dir=1, limit=80, lowlim=4; + float set=9, loc=0, Rigloc=0; bool SeeWaveGap = false; if(rig == 1) Rigloc= 16; @@ -1027,28 +957,31 @@ if(direction == BACKWARD) { dir=-1; limit = 100; - } + } else if(direction == FORWARD) lowlim=-20; if(section == TOOLS) { - set= 6; + set= 9; limit = 86; - } - if(section == RETURN) lowlim=15; + } else if(section == RIGS) set = 9; + else if(section == RETURN) lowlim=4; + else if(section == MID2) limit =85; + + if(location <4) limit=80; leftEncoder.reset(); rightEncoder.reset(); - //pc.printf("before %f\r\n", location); + 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); + //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); + //pc.printf("loc %f \r\n", loc); pid1.setInputLimits(0.0, set); - pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED); + pid1.setOutputLimits( -MAX_SPEED1, MAX_SPEED1); pid1.setSetPoint(set); if(side) { @@ -1066,7 +999,7 @@ wait_ms(20); rangeFinderLeft.getMeas(range2); - if(range2< 20) { + if(range2< 15) { if( abs(dir*loc + location - Rigloc) < 10) { //STOP motors.stopBothMotors(127); @@ -1078,18 +1011,19 @@ //pc.printf("wall follow 2 range %f\r\n",range); //pc.printf("loc+location = %f\r\n", loc+location); - if(range > 20 ) { + if(range > 15 ) { if(section == RIGS || section == RETURN) { motors.setMotor0Speed(dir*0.25*127); //right motors.setMotor1Speed(dir*0.25*127); //left } else { if(!SeeWaveGap) { + wait_ms(40); SeeWaveGap=true; } else { //STOP motors.stopBothMotors(127); - pc.printf("wavegap\r\n"); + //pc.printf("wavegap\r\n"); // AT WAVE OPENING!!!! break; } @@ -1098,7 +1032,7 @@ SeeWaveGap = false; pid1.setProcessValue(range); pid_return = pid1.compute(); - //pc.printf("Range: %f\n PID: %f\r\n", range, pid_return); + pc.printf("Range: %f\n PID: %f\r\n", range, pid_return); if(pid_return > 0) { if(side) { @@ -1128,15 +1062,12 @@ } - - - void alignWithWall(int section) { float usValue = 0; if(section == TOOLS) { - pc.printf("tools section align\r\n"); + //pc.printf("tools section align\r\n"); // turn at an angle leftEncoder.reset(); rightEncoder.reset(); @@ -1160,11 +1091,28 @@ motors.setMotor1Speed(-MAX_SPEED); //left while(rightEncoder.getPulses() < 20 || abs(leftEncoder.getPulses()) < 20); - motors.stopBothMotors(0); + motors.stopBothMotors(127); + wait_ms(300); + return; + /* + rangeFinderRight.startMeas(); + wait_ms(20); + rangeFinderRight.getMeas(range); + if(range>15){ + // turning left + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(-MAX_SPEED); //right + motors.setMotor1Speed(MAX_SPEED); //left + while(abs(rightEncoder.getPulses()) < 50 || abs(leftEncoder.getPulses()) < 50); + motors.stopBothMotors(127); + return; + } + */ // turning left - motors.setMotor0Speed(0.9*MAX_SPEED); //right - motors.setMotor1Speed(-0.9*MAX_SPEED); //left + //motors.setMotor0Speed(0.9*MAX_SPEED); //right + //motors.setMotor1Speed(-0.9*MAX_SPEED); //left } else if(section == RIGS) { // check distance to wall @@ -1172,7 +1120,7 @@ wait_ms(20); rangeFinderRight.getMeas(range); - if(range < 3 || range > 20) return; + if(range < 3) return; // turn at an angle leftEncoder.reset(); @@ -1206,34 +1154,61 @@ motors.setMotor0Speed(-0.9*MAX_SPEED); //right motors.setMotor1Speed(0.9*MAX_SPEED); //left */ - } else {// MID - pc.printf("in mid section align\r\n"); + } else if(section == MID2) { + 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()) < 150 || abs(rightEncoder.getPulses()) < 150); + + // turn left towards wall + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(0.4*127); //right + motors.setMotor1Speed(-0.4*127); //left + while(abs(rightEncoder.getPulses()) < 65 || abs(leftEncoder.getPulses()) < 65); + motors.stopBothMotors(127); + + slightMove(FORWARD,100); + return; + + } else { // MID + //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 == 10) { // CURENTLY NOT USED (WAS RIGS) - rangeFinderRight.startMeas(); - wait_ms(20); - rangeFinderRight.getMeas(range); - } else { - rangeFinderLeft.startMeas(); - wait_ms(20); - rangeFinderLeft.getMeas(range); + /* while(1) { + if(section == 10) { // CURENTLY NOT USED (WAS 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; + } } - 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); + motors.stopBothMotors(0);*/ } void rightTurn(void) @@ -1243,7 +1218,7 @@ rightEncoder.reset(); motors.setMotor0Speed(-0.5*127);//right motors.setMotor1Speed(0.5*127);//left - while(abs(leftEncoder.getPulses())<1050 || abs(rightEncoder.getPulses())<1050); + while(abs(leftEncoder.getPulses())<850 || abs(rightEncoder.getPulses())<850); motors.stopBothMotors(127); } @@ -1254,7 +1229,7 @@ rightEncoder.reset(); motors.setMotor0Speed(0.5*127);// right motors.setMotor1Speed(-0.5*127);// left - while(abs(leftEncoder.getPulses())<1100 || rightEncoder.getPulses()<1100); + while(abs(leftEncoder.getPulses())<1075 || rightEncoder.getPulses()<1075); motors.stopBothMotors(127); } @@ -1265,7 +1240,7 @@ rightEncoder.reset(); motors.setMotor0Speed(0.5*127);// right motors.setMotor1Speed(-0.5*127);// left - while(abs(leftEncoder.getPulses())<90 || rightEncoder.getPulses()<90); + while(abs(leftEncoder.getPulses())<70 || rightEncoder.getPulses()<70); motors.stopBothMotors(127); } @@ -1276,7 +1251,7 @@ rightEncoder.reset(); motors.setMotor0Speed(-0.4*127);// right motors.setMotor1Speed(0.4*127);// left - while(abs(leftEncoder.getPulses())<50 || abs(rightEncoder.getPulses())<50); + while(abs(leftEncoder.getPulses())<200 || abs(rightEncoder.getPulses())<200); motors.stopBothMotors(127); } @@ -1329,7 +1304,7 @@ while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses()) < 50); motors.stopBothMotors(127); - pc.printf("slight backwards\r\n"); + //pc.printf("slight backwards\r\n"); wait_ms(200); // Over bump @@ -1337,16 +1312,11 @@ 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; - */ - } + while((abs(leftEncoder.getPulses()) < 800 || abs(rightEncoder.getPulses()) < 800) /*&& preLeft!=0*/ && IR.getIRDistance() >15 ); + - pc.printf("forward \r\n"); + + //pc.printf("forward \r\n"); wait_ms(200); motors.stopBothMotors(0); @@ -1368,8 +1338,9 @@ 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()) < 300 || abs(rightEncoder.getPulses()) < 300) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) { + if(section == MID2) while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100)); + + while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) { if(IR.getIRDistance() > 38) break; @@ -1394,6 +1365,7 @@ if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0; } motors.stopBothMotors(127); + motors.begin(); return; } @@ -1403,16 +1375,18 @@ motors.begin(); } + + void to_tools_section1(float* location, float ¤t) { - slightMove(FORWARD,6600); + slightMove(FORWARD,6800); 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); + slightMove(FORWARD,3250); current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; } @@ -1420,43 +1394,44 @@ void from_tools_section(float* location, float ¤t) { - alignWithWall(TOOLS); - pc.printf("align\r\n"); - wait_ms(100); + //alignWithWall(TOOLS); + //current-=4; + + //slightMove(FORWARD,150); + //current+=1; + //pc.printf("align\r\n"); + //wait_ms(200); //wall_follow2(LEFT,FORWARD,MID, current); - //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + slightMove(BACKWARD,400); + current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + rangeFinderLeft.startMeas(); wait_ms(20); rangeFinderLeft.getMeas(range); - if(range < 20) { - wall_follow2(LEFT,BACKWARD,TOOLS, current,0); - pc.printf("wall follow\r\n"); + if(range < 15) { + wall_follow2(LEFT,BACKWARD,MID, 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); + //pc.printf("current %f \r\n",current); // go backwards - leftEncoder.reset(); - rightEncoder.reset(); - motors.setMotor0Speed(-MAX_SPEED); //right - motors.setMotor1Speed(-MAX_SPEED); //left - while(abs(leftEncoder.getPulses()) < 120 || abs(rightEncoder.getPulses())< 120); - // hard stop - motors.stopBothMotors(127); + //slightMove(BACKWARD,200); wait_ms(100); leftTurn(); + slightleft(); overBump(TOOLS); } else { - pc.printf("else greater than 20\r\n"); + //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("First Wavegap = %f\r\n",location[0]); } void tools_section(float* location, float ¤t) @@ -1470,14 +1445,15 @@ /////////////////////////////////////////////////////////////////////////////////////// // Move Forward slightMove(FORWARD, 100); + current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); //////////////////////////////////////////Tool aquiring wait(2); //////////////////////////////////////////////////////////////////// After tool is aquired - alignWithWall(TOOLS); - pc.printf("align\r\n"); - wait_ms(100); + //alignWithWall(TOOLS); + //pc.printf("align\r\n"); + //wait_ms(100); //wall_follow2(LEFT,FORWARD,MID, current); //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); @@ -1488,10 +1464,10 @@ if(range < 20) { wall_follow2(LEFT,BACKWARD,TOOLS, current,0); - pc.printf("wall follow\r\n"); + //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); + //pc.printf("current %f \r\n",current); // go backwards leftEncoder.reset(); rightEncoder.reset(); @@ -1506,32 +1482,36 @@ leftTurn(); overBump(TOOLS); } else { - pc.printf("else greater than 20\r\n"); + //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("First Wavegap = %f\r\n",location[0]); } 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("before align with wall \r\n"); + //alignWithWall(MID); + //current-=4; + //wait_ms(200); - pc.printf("mid section current = %f\r\n",current); + //if(current > 20){ + //alignWithWall(MID2); + //current-=4; + //} + rightTurn(); + //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); + //pc.printf("after wf2 current = %f\r\n",current); wait_ms(500); rangeFinderLeft.startMeas(); @@ -1541,8 +1521,9 @@ 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); + wait_ms(300); + slightMove(FORWARD,100); + 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); @@ -1550,14 +1531,23 @@ current= location[1]; if(location[1] < 18) { - slightMove(FORWARD, 50); + slightMove(FORWARD, 75); current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); } + //slightMove(BACKWARD,100); } - pc.printf("wavegap2 = %f\r\n",location[1]); - leftTurn(); + wait_ms(200); + //pc.printf("wavegap2 = %f\r\n",location[1]); + //left turn + motors.begin(); + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(0.5*127);// right + motors.setMotor1Speed(-0.5*127);// left + while(abs(leftEncoder.getPulses())<1045 || rightEncoder.getPulses()<1045); + motors.stopBothMotors(127); wait_ms(100); @@ -1567,9 +1557,7 @@ void mid_section2(float* location, float ¤t, int* direction) { - motors.begin(); - - pc.printf("mid section 2\r\n"); + //pc.printf("mid section 2\r\n"); if(IR.getIRDistance() > 38) { direction[1]= STRAIGHT; @@ -1577,15 +1565,20 @@ return; } - alignWithWall(MID); - pc.printf("midsection 2 alignt with wall mid \r\n"); + //alignWithWall(MID); + wait_ms(100); + + rightTurn(); + slightright(); + wait_ms(100); + wall_follow2(LEFT,FORWARD,MID, current,0); current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); wait_ms(500); - pc.printf("midseection 2 after wf2 %f",current); + //pc.printf("midseection 2 after wf2 %f",current); rangeFinderLeft.startMeas(); wait_ms(20); rangeFinderLeft.getMeas(range); @@ -1593,19 +1586,27 @@ if(range > 20 ) { direction[1]= RIGHT; location[2]= current; - slightMove(FORWARD,100); + slightMove(FORWARD,50); 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,100); + //slightMove(BACKWARD,100); } - leftTurn(); + //LEFT turn + motors.begin(); + leftEncoder.reset(); + rightEncoder.reset(); + motors.setMotor0Speed(0.5*127);// right + motors.setMotor1Speed(-0.5*127);// left + while(abs(leftEncoder.getPulses())<950 || rightEncoder.getPulses()<950); + motors.stopBothMotors(127); + overBump(RIGS); - pc.printf("overbump rigs\r\n"); + //pc.printf("overbump rigs\r\n"); } void rig_section(float* location, float ¤t, int* direction, int rig) @@ -1617,28 +1618,34 @@ else loc = 75; // Slight forward for turn - slightMove(FORWARD,100); + slightMove(FORWARD,150); + current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); wait_ms(100); rightTurn(); - slightright(); - wait(5); + //slightright(); + if(current > loc) { - pc.printf("RIG section %f\r\n",current); + //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); + //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); } - alignWithWall(RIGS); - // Go forward until Rig + alignWithWall(MID2); + current-=4; wall_follow2(RIGHT, FORWARD, RIGS, current, rig); - // line back wheel up with side of rig - slightMove(FORWARD,300); + current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + slightMove(FORWARD, 75); current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); + + + + + } void tools_section_return(float* location, float ¤t) @@ -1697,4 +1704,83 @@ } rightTurn(); overBump(MID2); -} \ No newline at end of file +} + + + + +int Xadjust(int tool) +{ + int areaArray[10]; + float C, T, S; + for(int i = 0; i < 10; i++) { + areaArray[i] = shapeDetection(); + wait(2); + if(get_com_x() > X_CENTER ) { + deltaX = get_com_x()-X_CENTER; + setServoPulse2(0,Arm_Table[tool].base_rotate += (deltaX/60.0) ); + Arm_Table[PU_TOOL_2].base_rotate = Arm_Table[TOOL_2].base_rotate; + } + if(get_com_x() < X_CENTER) { + deltaX = get_com_x()-X_CENTER; + setServoPulse2(0,Arm_Table[tool].base_rotate += (deltaX/60.0) ); + Arm_Table[PU_TOOL_2].base_rotate = Arm_Table[TOOL_2].base_rotate; + } + + + } + + C = normd(areaArray, 10, CIRCLE_AREA); + // S = normd(areaArray, 10, SQUARE_AREA); + // T = normd(areaArray, 10, TRIANGLE_AREA); + + if((C < SQUARE_AREA) && (C > CIRCLE_AREA)) { + pc.printf("\n\nCIRCLE DETECTED\n\r"); + return CIRCLE; + } else if( ( C > SQUARE_AREA ) ) { + pc.printf("\n\nSQUARE DETECTED\n\r"); + return SQUARE; + } else { + pc.printf("\n\nTRIANGLE DETECTED\n\r"); + return TRIANGLE; + + + +/* + if((C < S) && (C < T)) { + pc.printf("\n\nCIRCLE DETECTED\n\r"); + return CIRCLE; + } else if( ( S<C ) && ( S<T ) ) { + pc.printf("\n\nSQUARE DETECTED\n\r"); + return SQUARE; + } else { + pc.printf("\n\nTRIANGLE DETECTED\n\r"); + return TRIANGLE; + }*/ +} + +float normd(int* pop, int count, int threshold) +{ + int i = 0; + float mean=0, std=0; + for(i=0; i<count; i++) { + mean += pop[i]; + } + mean /= (float)count; + pc.printf("\n\nMean: %f\n\r", mean); + + for(i=0; i<count; i++) { + std += pow(((float)pop[i]-mean),2); + } + std /= (float)count; + std = sqrt(std); + //pc.printf("\n\nStd: %f\n\r", std); + + //pc.printf("\n\nNorm: %f\n\r", (1/(std*sqrt(2*PI)))*exp(-pow(((float)threshold-mean),2)/(2*pow(std,2)))); + + + //return abs(mean - threshold); + return mean; + //return (1/(std*sqrt(2*PI)))*exp(-pow(((float)threshold-mean),2)/(2*pow(std,2))); + +}