Justin Carr / NavigationTest

Tools Section Navigation Calibrated slightly

Dependencies:   HCSR04 PID PololuQik2 QEI mbed-rtos

Fork of NavigationTest by Paolina Povolotskaya

main.cpp@7:0b7897232e93, 2014-03-21 (annotated)

Committer:
jjcarr2
Date:
Fri Mar 21 00:02:51 2014 +0000
Revision:
7:0b7897232e93
Parent:
6:109f46d3cb96
Child:
8:32ba0fad1689
Tools Section Calibrated  further

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 7:0b7897232e93 37 PID pid1(15.0,0.0,10.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 7:0b7897232e93 54 void alignWithWall(void);
jjcarr2 2:0351277ee5dd 55
Fairy_Paolina 1:801f0b9a862a 56 //Variables
jjcarr2 2:0351277ee5dd 57
jjcarr2 2:0351277ee5dd 58 int main(void)
jjcarr2 2:0351277ee5dd 59 {
jjcarr2 2:0351277ee5dd 60 float location[3], current=0;
jjcarr2 2:0351277ee5dd 61 int direction[3];
jjcarr2 2:0351277ee5dd 62
Fairy_Paolina 0:ff94cc47fef7 63 pc.baud(115200);
Fairy_Paolina 1:801f0b9a862a 64 bt.baud(115200);
jjcarr2 2:0351277ee5dd 65 motors.begin();
jjcarr2 6:109f46d3cb96 66
jjcarr2 7:0b7897232e93 67 /*****************************OLD CODE FOR GOING THROUGH WAVE SECTION****************/
jjcarr2 7:0b7897232e93 68 /*
jjcarr2 7:0b7897232e93 69 leftEncoder.reset();
jjcarr2 7:0b7897232e93 70 rightEncoder.reset();
jjcarr2 7:0b7897232e93 71 wall_follow2(LEFT,FORWARD,TOOLS);
jjcarr2 7:0b7897232e93 72
jjcarr2 7:0b7897232e93 73 motors.stopBothMotors();
jjcarr2 7:0b7897232e93 74 wait_ms(500);
jjcarr2 7:0b7897232e93 75 leftEncoder.reset();
jjcarr2 7:0b7897232e93 76 rightEncoder.reset();
jjcarr2 7:0b7897232e93 77
jjcarr2 7:0b7897232e93 78 motors.setMotor0Speed(0.6*MAX_SPEED); //right
jjcarr2 7:0b7897232e93 79 motors.setMotor1Speed(0.6*MAX_SPEED); //left
jjcarr2 7:0b7897232e93 80
jjcarr2 7:0b7897232e93 81 wait_ms(200);
jjcarr2 7:0b7897232e93 82 motors.stopBothMotors();
jjcarr2 7:0b7897232e93 83
jjcarr2 7:0b7897232e93 84 leftEncoder.reset();
jjcarr2 7:0b7897232e93 85 rightEncoder.reset();
jjcarr2 7:0b7897232e93 86 bt.printf("1st turn left\r\n");
jjcarr2 7:0b7897232e93 87
jjcarr2 7:0b7897232e93 88 motors.setMotor0Speed(MAX_SPEED); //right
jjcarr2 7:0b7897232e93 89 motors.setMotor1Speed(-MAX_SPEED); //left
jjcarr2 7:0b7897232e93 90
jjcarr2 7:0b7897232e93 91 while(((abs(leftEncoder.getPulses())/(PPR) + abs(rightEncoder.getPulses())/(PPR))/2) < 1);
jjcarr2 7:0b7897232e93 92
jjcarr2 7:0b7897232e93 93 leftEncoder.reset();
jjcarr2 7:0b7897232e93 94 rightEncoder.reset();
jjcarr2 7:0b7897232e93 95
jjcarr2 7:0b7897232e93 96 motors.setMotor0Speed(MAX_SPEED); //right
jjcarr2 7:0b7897232e93 97 motors.setMotor1Speed(MAX_SPEED); //left
jjcarr2 7:0b7897232e93 98
jjcarr2 7:0b7897232e93 99 while(((abs(leftEncoder.getPulses())/(PPR) + abs(rightEncoder.getPulses())/PPR)/2) < 2);
jjcarr2 7:0b7897232e93 100
jjcarr2 7:0b7897232e93 101 leftEncoder.reset();
jjcarr2 7:0b7897232e93 102 rightEncoder.reset();
jjcarr2 7:0b7897232e93 103 motors.setMotor0Speed(-MAX_SPEED); //right
jjcarr2 7:0b7897232e93 104 motors.setMotor1Speed(MAX_SPEED); //left
jjcarr2 7:0b7897232e93 105 float usValue = 0;
jjcarr2 7:0b7897232e93 106 while(1){
jjcarr2 7:0b7897232e93 107 rangeFinderLeft.startMeas();
jjcarr2 7:0b7897232e93 108 wait_ms(100);
jjcarr2 7:0b7897232e93 109 rangeFinderLeft.getMeas(range);
jjcarr2 7:0b7897232e93 110 bt.printf("Range %f \t OldValue %f\n\r",range, usValue);
jjcarr2 7:0b7897232e93 111 if(range > usValue && usValue != 0 && range < 10){
jjcarr2 7:0b7897232e93 112 break;
jjcarr2 7:0b7897232e93 113 } else {
jjcarr2 7:0b7897232e93 114 usValue = range;
jjcarr2 7:0b7897232e93 115 }
jjcarr2 7:0b7897232e93 116 }
jjcarr2 7:0b7897232e93 117 motors.stopBothMotors();
jjcarr2 7:0b7897232e93 118
jjcarr2 7:0b7897232e93 119 leftEncoder.reset();
jjcarr2 7:0b7897232e93 120 rightEncoder.reset();
jjcarr2 7:0b7897232e93 121
jjcarr2 7:0b7897232e93 122 wall_follow2(LEFT,FORWARD,TOOLS);
jjcarr2 7:0b7897232e93 123
jjcarr2 7:0b7897232e93 124 while(1){}
jjcarr2 7:0b7897232e93 125 */
jjcarr2 7:0b7897232e93 126 /*
jjcarr2 2:0351277ee5dd 127 // Very Consistent Turn
jjcarr2 2:0351277ee5dd 128 leftEncoder.reset();
jjcarr2 2:0351277ee5dd 129 rightEncoder.reset();
jjcarr2 2:0351277ee5dd 130
jjcarr2 2:0351277ee5dd 131 motors.setMotor0Speed(-MAX_SPEED); //right
jjcarr2 2:0351277ee5dd 132 motors.setMotor1Speed(MAX_SPEED); //left
jjcarr2 2:0351277ee5dd 133 float usValue = 0;
jjcarr2 2:0351277ee5dd 134 while(1){
jjcarr2 2:0351277ee5dd 135 rangeFinderLeft.startMeas();
jjcarr2 2:0351277ee5dd 136 wait_ms(100);
jjcarr2 2:0351277ee5dd 137 rangeFinderLeft.getMeas(range);
jjcarr2 2:0351277ee5dd 138 bt.printf("Range %f \t OldValue %f\n\r",range, usValue);
jjcarr2 2:0351277ee5dd 139 if(range > usValue && usValue != 0 && range < 10){
jjcarr2 2:0351277ee5dd 140 break;
jjcarr2 2:0351277ee5dd 141 } else {
jjcarr2 2:0351277ee5dd 142 usValue = range;
jjcarr2 2:0351277ee5dd 143 }
jjcarr2 2:0351277ee5dd 144 }
jjcarr2 7:0b7897232e93 145 */
jjcarr2 2:0351277ee5dd 146 motors.stopBothMotors();
jjcarr2 2:0351277ee5dd 147 /*
jjcarr2 2:0351277ee5dd 148
jjcarr2 2:0351277ee5dd 149 leftEncoder.reset();
jjcarr2 2:0351277ee5dd 150 rightEncoder.reset();
jjcarr2 2:0351277ee5dd 151 motors.setMotor0Speed(MAX_SPEED); //right
jjcarr2 2:0351277ee5dd 152 motors.setMotor1Speed(MAX_SPEED); //left
jjcarr2 2:0351277ee5dd 153
jjcarr2 2:0351277ee5dd 154 while(leftEncoder.getPulses()/(PPR) < 3);
Fairy_Paolina 1:801f0b9a862a 155 */
jjcarr2 2:0351277ee5dd 156
jjcarr2 2:0351277ee5dd 157 //leftEncoder.reset();
jjcarr2 2:0351277ee5dd 158 //rightEncoder.reset();
jjcarr2 2:0351277ee5dd 159 //motors.setMotor0Speed(MAX_SPEED); //right
jjcarr2 2:0351277ee5dd 160 //motors.setMotor1Speed(MAX_SPEED); //left
jjcarr2 2:0351277ee5dd 161
jjcarr2 2:0351277ee5dd 162 //while((abs(leftEncoder.getPulses())/(PPR) + abs(rightEncoder.getPulses())/(PPR))/2 < 3)
jjcarr2 2:0351277ee5dd 163
jjcarr2 2:0351277ee5dd 164
jjcarr2 2:0351277ee5dd 165
jjcarr2 2:0351277ee5dd 166 //Go to tools
jjcarr2 2:0351277ee5dd 167 tools_section(location, current);
jjcarr2 2:0351277ee5dd 168 bt.printf("Location 0 = %f", location[0]);
Fairy_Paolina 0:ff94cc47fef7 169
jjcarr2 2:0351277ee5dd 170 /*
jjcarr2 2:0351277ee5dd 171 //////////////////////////////// without predefined wavegaps//////////////////////////////////////////////
jjcarr2 2:0351277ee5dd 172 current=0;
jjcarr2 2:0351277ee5dd 173 if(location[0]< 75){
jjcarr2 2:0351277ee5dd 174 turnRight();
jjcarr2 2:0351277ee5dd 175 current=wall_follow(LEFT,FORWARD);
jjcarr2 2:0351277ee5dd 176 if(current == 0)turnLeft();
jjcarr2 2:0351277ee5dd 177 else{
jjcarr2 2:0351277ee5dd 178 direction[0]= RIGHT;
jjcarr2 2:0351277ee5dd 179 turnLeft();
jjcarr2 2:0351277ee5dd 180 overBump();
jjcarr2 2:0351277ee5dd 181 }
jjcarr2 2:0351277ee5dd 182 }
jjcarr2 2:0351277ee5dd 183 else if(location[0]>=75 || current == 0){
jjcarr2 2:0351277ee5dd 184 turnLeft();
jjcarr2 2:0351277ee5dd 185 wall_follow2(RIGHT,FORWARD);
jjcarr2 2:0351277ee5dd 186 }
jjcarr2 2:0351277ee5dd 187
jjcarr2 2:0351277ee5dd 188
jjcarr2 2:0351277ee5dd 189
jjcarr2 2:0351277ee5dd 190
jjcarr2 2:0351277ee5dd 191
jjcarr2 2:0351277ee5dd 192
jjcarr2 2:0351277ee5dd 193 // left or right
jjcarr2 2:0351277ee5dd 194
jjcarr2 2:0351277ee5dd 195
jjcarr2 2:0351277ee5dd 196 location[1]=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
jjcarr2 2:0351277ee5dd 197
jjcarr2 2:0351277ee5dd 198
jjcarr2 2:0351277ee5dd 199
jjcarr2 2:0351277ee5dd 200
jjcarr2 2:0351277ee5dd 201
jjcarr2 2:0351277ee5dd 202
jjcarr2 2:0351277ee5dd 203
Fairy_Paolina 1:801f0b9a862a 204 leftTurn();
jjcarr2 2:0351277ee5dd 205 //wall_follow2(RIGHT);
Fairy_Paolina 1:801f0b9a862a 206 rightTurn();
Fairy_Paolina 0:ff94cc47fef7 207
jjcarr2 2:0351277ee5dd 208
jjcarr2 2:0351277ee5dd 209
jjcarr2 2:0351277ee5dd 210 bt.printf("LOCATION %f\n\r",location);
jjcarr2 2:0351277ee5dd 211
jjcarr2 2:0351277ee5dd 212 motors.stopBothMotors();
jjcarr2 2:0351277ee5dd 213 // leftTurn();
jjcarr2 2:0351277ee5dd 214 // wait(1);
jjcarr2 2:0351277ee5dd 215 // rightTurn();
jjcarr2 2:0351277ee5dd 216 */
jjcarr2 2:0351277ee5dd 217
Fairy_Paolina 0:ff94cc47fef7 218 }
jjcarr2 2:0351277ee5dd 219
jjcarr2 2:0351277ee5dd 220 void errFunction(void)
jjcarr2 2:0351277ee5dd 221 {
jjcarr2 2:0351277ee5dd 222 //Nothing
Fairy_Paolina 0:ff94cc47fef7 223 }
jjcarr2 2:0351277ee5dd 224
Fairy_Paolina 0:ff94cc47fef7 225 void us_distance(void)
Fairy_Paolina 0:ff94cc47fef7 226 {
jjcarr2 7:0b7897232e93 227
jjcarr2 2:0351277ee5dd 228 rangeFinderLeft.startMeas();
jjcarr2 2:0351277ee5dd 229 wait_us(20);
jjcarr2 2:0351277ee5dd 230 if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID)) {
jjcarr2 2:0351277ee5dd 231 pc.printf("Range = %f\n\r", range);
jjcarr2 2:0351277ee5dd 232 }
Fairy_Paolina 0:ff94cc47fef7 233 }
jjcarr2 2:0351277ee5dd 234
jjcarr2 2:0351277ee5dd 235 float wall_follow(int side, int direction, int section)
Fairy_Paolina 0:ff94cc47fef7 236 {
jjcarr2 2:0351277ee5dd 237 float location, wavegap;
jjcarr2 2:0351277ee5dd 238 int dir=1, set=5;
jjcarr2 2:0351277ee5dd 239
jjcarr2 2:0351277ee5dd 240 pid1.reset();
jjcarr2 2:0351277ee5dd 241
jjcarr2 2:0351277ee5dd 242 if(direction == BACKWARD) dir=-1;
jjcarr2 7:0b7897232e93 243 if(section == TOOLS)set= 10;
jjcarr2 2:0351277ee5dd 244
jjcarr2 2:0351277ee5dd 245 leftEncoder.reset();
jjcarr2 2:0351277ee5dd 246 rightEncoder.reset();
jjcarr2 2:0351277ee5dd 247
jjcarr2 2:0351277ee5dd 248 location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
jjcarr2 2:0351277ee5dd 249
jjcarr2 2:0351277ee5dd 250 while(location< 75) {
jjcarr2 2:0351277ee5dd 251 location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
jjcarr2 2:0351277ee5dd 252
jjcarr2 2:0351277ee5dd 253 pid1.setInputLimits(0, set);
jjcarr2 2:0351277ee5dd 254 pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
jjcarr2 2:0351277ee5dd 255 pid1.setSetPoint(set);
jjcarr2 2:0351277ee5dd 256 if(side){ //Side = 1 for Left
jjcarr2 2:0351277ee5dd 257 rangeFinderLeft.startMeas();
jjcarr2 2:0351277ee5dd 258 wait_ms(20);
jjcarr2 2:0351277ee5dd 259 rangeFinderLeft.getMeas(range);
jjcarr2 2:0351277ee5dd 260 }
jjcarr2 2:0351277ee5dd 261 else{ //Use right Ultrasonic
jjcarr2 2:0351277ee5dd 262 rangeFinderRight.startMeas();
jjcarr2 2:0351277ee5dd 263 wait_ms(20);
jjcarr2 2:0351277ee5dd 264 rangeFinderRight.getMeas(range);
jjcarr2 2:0351277ee5dd 265 pc.printf("%d\r\n",range);
jjcarr2 2:0351277ee5dd 266 }
jjcarr2 2:0351277ee5dd 267
jjcarr2 2:0351277ee5dd 268 if(range > 20) {
jjcarr2 2:0351277ee5dd 269 wavegap=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
jjcarr2 2:0351277ee5dd 270 bt.printf("wavegap %f\r\n",wavegap);
jjcarr2 2:0351277ee5dd 271 // AT WAVE OPENING!!!!
jjcarr2 7:0b7897232e93 272 motors.setMotor1Speed(dir*MAX_SPEED);//left
jjcarr2 2:0351277ee5dd 273 motors.setMotor0Speed(dir*MAX_SPEED);//right
jjcarr2 7:0b7897232e93 274
jjcarr2 2:0351277ee5dd 275 } else {
Fairy_Paolina 0:ff94cc47fef7 276
jjcarr2 7:0b7897232e93 277
jjcarr2 2:0351277ee5dd 278 pid1.setProcessValue(range);
jjcarr2 2:0351277ee5dd 279 pid_return = pid1.compute();
jjcarr2 2:0351277ee5dd 280
jjcarr2 2:0351277ee5dd 281 if(pid_return > 0) {
jjcarr2 2:0351277ee5dd 282 if(side){
jjcarr2 2:0351277ee5dd 283 motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
jjcarr2 2:0351277ee5dd 284 motors.setMotor1Speed(dir*MAX_SPEED);//left
jjcarr2 2:0351277ee5dd 285 }
jjcarr2 2:0351277ee5dd 286 else{
jjcarr2 2:0351277ee5dd 287 motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
jjcarr2 2:0351277ee5dd 288 motors.setMotor0Speed(dir*MAX_SPEED);//right
jjcarr2 2:0351277ee5dd 289 }
jjcarr2 2:0351277ee5dd 290 }else if(pid_return < 0) {
jjcarr2 2:0351277ee5dd 291 if(side){
jjcarr2 2:0351277ee5dd 292 motors.setMotor0Speed(dir*MAX_SPEED);//right
jjcarr2 2:0351277ee5dd 293 motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
jjcarr2 2:0351277ee5dd 294 }
jjcarr2 2:0351277ee5dd 295 else{
jjcarr2 2:0351277ee5dd 296 motors.setMotor1Speed(dir*MAX_SPEED);//left
jjcarr2 2:0351277ee5dd 297 motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
jjcarr2 2:0351277ee5dd 298 }
jjcarr2 2:0351277ee5dd 299 }else {
jjcarr2 2:0351277ee5dd 300 motors.setMotor0Speed(dir*MAX_SPEED);//right
jjcarr2 2:0351277ee5dd 301 motors.setMotor1Speed(dir*MAX_SPEED);//left
jjcarr2 2:0351277ee5dd 302 }
jjcarr2 7:0b7897232e93 303 }
jjcarr2 7:0b7897232e93 304 }
jjcarr2 2:0351277ee5dd 305 return wavegap;
jjcarr2 2:0351277ee5dd 306 }
jjcarr2 2:0351277ee5dd 307
jjcarr2 2:0351277ee5dd 308 /* MODIFIED WALL_FOLLOW FOR NAVIGATION */
jjcarr2 2:0351277ee5dd 309
jjcarr2 2:0351277ee5dd 310 void wall_follow2(int side, int direction, int section)
jjcarr2 2:0351277ee5dd 311 {
jjcarr2 7:0b7897232e93 312 int SeeWaveGap = false, count=0, set=5;
jjcarr2 7:0b7897232e93 313 float dir=1.0;
jjcarr2 2:0351277ee5dd 314
jjcarr2 2:0351277ee5dd 315 pid1.reset();
jjcarr2 2:0351277ee5dd 316
jjcarr2 2:0351277ee5dd 317 if(direction == BACKWARD) dir=-1;
jjcarr2 2:0351277ee5dd 318 if(section == TOOLS)set= 5;
jjcarr2 2:0351277ee5dd 319
jjcarr2 2:0351277ee5dd 320 leftEncoder.reset();
jjcarr2 2:0351277ee5dd 321 rightEncoder.reset();
jjcarr2 2:0351277ee5dd 322
jjcarr2 2:0351277ee5dd 323 while(1) {
jjcarr2 2:0351277ee5dd 324
jjcarr2 2:0351277ee5dd 325 pid1.setInputLimits(0.0, set);
Fairy_Paolina 1:801f0b9a862a 326 pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
jjcarr2 2:0351277ee5dd 327 pid1.setSetPoint(set);
jjcarr2 2:0351277ee5dd 328
jjcarr2 2:0351277ee5dd 329 if(side){
jjcarr2 2:0351277ee5dd 330 rangeFinderLeft.startMeas();
jjcarr2 2:0351277ee5dd 331 wait_ms(20);
jjcarr2 2:0351277ee5dd 332 rangeFinderLeft.getMeas(range);
jjcarr2 2:0351277ee5dd 333 }
jjcarr2 2:0351277ee5dd 334 else{
jjcarr2 2:0351277ee5dd 335 rangeFinderRight.startMeas();
jjcarr2 2:0351277ee5dd 336 wait_ms(20);
jjcarr2 2:0351277ee5dd 337 rangeFinderRight.getMeas(range);
jjcarr2 2:0351277ee5dd 338 }
jjcarr2 2:0351277ee5dd 339
jjcarr2 2:0351277ee5dd 340
jjcarr2 2:0351277ee5dd 341 /*************CHECK FOR WAVE OPENING*****************/
jjcarr2 2:0351277ee5dd 342 /* If after 20 ms the ultrasonic still sees 20+ cm */
jjcarr2 2:0351277ee5dd 343 /* then robot is at wave opening */
jjcarr2 2:0351277ee5dd 344
jjcarr2 2:0351277ee5dd 345 pc.printf("range %f\r\n",range);
jjcarr2 2:0351277ee5dd 346 if(range > 20) {
jjcarr2 2:0351277ee5dd 347 motors.stopBothMotors();
jjcarr2 2:0351277ee5dd 348 bt.printf("wavegap\r\n");
jjcarr2 2:0351277ee5dd 349 // AT WAVE OPENING!!!!
jjcarr2 2:0351277ee5dd 350 break;
jjcarr2 2:0351277ee5dd 351 }
jjcarr2 2:0351277ee5dd 352
Fairy_Paolina 0:ff94cc47fef7 353 pid1.setProcessValue(range);
jjcarr2 2:0351277ee5dd 354 pid_return = pid1.compute();
jjcarr2 2:0351277ee5dd 355 //bt.printf("Range: %f\n PID: %f\r\n", range, pid_return);
jjcarr2 2:0351277ee5dd 356
jjcarr2 2:0351277ee5dd 357 if(pid_return > 0) {
jjcarr2 2:0351277ee5dd 358 if(side){
jjcarr2 2:0351277ee5dd 359 motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
jjcarr2 2:0351277ee5dd 360 motors.setMotor1Speed(dir*MAX_SPEED);//left
jjcarr2 2:0351277ee5dd 361 }
jjcarr2 2:0351277ee5dd 362 else{
jjcarr2 2:0351277ee5dd 363 motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
jjcarr2 2:0351277ee5dd 364 motors.setMotor0Speed(dir*MAX_SPEED);//right
jjcarr2 2:0351277ee5dd 365 }
jjcarr2 2:0351277ee5dd 366 }else if(pid_return < 0) {
jjcarr2 2:0351277ee5dd 367 if(side){
jjcarr2 2:0351277ee5dd 368 motors.setMotor0Speed(dir*MAX_SPEED);//right
jjcarr2 2:0351277ee5dd 369 motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
jjcarr2 2:0351277ee5dd 370 }
jjcarr2 2:0351277ee5dd 371 else{
jjcarr2 2:0351277ee5dd 372 motors.setMotor1Speed(dir*MAX_SPEED);//left
jjcarr2 2:0351277ee5dd 373 motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
jjcarr2 2:0351277ee5dd 374 }
jjcarr2 2:0351277ee5dd 375 } else {
jjcarr2 2:0351277ee5dd 376 motors.setMotor0Speed(dir*MAX_SPEED);
jjcarr2 2:0351277ee5dd 377 motors.setMotor1Speed(dir*MAX_SPEED);
jjcarr2 2:0351277ee5dd 378 }
jjcarr2 2:0351277ee5dd 379 }
jjcarr2 2:0351277ee5dd 380 }
jjcarr2 2:0351277ee5dd 381
jjcarr2 2:0351277ee5dd 382
jjcarr2 2:0351277ee5dd 383 /* MODIFIED WALL_FOLLOW FOR NAVIGATION WITH WAVE OPENINGS PASSED IN */
jjcarr2 2:0351277ee5dd 384 /* MEANT FOR RETURNING FROM OIL RIGS */
jjcarr2 2:0351277ee5dd 385
jjcarr2 2:0351277ee5dd 386 void wall_follow3(int &currentLocation, int &WaveOpening)
jjcarr2 2:0351277ee5dd 387 {
jjcarr2 2:0351277ee5dd 388 while(1) {
jjcarr2 2:0351277ee5dd 389
jjcarr2 2:0351277ee5dd 390
jjcarr2 2:0351277ee5dd 391 pid1.setInputLimits(0, 5);
jjcarr2 2:0351277ee5dd 392 pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
jjcarr2 2:0351277ee5dd 393 pid1.setSetPoint(5.0);
jjcarr2 2:0351277ee5dd 394
jjcarr2 2:0351277ee5dd 395 rangeFinderLeft.startMeas();
jjcarr2 2:0351277ee5dd 396 wait_ms(100);
jjcarr2 2:0351277ee5dd 397 if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID) && (range < 100.0) && (range > 3.0)) {
jjcarr2 2:0351277ee5dd 398 //bt.printf("Range = %f\n", range);
jjcarr2 2:0351277ee5dd 399 }
jjcarr2 2:0351277ee5dd 400
jjcarr2 2:0351277ee5dd 401 /*************CHECK FOR WAVE OPENING*****************/
jjcarr2 2:0351277ee5dd 402 /* If after 100 ms the ultrasonic still sees 20+ cm */
jjcarr2 2:0351277ee5dd 403 /* then robot is at wave opening */
jjcarr2 2:0351277ee5dd 404
jjcarr2 2:0351277ee5dd 405
jjcarr2 2:0351277ee5dd 406 if(range > 20 ) {
jjcarr2 2:0351277ee5dd 407 currentLocation--;
jjcarr2 2:0351277ee5dd 408 }
jjcarr2 2:0351277ee5dd 409
jjcarr2 2:0351277ee5dd 410 if( currentLocation == WaveOpening) {
jjcarr2 2:0351277ee5dd 411 // AT WAVE OPENING!!!!
jjcarr2 2:0351277ee5dd 412
jjcarr2 2:0351277ee5dd 413 break;
jjcarr2 2:0351277ee5dd 414 }
jjcarr2 2:0351277ee5dd 415
jjcarr2 2:0351277ee5dd 416
jjcarr2 2:0351277ee5dd 417 pid1.setProcessValue(range);
jjcarr2 2:0351277ee5dd 418 pid_return = pid1.compute();
jjcarr2 2:0351277ee5dd 419 bt.printf("Range: %f\n PID: %f", range, pid_return);
jjcarr2 2:0351277ee5dd 420
jjcarr2 2:0351277ee5dd 421 if(pid_return > 0) {
jjcarr2 2:0351277ee5dd 422 motors.setMotor0Speed(MAX_SPEED - pid_return);
Fairy_Paolina 0:ff94cc47fef7 423 motors.setMotor1Speed(MAX_SPEED);
jjcarr2 2:0351277ee5dd 424 } else if(pid_return < 0) {
Fairy_Paolina 0:ff94cc47fef7 425 motors.setMotor0Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 426 motors.setMotor1Speed(MAX_SPEED + pid_return);
jjcarr2 2:0351277ee5dd 427 } else {
Fairy_Paolina 0:ff94cc47fef7 428 motors.setMotor0Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 429 motors.setMotor1Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 430 }
jjcarr2 2:0351277ee5dd 431 }
Fairy_Paolina 0:ff94cc47fef7 432 }
jjcarr2 2:0351277ee5dd 433
Fairy_Paolina 1:801f0b9a862a 434 void rightTurn(void)
Fairy_Paolina 1:801f0b9a862a 435 {
jjcarr2 2:0351277ee5dd 436 float speedL, speedR;
jjcarr2 2:0351277ee5dd 437
jjcarr2 2:0351277ee5dd 438 speedL=speedR= 0.4;
jjcarr2 2:0351277ee5dd 439
Fairy_Paolina 1:801f0b9a862a 440 leftEncoder.reset();
jjcarr2 2:0351277ee5dd 441 rightEncoder.reset();
jjcarr2 2:0351277ee5dd 442 motors.setMotor0Speed(-speedR*127);//right
jjcarr2 2:0351277ee5dd 443 motors.setMotor1Speed(speedL*127);//left
jjcarr2 7:0b7897232e93 444 while(leftEncoder.getPulses()<900 || rightEncoder.getPulses()>-900);
Fairy_Paolina 1:801f0b9a862a 445 motors.stopBothMotors();
Fairy_Paolina 1:801f0b9a862a 446 }
jjcarr2 2:0351277ee5dd 447
Fairy_Paolina 1:801f0b9a862a 448 void leftTurn(void)
Fairy_Paolina 1:801f0b9a862a 449 {
Fairy_Paolina 1:801f0b9a862a 450 leftEncoder.reset();
jjcarr2 2:0351277ee5dd 451 rightEncoder.reset();
jjcarr2 2:0351277ee5dd 452 motors.setMotor0Speed(0.4*127);// right
jjcarr2 2:0351277ee5dd 453 motors.setMotor1Speed(-0.4*127);// left
jjcarr2 7:0b7897232e93 454 while(leftEncoder.getPulses()>-940 || rightEncoder.getPulses()<940);
jjcarr2 2:0351277ee5dd 455 motors.stopBothMotors();
jjcarr2 2:0351277ee5dd 456 }
jjcarr2 2:0351277ee5dd 457
jjcarr2 2:0351277ee5dd 458 void overBump(void){
jjcarr2 2:0351277ee5dd 459
jjcarr2 2:0351277ee5dd 460 leftEncoder.reset();
jjcarr2 2:0351277ee5dd 461 rightEncoder.reset();
jjcarr2 2:0351277ee5dd 462 motors.setMotor0Speed(MAX_SPEED); //right
jjcarr2 2:0351277ee5dd 463 motors.setMotor1Speed(MAX_SPEED); //left
jjcarr2 2:0351277ee5dd 464 while(abs(leftEncoder.getPulses()/PPR) + abs(rightEncoder.getPulses()/PPR)/2 < 1);
jjcarr2 2:0351277ee5dd 465 /*
jjcarr2 2:0351277ee5dd 466 leftEncoder.reset();
jjcarr2 2:0351277ee5dd 467 rightEncoder.reset();
jjcarr2 2:0351277ee5dd 468 motors.setMotor0Speed(MAX_SPEED); //right
jjcarr2 2:0351277ee5dd 469 motors.setMotor1Speed(MAX_SPEED); //left
jjcarr2 2:0351277ee5dd 470 while(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR)/2 <3);
jjcarr2 2:0351277ee5dd 471 */
Fairy_Paolina 1:801f0b9a862a 472 motors.stopBothMotors();
Fairy_Paolina 1:801f0b9a862a 473 }
jjcarr2 7:0b7897232e93 474 void alignWithWall(void){
jjcarr2 7:0b7897232e93 475
jjcarr2 7:0b7897232e93 476 leftEncoder.reset();
jjcarr2 7:0b7897232e93 477 rightEncoder.reset();
jjcarr2 7:0b7897232e93 478
jjcarr2 7:0b7897232e93 479 motors.setMotor0Speed(-MAX_SPEED); //right
jjcarr2 7:0b7897232e93 480 motors.setMotor1Speed(0.4*MAX_SPEED); //left
jjcarr2 7:0b7897232e93 481 while(rightEncoder.getPulses()>-1000);
jjcarr2 7:0b7897232e93 482 motors.stopBothMotors();
jjcarr2 7:0b7897232e93 483
jjcarr2 7:0b7897232e93 484 leftEncoder.reset();
jjcarr2 7:0b7897232e93 485 rightEncoder.reset();
jjcarr2 7:0b7897232e93 486
jjcarr2 7:0b7897232e93 487 motors.setMotor0Speed(-MAX_SPEED); //right
jjcarr2 7:0b7897232e93 488 motors.setMotor1Speed(-MAX_SPEED); //left
jjcarr2 7:0b7897232e93 489 while(abs(leftEncoder.getPulses()*11.12/PPR) < 3);
jjcarr2 7:0b7897232e93 490
jjcarr2 7:0b7897232e93 491 motors.setMotor0Speed(0.1*MAX_SPEED); //right
jjcarr2 7:0b7897232e93 492 motors.setMotor1Speed(0.1*MAX_SPEED); //left
jjcarr2 7:0b7897232e93 493
jjcarr2 7:0b7897232e93 494 wait_ms(200);
jjcarr2 7:0b7897232e93 495
jjcarr2 7:0b7897232e93 496 motors.stopBothMotors();
jjcarr2 7:0b7897232e93 497
jjcarr2 7:0b7897232e93 498 leftEncoder.reset();
jjcarr2 7:0b7897232e93 499 rightEncoder.reset();
jjcarr2 7:0b7897232e93 500
jjcarr2 7:0b7897232e93 501 motors.setMotor0Speed(MAX_SPEED); //right
jjcarr2 7:0b7897232e93 502 motors.setMotor1Speed(-MAX_SPEED); //left
jjcarr2 7:0b7897232e93 503 while(rightEncoder.getPulses() < 260);
jjcarr2 7:0b7897232e93 504
jjcarr2 7:0b7897232e93 505 motors.setMotor0Speed(0.7*MAX_SPEED); //right
jjcarr2 7:0b7897232e93 506 motors.setMotor1Speed(-0.7*MAX_SPEED); //left
jjcarr2 7:0b7897232e93 507 float usValue = 0;
jjcarr2 7:0b7897232e93 508 while(1){
jjcarr2 7:0b7897232e93 509 rangeFinderLeft.startMeas();
jjcarr2 7:0b7897232e93 510 wait_ms(100);
jjcarr2 7:0b7897232e93 511 rangeFinderLeft.getMeas(range);
jjcarr2 7:0b7897232e93 512 bt.printf("Range %f \t OldValue %f\n\r",range, usValue);
jjcarr2 7:0b7897232e93 513 if(range > usValue && usValue != 0 && range < 25){
jjcarr2 7:0b7897232e93 514 break;
jjcarr2 7:0b7897232e93 515 } else {
jjcarr2 7:0b7897232e93 516 usValue = range;
jjcarr2 7:0b7897232e93 517 }
jjcarr2 7:0b7897232e93 518 }
jjcarr2 7:0b7897232e93 519
jjcarr2 7:0b7897232e93 520
jjcarr2 7:0b7897232e93 521
jjcarr2 7:0b7897232e93 522
jjcarr2 7:0b7897232e93 523 }
jjcarr2 7:0b7897232e93 524
jjcarr2 2:0351277ee5dd 525
jjcarr2 2:0351277ee5dd 526 void tools_section(float *location, float &current){
jjcarr2 2:0351277ee5dd 527
jjcarr2 7:0b7897232e93 528 //location[0]=wall_follow(LEFT,FORWARD, TOOLS); //location from the left edge of the field
jjcarr2 7:0b7897232e93 529 //Too Far from wall to see gap :(
jjcarr2 7:0b7897232e93 530 wall_follow(LEFT,FORWARD,TOOLS);
jjcarr2 7:0b7897232e93 531
jjcarr2 2:0351277ee5dd 532 current=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
jjcarr2 2:0351277ee5dd 533
jjcarr2 2:0351277ee5dd 534 bt.printf("wavegap %f \t current %f \r\n",location[0],current);
jjcarr2 2:0351277ee5dd 535
jjcarr2 2:0351277ee5dd 536 motors.stopBothMotors();
jjcarr2 2:0351277ee5dd 537 ////////////////////////////////////////// determine shape and pick up tool ///////////////////////////////////////////////////////
jjcarr2 7:0b7897232e93 538 alignWithWall();
jjcarr2 7:0b7897232e93 539 wait_ms(200);
jjcarr2 7:0b7897232e93 540
jjcarr2 7:0b7897232e93 541 rangeFinderLeft.startMeas();
jjcarr2 7:0b7897232e93 542 wait_ms(100);
jjcarr2 7:0b7897232e93 543 rangeFinderLeft.getMeas(range);
jjcarr2 7:0b7897232e93 544 if(range < 10){
jjcarr2 7:0b7897232e93 545
jjcarr2 2:0351277ee5dd 546 wall_follow2(LEFT,BACKWARD,TOOLS);
jjcarr2 7:0b7897232e93 547
jjcarr2 7:0b7897232e93 548 leftEncoder.reset();
jjcarr2 7:0b7897232e93 549 rightEncoder.reset();
jjcarr2 7:0b7897232e93 550 motors.setMotor0Speed(-0.9*MAX_SPEED); //right
jjcarr2 7:0b7897232e93 551 motors.setMotor1Speed(-0.9*MAX_SPEED); //left
jjcarr2 7:0b7897232e93 552 while(abs(leftEncoder.getPulses()*11.12/PPR) < 2);
jjcarr2 7:0b7897232e93 553 motors.stopBothMotors();
jjcarr2 7:0b7897232e93 554
jjcarr2 2:0351277ee5dd 555 // forward
jjcarr2 2:0351277ee5dd 556
jjcarr2 7:0b7897232e93 557 /* leftEncoder.reset();
jjcarr2 2:0351277ee5dd 558 rightEncoder.reset();
jjcarr2 2:0351277ee5dd 559 motors.setMotor0Speed(-MAX_SPEED); //right
jjcarr2 2:0351277ee5dd 560 motors.setMotor1Speed(-MAX_SPEED); //left
jjcarr2 2:0351277ee5dd 561 while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300);
jjcarr2 2:0351277ee5dd 562 motors.stopBothMotors();
jjcarr2 2:0351277ee5dd 563 wait_ms(500);*/
jjcarr2 2:0351277ee5dd 564 current = (abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
jjcarr2 2:0351277ee5dd 565 // REMOVED += because current should just be current pulses?
jjcarr2 7:0b7897232e93 566 /*
jjcarr2 2:0351277ee5dd 567 wall_follow2(LEFT,FORWARD);
jjcarr2 2:0351277ee5dd 568 // backward
jjcarr2 2:0351277ee5dd 569 leftEncoder.reset();
jjcarr2 2:0351277ee5dd 570 rightEncoder.reset();
jjcarr2 2:0351277ee5dd 571 motors.setMotor0Speed(MAX_SPEED); //right
jjcarr2 2:0351277ee5dd 572 motors.setMotor1Speed(MAX_SPEED); //left
jjcarr2 2:0351277ee5dd 573 while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300);
jjcarr2 2:0351277ee5dd 574 motors.stopBothMotors();
jjcarr2 2:0351277ee5dd 575 current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;*/
jjcarr2 7:0b7897232e93 576
jjcarr2 2:0351277ee5dd 577
jjcarr2 2:0351277ee5dd 578 leftTurn();
jjcarr2 2:0351277ee5dd 579
jjcarr2 2:0351277ee5dd 580 //Go over
jjcarr2 2:0351277ee5dd 581 overBump();
jjcarr2 7:0b7897232e93 582 } else {
jjcarr2 7:0b7897232e93 583
jjcarr2 7:0b7897232e93 584 leftTurn();
jjcarr2 7:0b7897232e93 585 //Go over
jjcarr2 7:0b7897232e93 586 overBump();
jjcarr2 7:0b7897232e93 587 }
jjcarr2 2:0351277ee5dd 588
jjcarr2 2:0351277ee5dd 589 }
jjcarr2 2:0351277ee5dd 590