uyvug
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of theRobot by
main.cpp
- Committer:
- Fairy_Paolina
- Date:
- 2014-04-15
- Revision:
- 27:5540aa3c828c
- Parent:
- 26:7257bd16bc67
- Child:
- 28:2bb6b0fe39d0
File content as of revision 27:5540aa3c828c:
#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" #define PI 3.14159 /* 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 MAX_SPEED1 (0.25*127) #define PPR (4331/4) #define LEFT (1) #define RIGHT (0) #define STRAIGHT (2) #define FORWARD (1) #define BACKWARD (0) #define TOOLS (0) #define MID (1) #define RIGS (2) #define MID2 (3) #define RETURN (4) #define FAR (1) //States #define START 0 #define OILRIG1_POS 1 #define OILRIG2_POS 2 #define GOTO_TOOLS1 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 #define GOTO_TOOLS2 17 #define RETURN_TO_START 18 //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 #define PU_TOOL_1 10 #define PU_TOOL_2 11 #define PU_TOOL_3 12 #define PU_TOOL_1_STAB 13 #define PU_TOOL_2_STAB 14 #define PU_TOOL_3_STAB 15 //Rig definitions #define SQUARE 1 #define TRIANGLE 2 #define CIRCLE 3 //Oil Rig distance thresholds #define OILRIG1_MAX 1800 #define OILRIG1_MIN 1000 #define OILRIG2_MAX 1800 #define OILRIG2_MIN 1000 #define OILRIG3_MAX 1800 #define OILRIG3_MIN 1000 //for servo normalization #define MIN_SERVO_PULSE 900 #define MAX_SERVO_PULSE 2100 #define SERVO_MAX_ANGLE 180 #define X_CENTER 80 #define Y_CENTER 60 DigitalOut myled1(LED1); DigitalOut myled2(LED2); DigitalOut myled3(LED3); DigitalOut myled4(LED4); InterruptIn startBtn(p7); void errFunction(void); bool cRc; Serial pc(USBTX,USBRX); //USB Comm Adafruit_PWMServoDriver pwm(p28,p27); //pwm(SDA,SCL) - Servo Control PWM extern Serial lrf; //Laser Range Finder lrf(p13,p14) //Hardware Initialization //Serial bt(p13,p14); HCSR04 rangeFinderFront( 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 IRFront(p19); Sharp IRBack(p20); //InterruptIn encoder(p29); /*************** local servo functions ****************/ void servoBegin(void); void initServoDriver(void); void setServoPulse(uint8_t n, int angle); void setServoPulse2(uint8_t n, float angle); //float precision void setServoPulseNo_delay(uint8_t n, int angle); void servoPosition(int set); int fire_checker(int rig); int button_start = 0; //Navigation Functions void wall_follow2(int side, int direction, int section, float location, int rig); void leftTurn(void); void slightleft(void); void slightright(void); void rightTurn(void); void slightMove(int direction, float pulses); void tools_section_return(float* location, float ¤t); void to_tools_section1(float* location, float ¤t); void to_tools_section2(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 mid_section_return(float* location, float ¤t, int* direction); void mid_section2_return(float* location, float ¤t, int* direction); void rig_section_return(float* location, float ¤t, int* direction); void overBump(int section); void alignWithWall(int section); void UntilWall(int dir); void testSensors(void); float normd(int* pop, int count, int threshold); int Xadjust(int tool); extern "C" void mbed_reset(); /************ Main Variables *************/ int state = START; int fire = 0; int tool_needed = 1; int shape_detected = 0; float range, range2, range3, pid_return; int num, input; /************ 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, 165, 175, 0}, // storing position {OIL_RIG1, 164, 20, 60, 44, 175, 0}, // point laser at oilrig1 {OIL_RIG2, 164, 20, 60, 44, 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 {TOOL_2, 82, 50, 80, 113, 90, 0}, // Look over second tool {TOOL_3, 62, 50, 80, 112, 90, 0}, // Look over third tool {DRIVE_POSITION_TOOL, 85, 10, 0, 165, 90, 105}, // Drive with tool loaded {ORIENT_TOOL, 135, 60, 75, 60, 90, 90}, // position tool to be inserted {PU_TOOL_1, 99, 46, 78, 110, 170, 0}, // STATIC Pickup tool POSITION {PU_TOOL_2, 76, 47, 80, 112, 170, 0}, // STATIC Pickup tool POSITION {PU_TOOL_3, 59, 44, 76, 110, 170, 0}, // STATIC Pickup tool POSITION {PU_TOOL_1_STAB, 98, 50, 90, 118, 90, 0}, // STAB TOOL 1 {PU_TOOL_2_STAB, 78, 50, 90, 108, 90, 0}, // STAB TOOL 2 {PU_TOOL_3_STAB, 53, 50, 90, 118, 90, 0}, // STAB TOOL 3 }; /* Variables for imageprocessing and distance */ int x_coord; int y_coord; int area; int shape; int shape_alignX_done = 0; int shape_alignY_done = 0; float deltaX = 0; /* Variables for distance sensor*/ int distLaser; int fire_detected = 0; int fire_not_detected = 0; void button_int(void) { if(!button_start) { button_start = 1; wait(1.0); } else { button_start = 0; mbed_reset(); } return; } int main() { /***************** INITIALIZATIONS *******************/ float location[3], current=4; int direction[3]; double distance; int pu, num, input; pc.baud(115200); //testSensors(); //Laser Range Finder Initialization lrf_baudCalibration(); motors.begin(); startBtn.rise(&button_int); //Servo initialization initServoDriver(); servoBegin(); //initiates servos to start position //ServoOutputDisable = 0; /******************************** MAIN WHILE LOOP FOR COMPETITION *********************************/ while(1) { if(button_start == 1) { switch (state) { /************************************************** * STAGE 0 * * - START OF THE COMETITION * **************************************************/ case START : myled1 = 1; //current=75; //state = NAVIGATE_WAVES_ROW1; state = OILRIG1_POS; break; /************************************************** * STAGE 1 * * - DETERMINE OIL RIG ON FIRE * **************************************************/ case OILRIG1_POS: //aims arm at square oil rig 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 //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); wait(3); //wait for servos to settle to_tools_section2(location, current); // moves to second rig servoPosition(OIL_RIG2); //position arm to point at first oilrig wait(3); fire = fire_checker(OIL_RIG2); 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; } break; /************************************************** * STAGE 2 * * - TRAVEL TO TOOLS * **************************************************/ 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(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(3); //wait for servos to settle slightMove(FORWARD,3250); current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; state = IDENTIFY_TOOLS; break; /************************************************** * STAGE 3 * * - Determine order of tools * - Aquire appropriate tool * **************************************************/ case IDENTIFY_TOOLS: //wait(5); servoPosition(TOOL_2); //arm/camera looks over tool wait(5); //wait for servos to settle //shape_detected = shapeDetection(); //determines the shape //clearBounds(); //printImageToFile(BINARY); shape_alignX_done = 0; shape_alignY_done = 0; /* while( shape_alignY_done == 0) { wait(1); shape_detected = shapeDetection(); pc.printf("Y - Adjust to center tool\n\r"); if(get_com_y() < 50) { wait(1); slightMove(FORWARD,25); current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; } else if(get_com_y() > 70) { wait(1); slightMove(BACKWARD,25); current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; } else { shape_alignY_done = 1; } }*/ // aveArea = sumArea/count; //printImageToFile(BINARY); //either goes to aquire the tool or look at the next shape //****************//if(Xadjust(TOOL_2) == tool_needed) { if(Xadjust(TOOL_2) == 162) { //printImageToFile(BINARY); state = AQUIRE_TOOL2; break; } else { //printImageToFile(BINARY); slightMove(FORWARD,70); 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; shape_alignY_done = 0; /* while( shape_alignY_done == 0) { wait(1); shape_detected = shapeDetection(); pc.printf("Y - Adjust to center tool\n\r"); if(get_com_y() < 50) { wait(1); slightMove(FORWARD,25); current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; } else if(get_com_y() > 70) { wait(1); slightMove(BACKWARD,25); current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; } else { shape_alignY_done = 1; } }*/ //****************// if (Xadjust(TOOL_1) == tool_needed) { if (Xadjust(TOOL_1) == 169) { state = AQUIRE_TOOL1; break; } else { servoPosition(TOOL_3); wait(3); //wait for servos to settle state = AQUIRE_TOOL3; } } break; case AQUIRE_TOOL1: servoPosition(PU_TOOL_1); setServoPulse(4, 175); wait(5); 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(1, Arm_Table[PU_TOOL_1].base_arm - 6); wait(.5); setServoPulse(5, 140); wait(.5); setServoPulse(5, 00); setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm - 10); wait(1); setServoPulse(5, 140); wait(1); setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm); wait(2); setServoPulse(2, Arm_Table[DRIVE_POSITION_TOOL].big_arm); setServoPulse(3, Arm_Table[DRIVE_POSITION_TOOL].claw_arm); setServoPulse(1, Arm_Table[DRIVE_POSITION_TOOL].base_arm); setServoPulse(0, Arm_Table[DRIVE_POSITION_TOOL].base_rotate); setServoPulse(4, Arm_Table[DRIVE_POSITION_TOOL].claw_rotate); setServoPulse(5, Arm_Table[DRIVE_POSITION_TOOL].claw_open); state = NAVIGATE_WAVES_ROW1; break; case AQUIRE_TOOL2: servoPosition(PU_TOOL_2); setServoPulse(4, 175); wait(5); 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(1, Arm_Table[PU_TOOL_2].base_arm - 6); wait(2); setServoPulse(5, 140); wait(2); setServoPulse(5, 00); setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 10); wait(2); setServoPulse(5, 140); wait(2); setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm); wait(2); setServoPulse(2, Arm_Table[DRIVE_POSITION_TOOL].big_arm); setServoPulse(3, Arm_Table[DRIVE_POSITION_TOOL].claw_arm); setServoPulse(1, Arm_Table[DRIVE_POSITION_TOOL].base_arm); setServoPulse(0, Arm_Table[DRIVE_POSITION_TOOL].base_rotate); setServoPulse(4, Arm_Table[DRIVE_POSITION_TOOL].claw_rotate); setServoPulse(5, Arm_Table[DRIVE_POSITION_TOOL].claw_open); state = NAVIGATE_WAVES_ROW1; break; case AQUIRE_TOOL3: /* while( shape_alignY_done == 0) { wait(1); servoPosition(PU_TOOL_3); shape_detected = shapeDetection(); wait(2); pc.printf("Y - Adjust to center tool\n\r"); if(get_com_y() < 50) { wait(1); slightMove(FORWARD,25); current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; } else if(get_com_y() > 70) { wait(1); slightMove(BACKWARD,25); current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; } else { shape_alignY_done = 1; } } */ Xadjust(TOOL_3); setServoPulse(4, 175); wait(5); setServoPulse(0, Arm_Table[PU_TOOL_3].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 - 3); wait(1); setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm - 9); wait(.5); setServoPulse(5, 110); wait(.5); setServoPulse(5, 00); setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm - 10); wait(1); setServoPulse(5, 115); wait(1); setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm); wait(2); setServoPulse(2, Arm_Table[DRIVE_POSITION_TOOL].big_arm); setServoPulse(3, Arm_Table[DRIVE_POSITION_TOOL].claw_arm); setServoPulse(1, Arm_Table[DRIVE_POSITION_TOOL].base_arm); setServoPulse(0, Arm_Table[DRIVE_POSITION_TOOL].base_rotate); setServoPulse(4, Arm_Table[DRIVE_POSITION_TOOL].claw_rotate); setServoPulse(5, Arm_Table[DRIVE_POSITION_TOOL].claw_open); state = NAVIGATE_WAVES_ROW1; break; /************************************************** * STAGE 4 * * - Navigate through the ocean * **************************************************/ case NAVIGATE_WAVES_ROW1: from_tools_section(location,current); mid_section(location, current, direction); state = NAVIGATE_WAVES_ROW2; break; case NAVIGATE_WAVES_ROW2: mid_section2(location, current, direction); state = NAVIGATE_WAVES_ROW3; break; case NAVIGATE_WAVES_ROW3: //shape_detected = 1; if(tool_needed == 1) { rig_section(location, current, direction, 1); //state = NAVIGATE_TO_SQUARE_RIG; state = RETURN_TO_START; } else if(tool_needed == 2) { rig_section(location, current, direction, 2); //state = NAVIGATE_TO_TRIANGLE_RIG; state = RETURN_TO_START; } else { rig_section(location, current, direction, 3); //state = NAVIGATE_TO_CIRCLE_RIG; state = RETURN_TO_START; } 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 * * - Return to start zone * **************************************************/ case RETURN_TO_START: wait(3); rig_section_return(location, current, direction); mid_section2_return(location, current, direction); mid_section_return(location, current, direction); tools_section_return(location,current); while(1); state = END; break; /************************************************** * STAGE 9 * * - 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; } } // End while loop } // End if for start button } // main loop /************ 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("END: pulse: %d, angle: %d\n\r", 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 Servo Position\n\r"); setServoPulseNo_delay(3, Arm_Table[STORE_POSITION].claw_arm); setServoPulseNo_delay(2, Arm_Table[STORE_POSITION].big_arm); wait(2); setServoPulseNo_delay(1, Arm_Table[STORE_POSITION].base_arm); setServoPulseNo_delay(0, Arm_Table[STORE_POSITION].base_rotate); setServoPulseNo_delay(4, Arm_Table[STORE_POSITION].claw_rotate); setServoPulseNo_delay(5, Arm_Table[STORE_POSITION].claw_open); } 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 setServoPulse2(uint8_t n, float angle) { float pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE); float pulselength = 20000; // 10,000 us per second pulse = 4094 * pulse / pulselength; pwm.setPWM(n, 0, pulse); } 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); } int fire_checker(int rig) { switch (rig) { 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; } } void errFunction(void) { pc.printf("\n\nERROR: %d", motors.getError() ); } /* MODIFIED WALL_FOLLOW FOR NAVIGATION */ void wall_follow2(int side, int direction, int section, float location, int rig) { int dir=1, limit=80, lowlim=4; float set=28, loc=0, Rigloc=0, location_cal=0; bool SeeWaveGap = false; if(rig == 1) Rigloc= 16; else if(rig == 2) Rigloc= 45; else if(rig== 3) Rigloc = 70; pid1.reset(); if(section == TOOLS) { limit = 100; lowlim=10; }else if(section == RIGS) set = 18; else if(section == MID2) limit =85; if(direction == BACKWARD) { dir=-1; limit = 100; } else if(direction == FORWARD) lowlim=-20; if(location <4) limit=80; 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); if((section == RIGS || section == RETURN) && side == LEFT)location_cal= -1*dir*loc + location; else location_cal= dir*loc + location; while((location_cal <= limit) && (location_cal >= lowlim)) { loc=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; if((section == RIGS || section == RETURN) && side == LEFT)location_cal= -1*dir*loc + location; else location_cal= dir*loc + location; pid1.setInputLimits(0.0, set); pid1.setOutputLimits( -MAX_SPEED1, MAX_SPEED1); pid1.setSetPoint(set); if(side) { range = IRFront.getIRDistance(); range3= IRBack.getIRDistance(); } else { rangeFinderRight.startMeas(); wait_ms(20); rangeFinderRight.getMeas(range); } if(section == RIGS) { if(side == LEFT){ range2 = IRFront.getIRDistance(); range3= IRBack.getIRDistance(); }else{ rangeFinderRight.startMeas(); wait_ms(20); rangeFinderRight.getMeas(range2); } if(range2< 15) { if( abs(location_cal - Rigloc) < 10) { //STOP motors.stopBothMotors(127); break; } } } //pc.printf("wall follow 2 range %f\r\n",range); //pc.printf("loc+location = %f\r\n", loc+location); if(range > 35 && range3 > 35) { if(section != RETURN) { pc.printf("RANGE %f \tRANGE3 %f\r\n",range,range3); //STOP motors.stopBothMotors(127); 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.stopBothMotors(127); } 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(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 } else if(section == RIGS) { // turn left 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())<500); motors.stopBothMotors(0); wait_ms(200); //go backwards away form 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(127); wait(2); // turn right away from wall leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(-MAX_SPEED); //right motors.setMotor1Speed(MAX_SPEED); //left while(abs(rightEncoder.getPulses()) < 25 || abs(leftEncoder.getPulses()) < 25 ); motors.stopBothMotors(127); } else if(section == MID2) { // check distance to wall if(IRFront.getIRDistance() > 4) { 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 { range = IRFront.getIRDistance(); } //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())<800 || abs(rightEncoder.getPulses())<800); motors.stopBothMotors(127); wait_ms(300); if(IRFront.getIRDistance() > 35 && IRBack.getIRDistance() > 35) return; // align with wave while(IRFront.getIRDistance() > 35){ pc.printf("front sensor sees gap\r\n"); motors.setMotor0Speed(-0.1*127);//right motors.setMotor1Speed(-0.1*127);//left } while(IRBack.getIRDistance() >35 ){ pc.printf("back sensor sees gap\r\n"); motors.setMotor0Speed(0.1*127);//right motors.setMotor1Speed(0.1*127);//left } motors.stopBothMotors(127); wait_ms(300); while(IRFront.getIRDistance() - IRBack.getIRDistance() > 0.5){ pc.printf("turn left\r\n"); motors.setMotor0Speed(0.3*127);// right motors.setMotor1Speed(-0.3*127);// left } while(IRFront.getIRDistance() - IRBack.getIRDistance() < -0.5){ pc.printf("turn right\r\n"); motors.setMotor0Speed(-0.3*127);//right motors.setMotor1Speed(0.3*127);// left } motors.stopBothMotors(127); wait_ms(300); } 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())<1075 || rightEncoder.getPulses()<1075); motors.stopBothMotors(127); wait_ms(300); if(IRFront.getIRDistance() > 35 && IRBack.getIRDistance() > 35) return; // align with wave while(IRFront.getIRDistance() > 35){ pc.printf("front sensor sees gap\r\n"); motors.setMotor0Speed(-0.1*127);//right motors.setMotor1Speed(-0.1*127);//left } while(IRBack.getIRDistance() >35 ){ pc.printf("back sensor sees gap\r\n"); motors.setMotor0Speed(0.1*127);//right motors.setMotor1Speed(0.1*127);//left } motors.stopBothMotors(127); wait_ms(300); while(IRFront.getIRDistance() - IRBack.getIRDistance() > 0.5){ pc.printf("turn left\r\n"); motors.setMotor0Speed(0.3*127);// right motors.setMotor1Speed(-0.3*127);// left } while(IRFront.getIRDistance() - IRBack.getIRDistance() < -0.5){ pc.printf("turn right\r\n"); motors.setMotor0Speed(-0.3*127);//right motors.setMotor1Speed(0.3*127);// left } motors.stopBothMotors(127); wait_ms(300); } void slightleft(void) { leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(0.5*127);// right motors.setMotor1Speed(-0.5*127);// left while(abs(leftEncoder.getPulses())<70 || rightEncoder.getPulses()<70); motors.stopBothMotors(127); } void slightright(void) { leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(-0.4*127);// right motors.setMotor1Speed(0.4*127);// left while(abs(leftEncoder.getPulses())<200 || abs(rightEncoder.getPulses())<200); motors.stopBothMotors(127); } 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.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.stopBothMotors(127); } void overBump(int section) { float avg=0; int i; // first set motors.setMotor1Speed(0.1*127);//left motors.setMotor0Speed(0.1*127);//right wait_ms(100); while(motors.GetMotor0Current()<5 || motors.GetMotor1Current() <5 ); while(motors.GetMotor0Current()>5 || motors.GetMotor1Current() >5 ) { pc.printf("left %d \t right %d \r\n", motors.GetMotor0Current(), motors.GetMotor1Current()); motors.setMotor1Speed(0.3*127);//left motors.setMotor0Speed(0.3*127);//right wait_ms(220); motors.stopBothMotors(127); wait_ms(10); } motors.setMotor1Speed(0.1*127);//left motors.setMotor0Speed(0.1*127);//right wait_ms(100); while(motors.GetMotor0Current()<5 || motors.GetMotor1Current() <5 ); motors.setMotor1Speed(0.3*127);//left motors.setMotor0Speed(0.3*127);//right // second set wait_ms(200); motors.stopBothMotors(127); wait_ms(500); if(section!= RIGS) { range = 0; do{ rangeFinderFront.startMeas(); wait_ms(1); while(rangeFinderFront.getMeas(range) != RANGE_MEAS_INVALID); pc.printf("Ultrasonic front sensor %f\r\n", range); if(range < 9){ motors.stopBothMotors(127); wait_ms(200); break; } motors.setMotor1Speed(0.3*127);//left motors.setMotor0Speed(0.3*127);//right wait_ms(220); motors.stopBothMotors(127); wait_ms(500); }while(range > 9); motors.setMotor1Speed(0.1*127);//left motors.setMotor0Speed(0.1*127);//right wait_ms(100); while(motors.GetMotor0Current()<5 || motors.GetMotor1Current() <5); motors.stopBothMotors(127); wait_ms(200); } else { leftEncoder.reset(); rightEncoder.reset(); while(leftEncoder.getPulses()<500 || rightEncoder.getPulses()<500) { motors.setMotor1Speed(0.1*127);//left motors.setMotor0Speed(0.1*127);//right wait_ms(220); if(motors.GetMotor0Current()>5 || motors.GetMotor1Current() >5){ motors.setMotor1Speed(0.3*127);//left motors.setMotor0Speed(0.3*127);//right wait_ms(220); motors.stopBothMotors(127); wait_ms(200); leftEncoder.reset(); rightEncoder.reset(); } motors.stopBothMotors(127); wait_ms(200); } } motors.stopBothMotors(127); wait_ms(20); motors.begin(); } void to_tools_section1(float* location, float ¤t) { slightMove(FORWARD,6650); current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; } void to_tools_section2(float* location, float ¤t) { slightMove(FORWARD,3250); current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; } void from_tools_section(float* location, float ¤t) { pc.printf("IR front %f \t back %f \r\n", IRFront.getIRDistance(), IRBack.getIRDistance()); if(IRFront.getIRDistance() < 37 || IRBack.getIRDistance() < 37) { wall_follow2(LEFT,BACKWARD,TOOLS, current,0); pc.printf("IR front %f \t back %f \r\n", IRFront.getIRDistance(), IRBack.getIRDistance()); 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 //slightMove(BACKWARD,200); wait_ms(100); leftTurn(); slightleft(); overBump(TOOLS); } else { pc.printf("IR front %f \t back %f \r\n", IRFront.getIRDistance(), IRBack.getIRDistance()); //pc.printf("else greater than 20\r\n"); location[0]= current; leftTurn(); overBump(TOOLS); } pc.printf("First Wavegap = %f\r\n",location[0]); } void mid_section(float* location, float ¤t, int* direction) { wait_ms(500); rangeFinderFront.startMeas(); wait_ms(20); rangeFinderFront.getMeas(range); if(range > 20) { 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); direction[0]= STRAIGHT; overBump(MID); return; } //pc.printf("before align with wall \r\n"); //alignWithWall(MID); //current-=4; //wait_ms(200); //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); wait_ms(500); if(IRFront.getIRDistance() > 37 && IRBack.getIRDistance() > 37) { wait(1); direction[0]= RIGHT; location[1]= current; wait_ms(300); slightMove(FORWARD,200); 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, 75); current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); } else slightMove(BACKWARD,75); } 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())<110 || rightEncoder.getPulses()<1100); motors.stopBothMotors(127); wait_ms(100); overBump(MID); } void mid_section2(float* location, float ¤t, int* direction) { //pc.printf("mid section 2\r\n"); wait_ms(500); rangeFinderFront.startMeas(); wait_ms(20); rangeFinderFront.getMeas(range); if(range > 20) { 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); direction[1]= STRAIGHT; overBump(RIGS); return; } //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); if(IRFront.getIRDistance() > 37 && IRBack.getIRDistance() > 37) { direction[1]= RIGHT; location[2]= current; slightMove(FORWARD,150); 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(BACKWARD,100); } //LEFT turn motors.begin(); leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(0.5*127);// right motors.setMotor1Speed(-0.5*127);// left while(abs(leftEncoder.getPulses())<1115 || rightEncoder.getPulses()<1115); motors.stopBothMotors(127); 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; // Slight forward for turn slightMove(FORWARD,150); wait_ms(100); leftTurn(); //slightright(); if(current > loc) { //pc.printf("RIG section %f\r\n",current); wall_follow2(LEFT, FORWARD, 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(LEFT, BACKWARD, RIGS, current, rig); current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); } wait(1); alignWithWall(MID2); current+=4; wall_follow2(LEFT, FORWARD, RIGS, current, rig); 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) { if(location[0] > 16) { leftTurn(); wall_follow2(LEFT, BACKWARD, TOOLS, location[0], 0); } motors.stopBothMotors(127); } void mid_section_return(float* location, float ¤t, int* direction) { if(direction[0] == RIGHT) { rightTurn(); alignWithWall(MID); wall_follow2(LEFT, FORWARD, MID, current,0); current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); slightMove(FORWARD, 50); leftTurn(); } else if(direction[0] == LEFT) { rightTurn(); wall_follow2(RIGHT, BACKWARD, MID, current,0); current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); wait_ms(200); slightMove(BACKWARD, 100); leftTurn(); } //ELSE and GO FORWARD overBump(RIGS); } void mid_section2_return(float* location, float ¤t, int* direction) { if(direction[1] == RIGHT) { rightTurn(); wall_follow2(LEFT, FORWARD, RETURN, current,0); current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); leftTurn(); } else if(direction[1] == LEFT) { rightTurn(); wall_follow2(LEFT, BACKWARD, RETURN, current,0); current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); leftTurn(); } //ELSE and GO FORWARD overBump(MID); } void rig_section_return(float* location, float ¤t, int* direction) { alignWithWall(RIGS); if(location[2] > current) { wall_follow2(LEFT, BACKWARD, RETURN, current,0); current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); wait_ms(500); //slightMove(FORWARD,50); } else { wall_follow2(LEFT, FORWARD, RETURN, current,0); current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); } // LEFT TURN 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()<110); motors.stopBothMotors(127); overBump(MID2); } int Xadjust(int tool) { int areaArray[10]; float C, T, S; for(int i = 0; i < 5; 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))); } void testSensors(void){ float range, range2; while(1){ rangeFinderFront.startMeas(); rangeFinderRight.startMeas(); wait_ms(20); rangeFinderFront.getMeas(range); rangeFinderRight.getMeas(range2); pc.printf("us front %f \t us right %f \t IR front %f \t IR back %f\r\n", range, range2, IRFront.getIRDistance(), IRBack.getIRDistance()); //pc.printf("IR front %f \t IR back %f\r\n", IRFront.getIRDistance(), IRBack.getIRDistance()); } }