Yeah
Dependencies: HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of NavigationTest by
main.cpp
- Committer:
- Fairy_Paolina
- Date:
- 2014-03-20
- Revision:
- 5:70ccef3734ae
- Parent:
- 4:f2333e66ec2c
- Child:
- 6:f5c26372b2d0
File content as of revision 5:70ccef3734ae:
#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); void align(float speed); //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){ rightTurn(); current=wall_follow(LEFT,FORWARD,MID); if(current != 0) direction[0]= RIGHT; else{ direction[0]= LEFT; wall_follow2(LEFT,BACKWARD,MID); location[1]=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(-MAX_SPEED); //right motors.setMotor1Speed(-MAX_SPEED); //left while(abs(leftEncoder.getPulses())<75 || abs(rightEncoder.getPulses())<75); motors.stopBothMotors(); } leftTurn(); } else{ leftTurn(); direction[0]= LEFT; wall_follow2(RIGHT,FORWARD,MID); location[1]=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; rightTurn(); } overBump(); */ // left or right //leftTurn(); //wall_follow2(RIGHT); // rightTurn(); bt.printf("LOCATION %f\n\r",location); //overBump(); //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=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< 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, dir=1; float set=5; pid1.reset(); if(direction == BACKWARD) dir=-1; if(section == TOOLS)set= 6.5; leftEncoder.reset(); rightEncoder.reset(); while(1) { pid1.setInputLimits(0.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); } /*************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 align(float speed){ float left, right; left=right=speed; if(abs(leftEncoder.getPulses())> rightEncoder.getPulses()){ if(speed>0)left-=.01; else left+=.01; } else{ if(speed>0) right+=.01; else right-=.01; } motors.setMotor0Speed(right*127);//right motors.setMotor1Speed(left*127);//left pc.printf("speed left %f right %f\r\n\n",left, right); } void rightTurn(void) { leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(-0.4*127);//right motors.setMotor1Speed(0.4*127);//left while(leftEncoder.getPulses()<1030 || rightEncoder.getPulses()>-1030); motors.stopBothMotors(); } void leftTurn(void) { leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(0.4*127);// right motors.setMotor1Speed(-0.4*127);// left while(abs(leftEncoder.getPulses())<1120 || rightEncoder.getPulses()<1120); //align(0.4); motors.stopBothMotors(); } void slightRight(void) { leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(-0.4*127);//right motors.setMotor1Speed(0.4*127);//left while(leftEncoder.getPulses()<515 || rightEncoder.getPulses()>-515); motors.stopBothMotors(); } void slightLeft(void) { leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(0.4*127);// right motors.setMotor1Speed(-0.4*127);// left while(abs(leftEncoder.getPulses())<400 || rightEncoder.getPulses()< 400); //align(0.4); motors.stopBothMotors(); } void overBump(void){ 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 <9)//align(MAX_SPEED); 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 <6); 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(); slightRight(); //backward leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(-MAX_SPEED);//right motors.setMotor1Speed(-MAX_SPEED);//left while(abs(leftEncoder.getPulses())<500 || abs(rightEncoder.getPulses())<500); motors.stopBothMotors(); slightLeft(); /*leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(MAX_SPEED);//right motors.setMotor1Speed(MAX_SPEED);//left while(abs(leftEncoder.getPulses())<100 || abs(rightEncoder.getPulses())<100);*/ ////////////////////////////////////////// determine shape and pick up tool /////////////////////////////////////////////////////// wall_follow2(LEFT,BACKWARD,TOOLS); /* if(current >location[0]){ wall_follow2(LEFT,BACKWARD,TOOLS); wait_ms(1000); // back leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(-MAX_SPEED); //right motors.setMotor1Speed(-MAX_SPEED); //left while(abs(leftEncoder.getPulses())<75 || abs(rightEncoder.getPulses())<75); motors.stopBothMotors(); current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; } else{ wall_follow2(LEFT,FORWARD,TOOLS); // backward wait_ms(1000); leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(-MAX_SPEED); //right motors.setMotor1Speed(-MAX_SPEED); //left while(abs(leftEncoder.getPulses())<150 || abs(rightEncoder.getPulses())<150); motors.stopBothMotors(); current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; }*/ wait_ms(1000); leftTurn(); //Go over overBump(); }