Paolina Povolotskaya / NavigationTest

revise

Dependencies:   HCSR04 PID PololuQik2 QEI mbed-rtos Sharp

main.cpp@1:801f0b9a862a, 2014-03-13 (annotated)

Committer:
Fairy_Paolina
Date:
Thu Mar 13 20:45:56 2014 +0000
Revision:
1:801f0b9a862a
Parent:
0:ff94cc47fef7
Child:
2:3d0be48abcf2
revised;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Fairy_Paolina 0:ff94cc47fef7 1 #include "rtos.h"
Fairy_Paolina 0:ff94cc47fef7 2 #include "PID.h"
Fairy_Paolina 0:ff94cc47fef7 3 #include "PololuQik2.h"
Fairy_Paolina 0:ff94cc47fef7 4 #include "QEI.h"
Fairy_Paolina 0:ff94cc47fef7 5 #include "mbed.h"
Fairy_Paolina 0:ff94cc47fef7 6 #include "HCSR04.h"
Fairy_Paolina 0:ff94cc47fef7 7 #include "stdio.h"
Fairy_Paolina 0:ff94cc47fef7 8 #include "LPC17xx.h"
Fairy_Paolina 0:ff94cc47fef7 9
Fairy_Paolina 0:ff94cc47fef7 10 #define PIN_TRIGGER (p12)
Fairy_Paolina 0:ff94cc47fef7 11 #define PIN_ECHO (p11)
Fairy_Paolina 0:ff94cc47fef7 12 #define PULSE_PER_REV (1192)
Fairy_Paolina 0:ff94cc47fef7 13 #define WHEEL_CIRCUM (12.56637)
Fairy_Paolina 0:ff94cc47fef7 14 #define DIST_PER_PULSE (0.01054225722682)
Fairy_Paolina 0:ff94cc47fef7 15 #define MTRS_TO_INCH (39.3701)
Fairy_Paolina 1:801f0b9a862a 16 #define MAX_SPEED (0.3*127)
Fairy_Paolina 0:ff94cc47fef7 17
Fairy_Paolina 0:ff94cc47fef7 18 float range, pid_return;
Fairy_Paolina 0:ff94cc47fef7 19 void errFunction(void);
Fairy_Paolina 0:ff94cc47fef7 20 bool cRc;
Fairy_Paolina 0:ff94cc47fef7 21
Fairy_Paolina 0:ff94cc47fef7 22 //Hardware Initialization
Fairy_Paolina 0:ff94cc47fef7 23 Serial bt(p13,p14);
Fairy_Paolina 0:ff94cc47fef7 24 Serial pc(USBTX,USBRX);
Fairy_Paolina 0:ff94cc47fef7 25 HCSR04 rangeFinder( PIN_TRIGGER, PIN_ECHO );
Fairy_Paolina 1:801f0b9a862a 26 PID pid1(2.0,2.0,0.0,0.02);
Fairy_Paolina 0:ff94cc47fef7 27 PololuQik2 motors(p9, p10, p8, p15, errFunction, cRc);
Fairy_Paolina 1:801f0b9a862a 28 QEI leftEncoder(p17,p18,NC,1192,QEI::X4_ENCODING);
Fairy_Paolina 1:801f0b9a862a 29 //QEI rightEncoder(p19,p20,NC,1192,QEI::X4_ENCODING);
Fairy_Paolina 1:801f0b9a862a 30 //InterruptIn encoder(p29);
Fairy_Paolina 1:801f0b9a862a 31
Fairy_Paolina 0:ff94cc47fef7 32
Fairy_Paolina 0:ff94cc47fef7 33 //Functions
Fairy_Paolina 0:ff94cc47fef7 34
Fairy_Paolina 0:ff94cc47fef7 35 void wall_follow(void);
Fairy_Paolina 1:801f0b9a862a 36 void wall_follow2(int *currentLocation);
Fairy_Paolina 0:ff94cc47fef7 37 void wall_follow3(int &currentLocation, int &WaveOpening);
Fairy_Paolina 1:801f0b9a862a 38 void leftTurn(void);
Fairy_Paolina 1:801f0b9a862a 39 void rightTurn(void);
Fairy_Paolina 0:ff94cc47fef7 40 void us_distance(void);
Fairy_Paolina 0:ff94cc47fef7 41
Fairy_Paolina 1:801f0b9a862a 42 //Variables
Fairy_Paolina 0:ff94cc47fef7 43
Fairy_Paolina 0:ff94cc47fef7 44 int main(void){
Fairy_Paolina 1:801f0b9a862a 45 int location=0;
Fairy_Paolina 0:ff94cc47fef7 46
Fairy_Paolina 0:ff94cc47fef7 47 pc.baud(115200);
Fairy_Paolina 1:801f0b9a862a 48 bt.baud(115200);
Fairy_Paolina 1:801f0b9a862a 49 motors.begin();
Fairy_Paolina 0:ff94cc47fef7 50
Fairy_Paolina 1:801f0b9a862a 51 /*motors.setMotor0Speed(MAX_SPEED); //left
Fairy_Paolina 1:801f0b9a862a 52 motors.setMotor1Speed(MAX_SPEED); //right
Fairy_Paolina 1:801f0b9a862a 53 wait_ms(350);
Fairy_Paolina 1:801f0b9a862a 54 */
Fairy_Paolina 1:801f0b9a862a 55 //wall_follow();
Fairy_Paolina 1:801f0b9a862a 56 //wall_follow2(&location);
Fairy_Paolina 1:801f0b9a862a 57 pc.printf("%d\n\r",location);
Fairy_Paolina 0:ff94cc47fef7 58
Fairy_Paolina 0:ff94cc47fef7 59 motors.stopBothMotors();
Fairy_Paolina 1:801f0b9a862a 60 leftTurn();
Fairy_Paolina 1:801f0b9a862a 61 wait(1);
Fairy_Paolina 1:801f0b9a862a 62 rightTurn();
Fairy_Paolina 0:ff94cc47fef7 63
Fairy_Paolina 0:ff94cc47fef7 64 }
Fairy_Paolina 0:ff94cc47fef7 65
Fairy_Paolina 0:ff94cc47fef7 66 void errFunction(void){
Fairy_Paolina 0:ff94cc47fef7 67 //Nothing
Fairy_Paolina 0:ff94cc47fef7 68 }
Fairy_Paolina 0:ff94cc47fef7 69
Fairy_Paolina 0:ff94cc47fef7 70 void us_distance(void)
Fairy_Paolina 0:ff94cc47fef7 71 {
Fairy_Paolina 0:ff94cc47fef7 72 pc.printf("Ultra Sonic\n\r");
Fairy_Paolina 0:ff94cc47fef7 73 rangeFinder.startMeas();
Fairy_Paolina 0:ff94cc47fef7 74 wait_us(20);
Fairy_Paolina 0:ff94cc47fef7 75 if ( (rangeFinder.getMeas(range) == RANGE_MEAS_VALID))
Fairy_Paolina 0:ff94cc47fef7 76 {
Fairy_Paolina 0:ff94cc47fef7 77 pc.printf("Range = %f\n\r", range);
Fairy_Paolina 0:ff94cc47fef7 78 }
Fairy_Paolina 0:ff94cc47fef7 79 }
Fairy_Paolina 0:ff94cc47fef7 80
Fairy_Paolina 0:ff94cc47fef7 81 void wall_follow(void)
Fairy_Paolina 0:ff94cc47fef7 82 {
Fairy_Paolina 0:ff94cc47fef7 83 while(1){
Fairy_Paolina 0:ff94cc47fef7 84
Fairy_Paolina 1:801f0b9a862a 85 pid1.setInputLimits(5.75, 6);
Fairy_Paolina 1:801f0b9a862a 86 pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
Fairy_Paolina 1:801f0b9a862a 87 pid1.setSetPoint(6.0);
Fairy_Paolina 0:ff94cc47fef7 88
Fairy_Paolina 0:ff94cc47fef7 89 rangeFinder.startMeas();
Fairy_Paolina 1:801f0b9a862a 90 wait_ms(20);
Fairy_Paolina 0:ff94cc47fef7 91 if ( (rangeFinder.getMeas(range) == RANGE_MEAS_VALID) && (range < 100.0) && (range > 3.0))
Fairy_Paolina 0:ff94cc47fef7 92 {
Fairy_Paolina 1:801f0b9a862a 93 pc.printf("Range = %f\n\r", range);
Fairy_Paolina 0:ff94cc47fef7 94 }
Fairy_Paolina 0:ff94cc47fef7 95 pid1.setProcessValue(range);
Fairy_Paolina 0:ff94cc47fef7 96 pid_return = pid1.compute();
Fairy_Paolina 1:801f0b9a862a 97 pc.printf("Range: %f\n PID: %f\r\n", range, pid_return);
Fairy_Paolina 0:ff94cc47fef7 98
Fairy_Paolina 0:ff94cc47fef7 99 if(pid_return > 0){
Fairy_Paolina 0:ff94cc47fef7 100 motors.setMotor0Speed(MAX_SPEED - pid_return);//left
Fairy_Paolina 0:ff94cc47fef7 101 motors.setMotor1Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 102 }else if(pid_return < 0){
Fairy_Paolina 0:ff94cc47fef7 103 motors.setMotor0Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 104 motors.setMotor1Speed(MAX_SPEED + pid_return);
Fairy_Paolina 0:ff94cc47fef7 105 }else{
Fairy_Paolina 0:ff94cc47fef7 106 motors.setMotor0Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 107 motors.setMotor1Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 108 }
Fairy_Paolina 0:ff94cc47fef7 109 }
Fairy_Paolina 0:ff94cc47fef7 110 }
Fairy_Paolina 0:ff94cc47fef7 111
Fairy_Paolina 0:ff94cc47fef7 112 /* MODIFIED WALL_FOLLOW FOR NAVIGATION */
Fairy_Paolina 0:ff94cc47fef7 113
Fairy_Paolina 1:801f0b9a862a 114 void wall_follow2(int *currentLocation)
Fairy_Paolina 0:ff94cc47fef7 115 {
Fairy_Paolina 0:ff94cc47fef7 116 int SeeWaveGap = false;
Fairy_Paolina 1:801f0b9a862a 117 int count=0;
Fairy_Paolina 1:801f0b9a862a 118
Fairy_Paolina 0:ff94cc47fef7 119 while(1){
Fairy_Paolina 0:ff94cc47fef7 120
Fairy_Paolina 1:801f0b9a862a 121 pid1.setInputLimits(0.0, 6.0);
Fairy_Paolina 1:801f0b9a862a 122 pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
Fairy_Paolina 1:801f0b9a862a 123 pid1.setSetPoint(6.0);
Fairy_Paolina 0:ff94cc47fef7 124
Fairy_Paolina 0:ff94cc47fef7 125 rangeFinder.startMeas();
Fairy_Paolina 1:801f0b9a862a 126 wait_ms(20);
Fairy_Paolina 0:ff94cc47fef7 127 if ( (rangeFinder.getMeas(range) == RANGE_MEAS_VALID) && (range < 100.0) && (range > 3.0))
Fairy_Paolina 0:ff94cc47fef7 128 {
Fairy_Paolina 1:801f0b9a862a 129 bt.printf("Range = %f\n", range);
Fairy_Paolina 0:ff94cc47fef7 130 }
Fairy_Paolina 0:ff94cc47fef7 131
Fairy_Paolina 0:ff94cc47fef7 132 /*************CHECK FOR WAVE OPENING*****************/
Fairy_Paolina 1:801f0b9a862a 133 /* If after 60 ms the ultrasonic still sees 20+ cm */
Fairy_Paolina 0:ff94cc47fef7 134 /* then robot is at wave opening */
Fairy_Paolina 0:ff94cc47fef7 135
Fairy_Paolina 1:801f0b9a862a 136 pc.printf("range %f\r\n",range);
Fairy_Paolina 1:801f0b9a862a 137 if(range > 20){
Fairy_Paolina 0:ff94cc47fef7 138 currentLocation++;
Fairy_Paolina 1:801f0b9a862a 139 bt.printf("saw gap \r\n");
Fairy_Paolina 1:801f0b9a862a 140
Fairy_Paolina 1:801f0b9a862a 141 if(SeeWaveGap){
Fairy_Paolina 1:801f0b9a862a 142 motors.stopBothMotors();
Fairy_Paolina 1:801f0b9a862a 143 bt.printf("wavegap\r\n");
Fairy_Paolina 1:801f0b9a862a 144 // AT WAVE OPENING!!!!
Fairy_Paolina 1:801f0b9a862a 145
Fairy_Paolina 1:801f0b9a862a 146 break;
Fairy_Paolina 1:801f0b9a862a 147 }
Fairy_Paolina 0:ff94cc47fef7 148 SeeWaveGap = true;
Fairy_Paolina 0:ff94cc47fef7 149 }
Fairy_Paolina 1:801f0b9a862a 150
Fairy_Paolina 0:ff94cc47fef7 151
Fairy_Paolina 0:ff94cc47fef7 152
Fairy_Paolina 0:ff94cc47fef7 153 pid1.setProcessValue(range);
Fairy_Paolina 0:ff94cc47fef7 154 pid_return = pid1.compute();
Fairy_Paolina 1:801f0b9a862a 155 bt.printf("Range: %f\n PID: %f\r\n", range, pid_return);
Fairy_Paolina 0:ff94cc47fef7 156
Fairy_Paolina 0:ff94cc47fef7 157 if(pid_return > 0){
Fairy_Paolina 0:ff94cc47fef7 158 motors.setMotor0Speed(MAX_SPEED - pid_return);
Fairy_Paolina 0:ff94cc47fef7 159 motors.setMotor1Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 160 }else if(pid_return < 0){
Fairy_Paolina 0:ff94cc47fef7 161 motors.setMotor0Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 162 motors.setMotor1Speed(MAX_SPEED + pid_return);
Fairy_Paolina 0:ff94cc47fef7 163 }else{
Fairy_Paolina 0:ff94cc47fef7 164 motors.setMotor0Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 165 motors.setMotor1Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 166 }
Fairy_Paolina 0:ff94cc47fef7 167 }
Fairy_Paolina 0:ff94cc47fef7 168 }
Fairy_Paolina 0:ff94cc47fef7 169
Fairy_Paolina 0:ff94cc47fef7 170
Fairy_Paolina 0:ff94cc47fef7 171 /* MODIFIED WALL_FOLLOW FOR NAVIGATION WITH WAVE OPENINGS PASSED IN */
Fairy_Paolina 0:ff94cc47fef7 172 /* MEANT FOR RETURNING FROM OIL RIGS */
Fairy_Paolina 0:ff94cc47fef7 173
Fairy_Paolina 0:ff94cc47fef7 174 void wall_follow3(int &currentLocation, int &WaveOpening)
Fairy_Paolina 0:ff94cc47fef7 175 {
Fairy_Paolina 0:ff94cc47fef7 176 while(1){
Fairy_Paolina 0:ff94cc47fef7 177
Fairy_Paolina 0:ff94cc47fef7 178
Fairy_Paolina 1:801f0b9a862a 179 pid1.setInputLimits(5.75, 6);
Fairy_Paolina 1:801f0b9a862a 180 pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
Fairy_Paolina 1:801f0b9a862a 181 pid1.setSetPoint(6.0);
Fairy_Paolina 0:ff94cc47fef7 182
Fairy_Paolina 0:ff94cc47fef7 183 rangeFinder.startMeas();
Fairy_Paolina 0:ff94cc47fef7 184 wait_ms(100);
Fairy_Paolina 0:ff94cc47fef7 185 if ( (rangeFinder.getMeas(range) == RANGE_MEAS_VALID) && (range < 100.0) && (range > 3.0))
Fairy_Paolina 0:ff94cc47fef7 186 {
Fairy_Paolina 0:ff94cc47fef7 187 //bt.printf("Range = %f\n", range);
Fairy_Paolina 0:ff94cc47fef7 188 }
Fairy_Paolina 0:ff94cc47fef7 189
Fairy_Paolina 0:ff94cc47fef7 190 /*************CHECK FOR WAVE OPENING*****************/
Fairy_Paolina 0:ff94cc47fef7 191 /* If after 100 ms the ultrasonic still sees 20+ cm */
Fairy_Paolina 0:ff94cc47fef7 192 /* then robot is at wave opening */
Fairy_Paolina 0:ff94cc47fef7 193
Fairy_Paolina 0:ff94cc47fef7 194
Fairy_Paolina 0:ff94cc47fef7 195 if(range > 20 ){
Fairy_Paolina 0:ff94cc47fef7 196 currentLocation--;
Fairy_Paolina 0:ff94cc47fef7 197 }
Fairy_Paolina 0:ff94cc47fef7 198
Fairy_Paolina 0:ff94cc47fef7 199 if( currentLocation == WaveOpening){
Fairy_Paolina 0:ff94cc47fef7 200 // AT WAVE OPENING!!!!
Fairy_Paolina 0:ff94cc47fef7 201
Fairy_Paolina 0:ff94cc47fef7 202 break;
Fairy_Paolina 0:ff94cc47fef7 203 }
Fairy_Paolina 0:ff94cc47fef7 204
Fairy_Paolina 0:ff94cc47fef7 205
Fairy_Paolina 0:ff94cc47fef7 206 pid1.setProcessValue(range);
Fairy_Paolina 0:ff94cc47fef7 207 pid_return = pid1.compute();
Fairy_Paolina 0:ff94cc47fef7 208 bt.printf("Range: %f\n PID: %f", range, pid_return);
Fairy_Paolina 0:ff94cc47fef7 209
Fairy_Paolina 0:ff94cc47fef7 210 if(pid_return > 0){
Fairy_Paolina 0:ff94cc47fef7 211 motors.setMotor0Speed(MAX_SPEED - pid_return);
Fairy_Paolina 0:ff94cc47fef7 212 motors.setMotor1Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 213 }else if(pid_return < 0){
Fairy_Paolina 0:ff94cc47fef7 214 motors.setMotor0Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 215 motors.setMotor1Speed(MAX_SPEED + pid_return);
Fairy_Paolina 0:ff94cc47fef7 216 }else{
Fairy_Paolina 0:ff94cc47fef7 217 motors.setMotor0Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 218 motors.setMotor1Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 219 }
Fairy_Paolina 0:ff94cc47fef7 220 }
Fairy_Paolina 0:ff94cc47fef7 221 }
Fairy_Paolina 1:801f0b9a862a 222
Fairy_Paolina 1:801f0b9a862a 223 void rightTurn(void)
Fairy_Paolina 1:801f0b9a862a 224 {
Fairy_Paolina 1:801f0b9a862a 225 leftEncoder.reset();
Fairy_Paolina 1:801f0b9a862a 226 //rightEncoder.reset();
Fairy_Paolina 1:801f0b9a862a 227 motors.setMotor0Speed(-0.4*127);
Fairy_Paolina 1:801f0b9a862a 228 motors.setMotor1Speed(0.4*127);
Fairy_Paolina 1:801f0b9a862a 229 while(leftEncoder.getPulses()<1400);
Fairy_Paolina 1:801f0b9a862a 230 motors.stopBothMotors();
Fairy_Paolina 1:801f0b9a862a 231 }
Fairy_Paolina 1:801f0b9a862a 232
Fairy_Paolina 1:801f0b9a862a 233 void leftTurn(void)
Fairy_Paolina 1:801f0b9a862a 234 {
Fairy_Paolina 1:801f0b9a862a 235 leftEncoder.reset();
Fairy_Paolina 1:801f0b9a862a 236 //rightEncoder.reset();
Fairy_Paolina 1:801f0b9a862a 237 motors.setMotor0Speed(0.4*127);
Fairy_Paolina 1:801f0b9a862a 238 motors.setMotor1Speed(-0.4*127);
Fairy_Paolina 1:801f0b9a862a 239 while(leftEncoder.getPulses()>-1500);
Fairy_Paolina 1:801f0b9a862a 240 motors.stopBothMotors();
Fairy_Paolina 1:801f0b9a862a 241 }