Justin Carr / NavigationTest

Tools Section Navigation Calibrated slightly

Dependencies:   HCSR04 PID PololuQik2 QEI mbed-rtos

Fork of NavigationTest by Paolina Povolotskaya

main.cpp@2:0351277ee5dd, 2014-03-19 (annotated)

Committer:
jjcarr2
Date:
Wed Mar 19 22:10:43 2014 +0000
Revision:
2:0351277ee5dd
Parent:
1:801f0b9a862a
Child:
6:109f46d3cb96
Tools Section closer to being done

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"
jjcarr2 2:0351277ee5dd 9
jjcarr2 2:0351277ee5dd 10 #define PIN_TRIGGERL (p12)
jjcarr2 2:0351277ee5dd 11 #define PIN_ECHOL (p11)
jjcarr2 2:0351277ee5dd 12 #define PIN_TRIGGERR (p29)
jjcarr2 2:0351277ee5dd 13 #define PIN_ECHOR (p30)
Fairy_Paolina 0:ff94cc47fef7 14 #define PULSE_PER_REV (1192)
Fairy_Paolina 0:ff94cc47fef7 15 #define WHEEL_CIRCUM (12.56637)
Fairy_Paolina 0:ff94cc47fef7 16 #define DIST_PER_PULSE (0.01054225722682)
Fairy_Paolina 0:ff94cc47fef7 17 #define MTRS_TO_INCH (39.3701)
Fairy_Paolina 1:801f0b9a862a 18 #define MAX_SPEED (0.3*127)
jjcarr2 2:0351277ee5dd 19 #define PPR (4331/4)
jjcarr2 2:0351277ee5dd 20 #define LEFT (1)
jjcarr2 2:0351277ee5dd 21 #define RIGHT (0)
jjcarr2 2:0351277ee5dd 22 #define FORWARD (1)
jjcarr2 2:0351277ee5dd 23 #define BACKWARD (0)
jjcarr2 2:0351277ee5dd 24 #define TOOLS (0)
jjcarr2 2:0351277ee5dd 25 #define MID (1)
jjcarr2 2:0351277ee5dd 26 #define RIGS (2)
jjcarr2 2:0351277ee5dd 27
Fairy_Paolina 0:ff94cc47fef7 28 float range, pid_return;
Fairy_Paolina 0:ff94cc47fef7 29 void errFunction(void);
Fairy_Paolina 0:ff94cc47fef7 30 bool cRc;
jjcarr2 2:0351277ee5dd 31
Fairy_Paolina 0:ff94cc47fef7 32 //Hardware Initialization
Fairy_Paolina 0:ff94cc47fef7 33 Serial bt(p13,p14);
Fairy_Paolina 0:ff94cc47fef7 34 Serial pc(USBTX,USBRX);
jjcarr2 2:0351277ee5dd 35 HCSR04 rangeFinderLeft( PIN_TRIGGERL, PIN_ECHOL );
jjcarr2 2:0351277ee5dd 36 HCSR04 rangeFinderRight( PIN_TRIGGERR, PIN_ECHOR );
jjcarr2 2:0351277ee5dd 37 PID pid1(15.0,0.0,4.0,0.02);
Fairy_Paolina 0:ff94cc47fef7 38 PololuQik2 motors(p9, p10, p8, p15, errFunction, cRc);
jjcarr2 2:0351277ee5dd 39 QEI rightEncoder(p17,p18,NC,PPR,QEI::X4_ENCODING);
jjcarr2 2:0351277ee5dd 40 QEI leftEncoder(p16,p15,NC,PPR,QEI::X4_ENCODING);
Fairy_Paolina 1:801f0b9a862a 41 //InterruptIn encoder(p29);
jjcarr2 2:0351277ee5dd 42
jjcarr2 2:0351277ee5dd 43
Fairy_Paolina 0:ff94cc47fef7 44 //Functions
jjcarr2 2:0351277ee5dd 45
jjcarr2 2:0351277ee5dd 46 float wall_follow(int side, int direction, int section);
jjcarr2 2:0351277ee5dd 47 void wall_follow2(int side, int direction, int section);
Fairy_Paolina 0:ff94cc47fef7 48 void wall_follow3(int &currentLocation, int &WaveOpening);
Fairy_Paolina 1:801f0b9a862a 49 void leftTurn(void);
Fairy_Paolina 1:801f0b9a862a 50 void rightTurn(void);
Fairy_Paolina 0:ff94cc47fef7 51 void us_distance(void);
jjcarr2 2:0351277ee5dd 52 void tools_section(float *location, float &current);
jjcarr2 2:0351277ee5dd 53 void overBump(void);
jjcarr2 2:0351277ee5dd 54
Fairy_Paolina 1:801f0b9a862a 55 //Variables
jjcarr2 2:0351277ee5dd 56
jjcarr2 2:0351277ee5dd 57 int main(void)
jjcarr2 2:0351277ee5dd 58 {
jjcarr2 2:0351277ee5dd 59 float location[3], current=0;
jjcarr2 2:0351277ee5dd 60 int direction[3];
jjcarr2 2:0351277ee5dd 61
Fairy_Paolina 0:ff94cc47fef7 62 pc.baud(115200);
Fairy_Paolina 1:801f0b9a862a 63 bt.baud(115200);
jjcarr2 2:0351277ee5dd 64 motors.begin();
jjcarr2 2:0351277ee5dd 65 /*
jjcarr2 2:0351277ee5dd 66 // Very Consistent Turn
jjcarr2 2:0351277ee5dd 67 leftEncoder.reset();
jjcarr2 2:0351277ee5dd 68 rightEncoder.reset();
jjcarr2 2:0351277ee5dd 69
jjcarr2 2:0351277ee5dd 70 motors.setMotor0Speed(-MAX_SPEED); //right
jjcarr2 2:0351277ee5dd 71 motors.setMotor1Speed(MAX_SPEED); //left
jjcarr2 2:0351277ee5dd 72 float usValue = 0;
jjcarr2 2:0351277ee5dd 73 while(1){
jjcarr2 2:0351277ee5dd 74 rangeFinderLeft.startMeas();
jjcarr2 2:0351277ee5dd 75 wait_ms(100);
jjcarr2 2:0351277ee5dd 76 rangeFinderLeft.getMeas(range);
jjcarr2 2:0351277ee5dd 77 bt.printf("Range %f \t OldValue %f\n\r",range, usValue);
jjcarr2 2:0351277ee5dd 78 if(range > usValue && usValue != 0 && range < 10){
jjcarr2 2:0351277ee5dd 79 break;
jjcarr2 2:0351277ee5dd 80 } else {
jjcarr2 2:0351277ee5dd 81 usValue = range;
jjcarr2 2:0351277ee5dd 82 }
jjcarr2 2:0351277ee5dd 83 }
jjcarr2 2:0351277ee5dd 84 */
jjcarr2 2:0351277ee5dd 85 motors.stopBothMotors();
jjcarr2 2:0351277ee5dd 86 /*
jjcarr2 2:0351277ee5dd 87
jjcarr2 2:0351277ee5dd 88 leftEncoder.reset();
jjcarr2 2:0351277ee5dd 89 rightEncoder.reset();
jjcarr2 2:0351277ee5dd 90 motors.setMotor0Speed(MAX_SPEED); //right
jjcarr2 2:0351277ee5dd 91 motors.setMotor1Speed(MAX_SPEED); //left
jjcarr2 2:0351277ee5dd 92
jjcarr2 2:0351277ee5dd 93 while(leftEncoder.getPulses()/(PPR) < 3);
Fairy_Paolina 1:801f0b9a862a 94 */
jjcarr2 2:0351277ee5dd 95
jjcarr2 2:0351277ee5dd 96 //leftEncoder.reset();
jjcarr2 2:0351277ee5dd 97 //rightEncoder.reset();
jjcarr2 2:0351277ee5dd 98 //motors.setMotor0Speed(MAX_SPEED); //right
jjcarr2 2:0351277ee5dd 99 //motors.setMotor1Speed(MAX_SPEED); //left
jjcarr2 2:0351277ee5dd 100
jjcarr2 2:0351277ee5dd 101 //while((abs(leftEncoder.getPulses())/(PPR) + abs(rightEncoder.getPulses())/(PPR))/2 < 3)
jjcarr2 2:0351277ee5dd 102
jjcarr2 2:0351277ee5dd 103
jjcarr2 2:0351277ee5dd 104
jjcarr2 2:0351277ee5dd 105 //Go to tools
jjcarr2 2:0351277ee5dd 106 tools_section(location, current);
jjcarr2 2:0351277ee5dd 107 bt.printf("Location 0 = %f", location[0]);
Fairy_Paolina 0:ff94cc47fef7 108
jjcarr2 2:0351277ee5dd 109 /*
jjcarr2 2:0351277ee5dd 110 //////////////////////////////// without predefined wavegaps//////////////////////////////////////////////
jjcarr2 2:0351277ee5dd 111 current=0;
jjcarr2 2:0351277ee5dd 112 if(location[0]< 75){
jjcarr2 2:0351277ee5dd 113 turnRight();
jjcarr2 2:0351277ee5dd 114 current=wall_follow(LEFT,FORWARD);
jjcarr2 2:0351277ee5dd 115 if(current == 0)turnLeft();
jjcarr2 2:0351277ee5dd 116 else{
jjcarr2 2:0351277ee5dd 117 direction[0]= RIGHT;
jjcarr2 2:0351277ee5dd 118 turnLeft();
jjcarr2 2:0351277ee5dd 119 overBump();
jjcarr2 2:0351277ee5dd 120 }
jjcarr2 2:0351277ee5dd 121 }
jjcarr2 2:0351277ee5dd 122 else if(location[0]>=75 || current == 0){
jjcarr2 2:0351277ee5dd 123 turnLeft();
jjcarr2 2:0351277ee5dd 124 wall_follow2(RIGHT,FORWARD);
jjcarr2 2:0351277ee5dd 125 }
jjcarr2 2:0351277ee5dd 126
jjcarr2 2:0351277ee5dd 127
jjcarr2 2:0351277ee5dd 128
jjcarr2 2:0351277ee5dd 129
jjcarr2 2:0351277ee5dd 130
jjcarr2 2:0351277ee5dd 131
jjcarr2 2:0351277ee5dd 132 // left or right
jjcarr2 2:0351277ee5dd 133
jjcarr2 2:0351277ee5dd 134
jjcarr2 2:0351277ee5dd 135 location[1]=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
jjcarr2 2:0351277ee5dd 136
jjcarr2 2:0351277ee5dd 137
jjcarr2 2:0351277ee5dd 138
jjcarr2 2:0351277ee5dd 139
jjcarr2 2:0351277ee5dd 140
jjcarr2 2:0351277ee5dd 141
jjcarr2 2:0351277ee5dd 142
Fairy_Paolina 1:801f0b9a862a 143 leftTurn();
jjcarr2 2:0351277ee5dd 144 //wall_follow2(RIGHT);
Fairy_Paolina 1:801f0b9a862a 145 rightTurn();
Fairy_Paolina 0:ff94cc47fef7 146
jjcarr2 2:0351277ee5dd 147
jjcarr2 2:0351277ee5dd 148
jjcarr2 2:0351277ee5dd 149 bt.printf("LOCATION %f\n\r",location);
jjcarr2 2:0351277ee5dd 150
jjcarr2 2:0351277ee5dd 151 motors.stopBothMotors();
jjcarr2 2:0351277ee5dd 152 // leftTurn();
jjcarr2 2:0351277ee5dd 153 // wait(1);
jjcarr2 2:0351277ee5dd 154 // rightTurn();
jjcarr2 2:0351277ee5dd 155 */
jjcarr2 2:0351277ee5dd 156
Fairy_Paolina 0:ff94cc47fef7 157 }
jjcarr2 2:0351277ee5dd 158
jjcarr2 2:0351277ee5dd 159 void errFunction(void)
jjcarr2 2:0351277ee5dd 160 {
jjcarr2 2:0351277ee5dd 161 //Nothing
Fairy_Paolina 0:ff94cc47fef7 162 }
jjcarr2 2:0351277ee5dd 163
Fairy_Paolina 0:ff94cc47fef7 164 void us_distance(void)
Fairy_Paolina 0:ff94cc47fef7 165 {
jjcarr2 2:0351277ee5dd 166 pc.printf("Ultra Sonic\n\r");
jjcarr2 2:0351277ee5dd 167 rangeFinderLeft.startMeas();
jjcarr2 2:0351277ee5dd 168 wait_us(20);
jjcarr2 2:0351277ee5dd 169 if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID)) {
jjcarr2 2:0351277ee5dd 170 pc.printf("Range = %f\n\r", range);
jjcarr2 2:0351277ee5dd 171 }
Fairy_Paolina 0:ff94cc47fef7 172 }
jjcarr2 2:0351277ee5dd 173
jjcarr2 2:0351277ee5dd 174 float wall_follow(int side, int direction, int section)
Fairy_Paolina 0:ff94cc47fef7 175 {
jjcarr2 2:0351277ee5dd 176 float location, wavegap;
jjcarr2 2:0351277ee5dd 177 int dir=1, set=5;
jjcarr2 2:0351277ee5dd 178
jjcarr2 2:0351277ee5dd 179 pid1.reset();
jjcarr2 2:0351277ee5dd 180
jjcarr2 2:0351277ee5dd 181 if(direction == BACKWARD) dir=-1;
jjcarr2 2:0351277ee5dd 182 if(section == TOOLS)set= 5;
jjcarr2 2:0351277ee5dd 183
jjcarr2 2:0351277ee5dd 184 leftEncoder.reset();
jjcarr2 2:0351277ee5dd 185 rightEncoder.reset();
jjcarr2 2:0351277ee5dd 186
jjcarr2 2:0351277ee5dd 187 location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
jjcarr2 2:0351277ee5dd 188
jjcarr2 2:0351277ee5dd 189 while(location< 75) {
jjcarr2 2:0351277ee5dd 190 location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
jjcarr2 2:0351277ee5dd 191
jjcarr2 2:0351277ee5dd 192 pid1.setInputLimits(0, set);
jjcarr2 2:0351277ee5dd 193 pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
jjcarr2 2:0351277ee5dd 194 pid1.setSetPoint(set);
jjcarr2 2:0351277ee5dd 195 if(side){ //Side = 1 for Left
jjcarr2 2:0351277ee5dd 196 rangeFinderLeft.startMeas();
jjcarr2 2:0351277ee5dd 197 wait_ms(20);
jjcarr2 2:0351277ee5dd 198 rangeFinderLeft.getMeas(range);
jjcarr2 2:0351277ee5dd 199 }
jjcarr2 2:0351277ee5dd 200 else{ //Use right Ultrasonic
jjcarr2 2:0351277ee5dd 201 rangeFinderRight.startMeas();
jjcarr2 2:0351277ee5dd 202 wait_ms(20);
jjcarr2 2:0351277ee5dd 203 rangeFinderRight.getMeas(range);
jjcarr2 2:0351277ee5dd 204 pc.printf("%d\r\n",range);
jjcarr2 2:0351277ee5dd 205 }
jjcarr2 2:0351277ee5dd 206
jjcarr2 2:0351277ee5dd 207 if(range > 20) {
jjcarr2 2:0351277ee5dd 208 wavegap=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
jjcarr2 2:0351277ee5dd 209 bt.printf("wavegap %f\r\n",wavegap);
jjcarr2 2:0351277ee5dd 210 // AT WAVE OPENING!!!!
jjcarr2 2:0351277ee5dd 211 motors.setMotor0Speed(dir*MAX_SPEED);//right
jjcarr2 2:0351277ee5dd 212 motors.setMotor1Speed(dir*MAX_SPEED);//left
jjcarr2 2:0351277ee5dd 213
jjcarr2 2:0351277ee5dd 214 } else {
Fairy_Paolina 0:ff94cc47fef7 215
jjcarr2 2:0351277ee5dd 216 pid1.setProcessValue(range);
jjcarr2 2:0351277ee5dd 217 pid_return = pid1.compute();
jjcarr2 2:0351277ee5dd 218
jjcarr2 2:0351277ee5dd 219 if(pid_return > 0) {
jjcarr2 2:0351277ee5dd 220 if(side){
jjcarr2 2:0351277ee5dd 221 motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
jjcarr2 2:0351277ee5dd 222 motors.setMotor1Speed(dir*MAX_SPEED);//left
jjcarr2 2:0351277ee5dd 223 }
jjcarr2 2:0351277ee5dd 224 else{
jjcarr2 2:0351277ee5dd 225 motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
jjcarr2 2:0351277ee5dd 226 motors.setMotor0Speed(dir*MAX_SPEED);//right
jjcarr2 2:0351277ee5dd 227 }
jjcarr2 2:0351277ee5dd 228 }else if(pid_return < 0) {
jjcarr2 2:0351277ee5dd 229 if(side){
jjcarr2 2:0351277ee5dd 230 motors.setMotor0Speed(dir*MAX_SPEED);//right
jjcarr2 2:0351277ee5dd 231 motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
jjcarr2 2:0351277ee5dd 232 }
jjcarr2 2:0351277ee5dd 233 else{
jjcarr2 2:0351277ee5dd 234 motors.setMotor1Speed(dir*MAX_SPEED);//left
jjcarr2 2:0351277ee5dd 235 motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
jjcarr2 2:0351277ee5dd 236 }
jjcarr2 2:0351277ee5dd 237 }else {
jjcarr2 2:0351277ee5dd 238 motors.setMotor0Speed(dir*MAX_SPEED);//right
jjcarr2 2:0351277ee5dd 239 motors.setMotor1Speed(dir*MAX_SPEED);//left
jjcarr2 2:0351277ee5dd 240 }
jjcarr2 2:0351277ee5dd 241 }}
jjcarr2 2:0351277ee5dd 242 return wavegap;
jjcarr2 2:0351277ee5dd 243 }
jjcarr2 2:0351277ee5dd 244
jjcarr2 2:0351277ee5dd 245 /* MODIFIED WALL_FOLLOW FOR NAVIGATION */
jjcarr2 2:0351277ee5dd 246
jjcarr2 2:0351277ee5dd 247 void wall_follow2(int side, int direction, int section)
jjcarr2 2:0351277ee5dd 248 {
jjcarr2 2:0351277ee5dd 249 int SeeWaveGap = false, count=0, dir=1, set=5;
jjcarr2 2:0351277ee5dd 250
jjcarr2 2:0351277ee5dd 251 pid1.reset();
jjcarr2 2:0351277ee5dd 252
jjcarr2 2:0351277ee5dd 253 if(direction == BACKWARD) dir=-1;
jjcarr2 2:0351277ee5dd 254 if(section == TOOLS)set= 5;
jjcarr2 2:0351277ee5dd 255
jjcarr2 2:0351277ee5dd 256 leftEncoder.reset();
jjcarr2 2:0351277ee5dd 257 rightEncoder.reset();
jjcarr2 2:0351277ee5dd 258
jjcarr2 2:0351277ee5dd 259 while(1) {
jjcarr2 2:0351277ee5dd 260
jjcarr2 2:0351277ee5dd 261 pid1.setInputLimits(0.0, set);
Fairy_Paolina 1:801f0b9a862a 262 pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
jjcarr2 2:0351277ee5dd 263 pid1.setSetPoint(set);
jjcarr2 2:0351277ee5dd 264
jjcarr2 2:0351277ee5dd 265 if(side){
jjcarr2 2:0351277ee5dd 266 rangeFinderLeft.startMeas();
jjcarr2 2:0351277ee5dd 267 wait_ms(20);
jjcarr2 2:0351277ee5dd 268 rangeFinderLeft.getMeas(range);
jjcarr2 2:0351277ee5dd 269 }
jjcarr2 2:0351277ee5dd 270 else{
jjcarr2 2:0351277ee5dd 271 rangeFinderRight.startMeas();
jjcarr2 2:0351277ee5dd 272 wait_ms(20);
jjcarr2 2:0351277ee5dd 273 rangeFinderRight.getMeas(range);
jjcarr2 2:0351277ee5dd 274 }
jjcarr2 2:0351277ee5dd 275
jjcarr2 2:0351277ee5dd 276
jjcarr2 2:0351277ee5dd 277 /*************CHECK FOR WAVE OPENING*****************/
jjcarr2 2:0351277ee5dd 278 /* If after 20 ms the ultrasonic still sees 20+ cm */
jjcarr2 2:0351277ee5dd 279 /* then robot is at wave opening */
jjcarr2 2:0351277ee5dd 280
jjcarr2 2:0351277ee5dd 281 pc.printf("range %f\r\n",range);
jjcarr2 2:0351277ee5dd 282 if(range > 20) {
jjcarr2 2:0351277ee5dd 283 motors.stopBothMotors();
jjcarr2 2:0351277ee5dd 284 bt.printf("wavegap\r\n");
jjcarr2 2:0351277ee5dd 285 // AT WAVE OPENING!!!!
jjcarr2 2:0351277ee5dd 286 break;
jjcarr2 2:0351277ee5dd 287 }
jjcarr2 2:0351277ee5dd 288
Fairy_Paolina 0:ff94cc47fef7 289 pid1.setProcessValue(range);
jjcarr2 2:0351277ee5dd 290 pid_return = pid1.compute();
jjcarr2 2:0351277ee5dd 291 //bt.printf("Range: %f\n PID: %f\r\n", range, pid_return);
jjcarr2 2:0351277ee5dd 292
jjcarr2 2:0351277ee5dd 293 if(pid_return > 0) {
jjcarr2 2:0351277ee5dd 294 if(side){
jjcarr2 2:0351277ee5dd 295 motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
jjcarr2 2:0351277ee5dd 296 motors.setMotor1Speed(dir*MAX_SPEED);//left
jjcarr2 2:0351277ee5dd 297 }
jjcarr2 2:0351277ee5dd 298 else{
jjcarr2 2:0351277ee5dd 299 motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
jjcarr2 2:0351277ee5dd 300 motors.setMotor0Speed(dir*MAX_SPEED);//right
jjcarr2 2:0351277ee5dd 301 }
jjcarr2 2:0351277ee5dd 302 }else if(pid_return < 0) {
jjcarr2 2:0351277ee5dd 303 if(side){
jjcarr2 2:0351277ee5dd 304 motors.setMotor0Speed(dir*MAX_SPEED);//right
jjcarr2 2:0351277ee5dd 305 motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
jjcarr2 2:0351277ee5dd 306 }
jjcarr2 2:0351277ee5dd 307 else{
jjcarr2 2:0351277ee5dd 308 motors.setMotor1Speed(dir*MAX_SPEED);//left
jjcarr2 2:0351277ee5dd 309 motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
jjcarr2 2:0351277ee5dd 310 }
jjcarr2 2:0351277ee5dd 311 } else {
jjcarr2 2:0351277ee5dd 312 motors.setMotor0Speed(dir*MAX_SPEED);
jjcarr2 2:0351277ee5dd 313 motors.setMotor1Speed(dir*MAX_SPEED);
jjcarr2 2:0351277ee5dd 314 }
jjcarr2 2:0351277ee5dd 315 }
jjcarr2 2:0351277ee5dd 316 }
jjcarr2 2:0351277ee5dd 317
jjcarr2 2:0351277ee5dd 318
jjcarr2 2:0351277ee5dd 319 /* MODIFIED WALL_FOLLOW FOR NAVIGATION WITH WAVE OPENINGS PASSED IN */
jjcarr2 2:0351277ee5dd 320 /* MEANT FOR RETURNING FROM OIL RIGS */
jjcarr2 2:0351277ee5dd 321
jjcarr2 2:0351277ee5dd 322 void wall_follow3(int &currentLocation, int &WaveOpening)
jjcarr2 2:0351277ee5dd 323 {
jjcarr2 2:0351277ee5dd 324 while(1) {
jjcarr2 2:0351277ee5dd 325
jjcarr2 2:0351277ee5dd 326
jjcarr2 2:0351277ee5dd 327 pid1.setInputLimits(0, 5);
jjcarr2 2:0351277ee5dd 328 pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
jjcarr2 2:0351277ee5dd 329 pid1.setSetPoint(5.0);
jjcarr2 2:0351277ee5dd 330
jjcarr2 2:0351277ee5dd 331 rangeFinderLeft.startMeas();
jjcarr2 2:0351277ee5dd 332 wait_ms(100);
jjcarr2 2:0351277ee5dd 333 if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID) && (range < 100.0) && (range > 3.0)) {
jjcarr2 2:0351277ee5dd 334 //bt.printf("Range = %f\n", range);
jjcarr2 2:0351277ee5dd 335 }
jjcarr2 2:0351277ee5dd 336
jjcarr2 2:0351277ee5dd 337 /*************CHECK FOR WAVE OPENING*****************/
jjcarr2 2:0351277ee5dd 338 /* If after 100 ms the ultrasonic still sees 20+ cm */
jjcarr2 2:0351277ee5dd 339 /* then robot is at wave opening */
jjcarr2 2:0351277ee5dd 340
jjcarr2 2:0351277ee5dd 341
jjcarr2 2:0351277ee5dd 342 if(range > 20 ) {
jjcarr2 2:0351277ee5dd 343 currentLocation--;
jjcarr2 2:0351277ee5dd 344 }
jjcarr2 2:0351277ee5dd 345
jjcarr2 2:0351277ee5dd 346 if( currentLocation == WaveOpening) {
jjcarr2 2:0351277ee5dd 347 // AT WAVE OPENING!!!!
jjcarr2 2:0351277ee5dd 348
jjcarr2 2:0351277ee5dd 349 break;
jjcarr2 2:0351277ee5dd 350 }
jjcarr2 2:0351277ee5dd 351
jjcarr2 2:0351277ee5dd 352
jjcarr2 2:0351277ee5dd 353 pid1.setProcessValue(range);
jjcarr2 2:0351277ee5dd 354 pid_return = pid1.compute();
jjcarr2 2:0351277ee5dd 355 bt.printf("Range: %f\n PID: %f", range, pid_return);
jjcarr2 2:0351277ee5dd 356
jjcarr2 2:0351277ee5dd 357 if(pid_return > 0) {
jjcarr2 2:0351277ee5dd 358 motors.setMotor0Speed(MAX_SPEED - pid_return);
Fairy_Paolina 0:ff94cc47fef7 359 motors.setMotor1Speed(MAX_SPEED);
jjcarr2 2:0351277ee5dd 360 } else if(pid_return < 0) {
Fairy_Paolina 0:ff94cc47fef7 361 motors.setMotor0Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 362 motors.setMotor1Speed(MAX_SPEED + pid_return);
jjcarr2 2:0351277ee5dd 363 } else {
Fairy_Paolina 0:ff94cc47fef7 364 motors.setMotor0Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 365 motors.setMotor1Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 366 }
jjcarr2 2:0351277ee5dd 367 }
Fairy_Paolina 0:ff94cc47fef7 368 }
jjcarr2 2:0351277ee5dd 369
Fairy_Paolina 1:801f0b9a862a 370 void rightTurn(void)
Fairy_Paolina 1:801f0b9a862a 371 {
jjcarr2 2:0351277ee5dd 372 float speedL, speedR;
jjcarr2 2:0351277ee5dd 373
jjcarr2 2:0351277ee5dd 374 speedL=speedR= 0.4;
jjcarr2 2:0351277ee5dd 375
Fairy_Paolina 1:801f0b9a862a 376 leftEncoder.reset();
jjcarr2 2:0351277ee5dd 377 rightEncoder.reset();
jjcarr2 2:0351277ee5dd 378 motors.setMotor0Speed(-speedR*127);//right
jjcarr2 2:0351277ee5dd 379 motors.setMotor1Speed(speedL*127);//left
jjcarr2 2:0351277ee5dd 380 while(leftEncoder.getPulses()<1050 || rightEncoder.getPulses()>-1050);
Fairy_Paolina 1:801f0b9a862a 381 motors.stopBothMotors();
Fairy_Paolina 1:801f0b9a862a 382 }
jjcarr2 2:0351277ee5dd 383
Fairy_Paolina 1:801f0b9a862a 384 void leftTurn(void)
Fairy_Paolina 1:801f0b9a862a 385 {
Fairy_Paolina 1:801f0b9a862a 386 leftEncoder.reset();
jjcarr2 2:0351277ee5dd 387 rightEncoder.reset();
jjcarr2 2:0351277ee5dd 388 motors.setMotor0Speed(0.4*127);// right
jjcarr2 2:0351277ee5dd 389 motors.setMotor1Speed(-0.4*127);// left
jjcarr2 2:0351277ee5dd 390 while(leftEncoder.getPulses()>-1200 || rightEncoder.getPulses()<1200);
jjcarr2 2:0351277ee5dd 391 motors.stopBothMotors();
jjcarr2 2:0351277ee5dd 392 }
jjcarr2 2:0351277ee5dd 393
jjcarr2 2:0351277ee5dd 394 void overBump(void){
jjcarr2 2:0351277ee5dd 395
jjcarr2 2:0351277ee5dd 396 leftEncoder.reset();
jjcarr2 2:0351277ee5dd 397 rightEncoder.reset();
jjcarr2 2:0351277ee5dd 398 motors.setMotor0Speed(MAX_SPEED); //right
jjcarr2 2:0351277ee5dd 399 motors.setMotor1Speed(MAX_SPEED); //left
jjcarr2 2:0351277ee5dd 400 while(abs(leftEncoder.getPulses()/PPR) + abs(rightEncoder.getPulses()/PPR)/2 < 1);
jjcarr2 2:0351277ee5dd 401 /*
jjcarr2 2:0351277ee5dd 402 leftEncoder.reset();
jjcarr2 2:0351277ee5dd 403 rightEncoder.reset();
jjcarr2 2:0351277ee5dd 404 motors.setMotor0Speed(MAX_SPEED); //right
jjcarr2 2:0351277ee5dd 405 motors.setMotor1Speed(MAX_SPEED); //left
jjcarr2 2:0351277ee5dd 406 while(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR)/2 <3);
jjcarr2 2:0351277ee5dd 407 */
Fairy_Paolina 1:801f0b9a862a 408 motors.stopBothMotors();
Fairy_Paolina 1:801f0b9a862a 409 }
jjcarr2 2:0351277ee5dd 410
jjcarr2 2:0351277ee5dd 411 void tools_section(float *location, float &current){
jjcarr2 2:0351277ee5dd 412
jjcarr2 2:0351277ee5dd 413 location[0]=wall_follow(LEFT,FORWARD, TOOLS); //location from the left edge of the field
jjcarr2 2:0351277ee5dd 414 current=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
jjcarr2 2:0351277ee5dd 415 bt.printf("left Encoder : %d \t right Encoder %d \r\n",leftEncoder.getPulses(),rightEncoder.getPulses());
jjcarr2 2:0351277ee5dd 416
jjcarr2 2:0351277ee5dd 417 bt.printf("wavegap %f \t current %f \r\n",location[0],current);
jjcarr2 2:0351277ee5dd 418
jjcarr2 2:0351277ee5dd 419 motors.stopBothMotors();
jjcarr2 2:0351277ee5dd 420 ////////////////////////////////////////// determine shape and pick up tool ///////////////////////////////////////////////////////
jjcarr2 2:0351277ee5dd 421
jjcarr2 2:0351277ee5dd 422 if(current >location[0]){
jjcarr2 2:0351277ee5dd 423 wall_follow2(LEFT,BACKWARD,TOOLS);
jjcarr2 2:0351277ee5dd 424 wait_ms(100);
jjcarr2 2:0351277ee5dd 425 // forward
jjcarr2 2:0351277ee5dd 426
jjcarr2 2:0351277ee5dd 427 /* leftEncoder.reset();
jjcarr2 2:0351277ee5dd 428 rightEncoder.reset();
jjcarr2 2:0351277ee5dd 429 motors.setMotor0Speed(-MAX_SPEED); //right
jjcarr2 2:0351277ee5dd 430 motors.setMotor1Speed(-MAX_SPEED); //left
jjcarr2 2:0351277ee5dd 431 while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300);
jjcarr2 2:0351277ee5dd 432 motors.stopBothMotors();
jjcarr2 2:0351277ee5dd 433 wait_ms(500);*/
jjcarr2 2:0351277ee5dd 434 current = (abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
jjcarr2 2:0351277ee5dd 435 // REMOVED += because current should just be current pulses?
jjcarr2 2:0351277ee5dd 436 }
jjcarr2 2:0351277ee5dd 437 else{/*
jjcarr2 2:0351277ee5dd 438 wall_follow2(LEFT,FORWARD);
jjcarr2 2:0351277ee5dd 439 // backward
jjcarr2 2:0351277ee5dd 440 leftEncoder.reset();
jjcarr2 2:0351277ee5dd 441 rightEncoder.reset();
jjcarr2 2:0351277ee5dd 442 motors.setMotor0Speed(MAX_SPEED); //right
jjcarr2 2:0351277ee5dd 443 motors.setMotor1Speed(MAX_SPEED); //left
jjcarr2 2:0351277ee5dd 444 while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300);
jjcarr2 2:0351277ee5dd 445 motors.stopBothMotors();
jjcarr2 2:0351277ee5dd 446 current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;*/
jjcarr2 2:0351277ee5dd 447 }
jjcarr2 2:0351277ee5dd 448
jjcarr2 2:0351277ee5dd 449 leftTurn();
jjcarr2 2:0351277ee5dd 450
jjcarr2 2:0351277ee5dd 451 //Go over
jjcarr2 2:0351277ee5dd 452 overBump();
jjcarr2 2:0351277ee5dd 453
jjcarr2 2:0351277ee5dd 454 }
jjcarr2 2:0351277ee5dd 455