Yeah
Dependencies: HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of NavigationTest by
main.cpp
- Committer:
- Fairy_Paolina
- Date:
- 2014-03-21
- Revision:
- 6:f5c26372b2d0
- Parent:
- 5:70ccef3734ae
- Child:
- 7:78745a518957
File content as of revision 6:f5c26372b2d0:
#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 FIRST_WAVE (0) #define FAR (1) 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, float location); 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 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 wave); void alignWithWall(int section); //Variables int main(void) { float location[3], current=0; int direction[3]; pc.baud(115200); bt.baud(115200); motors.begin(); //Go to tools tools_section(location, current); mid_section(location, current, direction); mid_section2(location, current, direction); } 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=4; 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< 78) { 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!!!! motors.setMotor1Speed(dir*0.3*127);//left motors.setMotor0Speed(dir*0.3*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=4, loc; pid1.reset(); if(direction == BACKWARD) dir=-1; if(section == TOOLS)set= 5; leftEncoder.reset(); rightEncoder.reset(); while(loc - location ) { location=(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(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); } } motors.stopBothMotors(); } void alignWithWall(int section){ float usValue = 0; if(section == TOOLS){ // turn at an angle leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(-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()) < 350 || abs(rightEncoder.getPulses()) < 350); motors.stopBothMotors(); // turn left towards wall leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(MAX_SPEED); //right motors.setMotor1Speed(-MAX_SPEED); //left while(rightEncoder.getPulses() < 100); motors.stopBothMotors(); } else{ // turn right towards wall leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(-MAX_SPEED); //right motors.setMotor1Speed(MAX_SPEED); //left while(abs(rightEncoder.getPulses()) < 300); motors.stopBothMotors(); } // align using the distance of the wall 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); bt.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) { leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(-0.4*127);//right motors.setMotor1Speed(0.4*127);//left while(leftEncoder.getPulses()<1000 || rightEncoder.getPulses()>-1000); 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())<1075 || rightEncoder.getPulses()<1075); motors.stopBothMotors(); } void overBump(int wave){ int preLeft=5000, preRight=5000 ; leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(0.15*127); //right motors.setMotor1Speed(0.15*127); //left while((abs(leftEncoder.getPulses()) < 700 || abs(rightEncoder.getPulses())< 700) && preLeft!=0){ preLeft=leftEncoder.getPulses(); preRight=rightEncoder.getPulses(); wait_ms(20); if(leftEncoder.getPulses() == preLeft && rightEncoder.getPulses()== preRight) preLeft=preRight=0; } if(wave == FAR){ while(leftEncoder.getPulses() != preLeft && rightEncoder.getPulses()!= preRight){ preLeft=leftEncoder.getPulses(); preRight=rightEncoder.getPulses(); wait_ms(20); } motors.stopBothMotors(); } 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(); } void 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; bt.printf("current %f \r\n",current); motors.stopBothMotors(); //Tool aquiring wait(2); // After tool is aquired alignWithWall(TOOLS); current-=8; wait_ms(100); rangeFinderLeft.startMeas(); wait_ms(20); rangeFinderLeft.getMeas(range); if(range < 20){ wall_follow2(LEFT,BACKWARD,TOOLS, current); location[0]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2); current= location[0]; /* // go backwards leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(-0.3*127); //right motors.setMotor1Speed(-0.3*127); //left while(abs(leftEncoder.getPulses()) < 150 || abs(rightEncoder.getPulses()) < 150); motors.stopBothMotors(); */ leftTurn(); overBump(FAR); } else{ location[0]= 77; leftTurn(); wait_ms(20); overBump(FIRST_WAVE); } bt.printf("wavegap = %f\r\n",location[0]); } void mid_section(float* location, float ¤t, int* direction){ motors.begin(); rightTurn(); alignWithWall(MID); /* 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(); */ bt.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); bt.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); } bt.printf("wavegap2 = %f\r\n",location[1]); leftTurn(); overBump(FAR); // 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(); rightTurn(); 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(FAR); } void rig_section(float* location, float ¤t, int* direction, int rig){ }