Yeah
Dependencies: HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of NavigationTest by
main.cpp
- Committer:
- Fairy_Paolina
- Date:
- 2014-03-18
- Revision:
- 4:f2333e66ec2c
- Parent:
- 3:58726d2e11f0
- Child:
- 5:70ccef3734ae
File content as of revision 4:f2333e66ec2c:
#include "rtos.h" #include "PID.h" #include "PololuQik2.h" #include "QEI.h" #include "mbed.h" #include "HCSR04.h" #include "stdio.h" #include "LPC17xx.h" #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) float range, pid_return; void errFunction(void); bool cRc; //Hardware Initialization Serial bt(p13,p14); Serial pc(USBTX,USBRX); 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); //InterruptIn encoder(p29); //Functions float wall_follow(int side, int direction, int section); void wall_follow2(int side, int direction, int section); void wall_follow3(int ¤tLocation, int &WaveOpening); void leftTurn(void); void rightTurn(void); void us_distance(void); void tools_section(float* location, float ¤t); void overBump(void); //Variables int main(void) { float location[3], current=0; int direction[3]; pc.baud(115200); bt.baud(115200); motors.begin(); //leftEncoder.reset(); //rightEncoder.reset(); //motors.setMotor0Speed(MAX_SPEED); //right //motors.setMotor1Speed(MAX_SPEED); //left //while((abs(leftEncoder.getPulses())/(PPR) + abs(rightEncoder.getPulses())/(PPR))/2 < 3) //Go to tools tools_section(location, current); /* //////////////////////////////// without predefined wavegaps////////////////////////////////////////////// current=0; if(location[0]< 75){ turnRight(); current=wall_follow(LEFT,FORWARD); if(current == 0)turnLeft(); else{ direction[0]= RIGHT; turnLeft(); overBump(); } } else if(location[0]>=75 || current == 0){ turnLeft(); wall_follow2(RIGHT,FORWARD); } // left or right location[1]=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; leftTurn(); //wall_follow2(RIGHT); rightTurn(); bt.printf("LOCATION %f\n\r",location); motors.stopBothMotors(); // leftTurn(); // wait(1); // rightTurn(); */ } 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; int dir=1, set=5; pid1.reset(); if(direction == BACKWARD) dir=-1; if(section == TOOLS)set= 9; leftEncoder.reset(); rightEncoder.reset(); location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; while(location< 75) { 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(20); rangeFinderLeft.getMeas(range); } else{ rangeFinderRight.startMeas(); wait_ms(20); 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; bt.printf("wavegap %f\r\n",wavegap); // AT WAVE OPENING!!!! } 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) { int SeeWaveGap = false, count=0, dir=1, set=5; pid1.reset(); if(direction == BACKWARD) dir=-1; if(section == TOOLS)set= 9; leftEncoder.reset(); rightEncoder.reset(); while(1) { pid1.setInputLimits(0.0, 5.0); pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED); pid1.setSetPoint(5.0); if(side){ rangeFinderLeft.startMeas(); wait_ms(20); rangeFinderLeft.getMeas(range); } else{ rangeFinderRight.startMeas(); wait_ms(20); 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("range %f\r\n",range); if(range > 20) { motors.stopBothMotors(); bt.printf("wavegap\r\n"); // AT WAVE OPENING!!!! break; } pid1.setProcessValue(range); pid_return = pid1.compute(); //bt.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); } } } /* MODIFIED WALL_FOLLOW FOR NAVIGATION WITH WAVE OPENINGS PASSED IN */ /* MEANT FOR RETURNING FROM OIL RIGS */ void wall_follow3(int ¤tLocation, int &WaveOpening) { while(1) { pid1.setInputLimits(0, 5); pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED); pid1.setSetPoint(5.0); rangeFinderLeft.startMeas(); wait_ms(100); if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID) && (range < 100.0) && (range > 3.0)) { //bt.printf("Range = %f\n", range); } /*************CHECK FOR WAVE OPENING*****************/ /* If after 100 ms the ultrasonic still sees 20+ cm */ /* then robot is at wave opening */ if(range > 20 ) { currentLocation--; } if( currentLocation == WaveOpening) { // AT WAVE OPENING!!!! break; } pid1.setProcessValue(range); pid_return = pid1.compute(); bt.printf("Range: %f\n PID: %f", range, pid_return); if(pid_return > 0) { motors.setMotor0Speed(MAX_SPEED - pid_return); motors.setMotor1Speed(MAX_SPEED); } else if(pid_return < 0) { motors.setMotor0Speed(MAX_SPEED); motors.setMotor1Speed(MAX_SPEED + pid_return); } else { motors.setMotor0Speed(MAX_SPEED); motors.setMotor1Speed(MAX_SPEED); } } } void rightTurn(void) { float speedL, speedR; speedL=speedR= 0.4; leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(-speedR*127);//right motors.setMotor1Speed(speedL*127);//left while(leftEncoder.getPulses()<1050 || rightEncoder.getPulses()>-1050); motors.stopBothMotors(); } void leftTurn(void) { leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(0.4*127);// right motors.setMotor1Speed(-0.4*127);// left while(leftEncoder.getPulses()>-1200 || rightEncoder.getPulses()<1200); motors.stopBothMotors(); } void overBump(void){ leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(127*.15); //right motors.setMotor1Speed(127*.15); //left while(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR)/2 <9); leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(MAX_SPEED); //right motors.setMotor1Speed(MAX_SPEED); //left while(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR)/2 <3); motors.stopBothMotors(); } void tools_section(float* location, float ¤t){ location[0]=wall_follow(LEFT,FORWARD, TOOLS); //location from the left edge of the field current=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; bt.printf("wavegap %f \t current %f \r\n",location[0],current); motors.stopBothMotors(); ////////////////////////////////////////// determine shape and pick up tool /////////////////////////////////////////////////////// if(current >location[0]){ wall_follow2(LEFT,BACKWARD,TOOLS); wait_ms(500); // forward /* 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(); wait_ms(500);*/ current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; } else{/* wall_follow2(LEFT,FORWARD); // backward 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(); current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;*/ } leftTurn(); //Go over overBump(); }