Yeah
Dependencies: HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of NavigationTest by
main.cpp
- Committer:
- Fairy_Paolina
- Date:
- 2014-03-15
- Revision:
- 2:3d0be48abcf2
- Parent:
- 1:801f0b9a862a
- Child:
- 3:58726d2e11f0
File content as of revision 2:3d0be48abcf2:
#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 PPRL (965) #define PPRR (1075) #define LEFT (1) #define RIGHT (0) 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,PPRR,QEI::X4_ENCODING); QEI leftEncoder(p16,p15,NC,PPRR,QEI::X4_ENCODING); //InterruptIn encoder(p29); //Functions void wall_follow(int side); void wall_follow2(int side); void wall_follow3(int ¤tLocation, int &WaveOpening); void leftTurn(void); void rightTurn(void); void us_distance(void); //Variables int main(void) { float location=0; pc.baud(115200); bt.baud(115200); motors.begin(); //motors.setMotor0Speed(MAX_SPEED); //right //motors.setMotor1Speed(MAX_SPEED); //left //motors.stopBothMotors(); //wall_follow(RIGHT); leftEncoder.reset(); rightEncoder.reset(); wall_follow2(LEFT); location=(abs(leftEncoder.getPulses()*11.12/PPRL) + abs(rightEncoder.getPulses()*11.12/PPRR))/2; leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(MAX_SPEED); //right motors.setMotor1Speed(MAX_SPEED); //left while(leftEncoder.getPulses()<300 || rightEncoder.getPulses()<300); motors.stopBothMotors(); leftTurn(); leftEncoder.reset(); rightEncoder.reset(); motors.setMotor0Speed(MAX_SPEED); //right motors.setMotor1Speed(MAX_SPEED); //left while(abs(leftEncoder.getPulses()*11.12/PPRL) + abs(rightEncoder.getPulses()*11.12/PPRR)/2 <30); motors.stopBothMotors(); 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); } } void wall_follow(int side) { while(1) { pid1.setInputLimits(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); pc.printf("%d\r\n",range); } pid1.setProcessValue(range); pid_return = pid1.compute(); if(pid_return > 0) { if(side){ motors.setMotor0Speed(MAX_SPEED - pid_return);//right motors.setMotor1Speed(MAX_SPEED);//left } else{ motors.setMotor1Speed(MAX_SPEED - pid_return);//left motors.setMotor0Speed(MAX_SPEED);//right } }else if(pid_return < 0) { if(side){ motors.setMotor0Speed(MAX_SPEED);//right motors.setMotor1Speed(MAX_SPEED + pid_return);//left } else{ motors.setMotor1Speed(MAX_SPEED);//left motors.setMotor0Speed(MAX_SPEED + pid_return);//right } }else { motors.setMotor0Speed(MAX_SPEED);//right motors.setMotor1Speed(MAX_SPEED);//left } } } /* MODIFIED WALL_FOLLOW FOR NAVIGATION */ void wall_follow2(int side) { int SeeWaveGap = false; int count=0; 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 60 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(MAX_SPEED - pid_return);//right motors.setMotor1Speed(MAX_SPEED);//left } else{ motors.setMotor1Speed(MAX_SPEED - pid_return);//left motors.setMotor0Speed(MAX_SPEED);//right } }else if(pid_return < 0) { if(side){ motors.setMotor0Speed(MAX_SPEED);//right motors.setMotor1Speed(MAX_SPEED + pid_return);//left } else{ motors.setMotor1Speed(MAX_SPEED);//left motors.setMotor0Speed(MAX_SPEED + pid_return);//right } } else { motors.setMotor0Speed(MAX_SPEED); motors.setMotor1Speed(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(); }