Justin Carr / NavigationTest

Tools Section Navigation Calibrated slightly

Dependencies:   HCSR04 PID PololuQik2 QEI mbed-rtos

Fork of NavigationTest by Paolina Povolotskaya

main.cpp@6:109f46d3cb96, 2014-03-20 (annotated)

Committer:
jjcarr2
Date:
Thu Mar 20 17:21:28 2014 +0000
Revision:
6:109f46d3cb96
Parent:
2:0351277ee5dd
Child:
7:0b7897232e93
Whatever;

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 6:109f46d3cb96 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 6:109f46d3cb96 84
jjcarr2 2:0351277ee5dd 85 motors.stopBothMotors();
jjcarr2 6:109f46d3cb96 86 while(1){}
jjcarr2 2:0351277ee5dd 87 /*
jjcarr2 2:0351277ee5dd 88
jjcarr2 2:0351277ee5dd 89 leftEncoder.reset();
jjcarr2 2:0351277ee5dd 90 rightEncoder.reset();
jjcarr2 2:0351277ee5dd 91 motors.setMotor0Speed(MAX_SPEED); //right
jjcarr2 2:0351277ee5dd 92 motors.setMotor1Speed(MAX_SPEED); //left
jjcarr2 2:0351277ee5dd 93
jjcarr2 2:0351277ee5dd 94 while(leftEncoder.getPulses()/(PPR) < 3);
Fairy_Paolina 1:801f0b9a862a 95 */
jjcarr2 2:0351277ee5dd 96
jjcarr2 2:0351277ee5dd 97 //leftEncoder.reset();
jjcarr2 2:0351277ee5dd 98 //rightEncoder.reset();
jjcarr2 2:0351277ee5dd 99 //motors.setMotor0Speed(MAX_SPEED); //right
jjcarr2 2:0351277ee5dd 100 //motors.setMotor1Speed(MAX_SPEED); //left
jjcarr2 2:0351277ee5dd 101
jjcarr2 2:0351277ee5dd 102 //while((abs(leftEncoder.getPulses())/(PPR) + abs(rightEncoder.getPulses())/(PPR))/2 < 3)
jjcarr2 2:0351277ee5dd 103
jjcarr2 2:0351277ee5dd 104
jjcarr2 2:0351277ee5dd 105
jjcarr2 2:0351277ee5dd 106 //Go to tools
jjcarr2 2:0351277ee5dd 107 tools_section(location, current);
jjcarr2 2:0351277ee5dd 108 bt.printf("Location 0 = %f", location[0]);
Fairy_Paolina 0:ff94cc47fef7 109
jjcarr2 2:0351277ee5dd 110 /*
jjcarr2 2:0351277ee5dd 111 //////////////////////////////// without predefined wavegaps//////////////////////////////////////////////
jjcarr2 2:0351277ee5dd 112 current=0;
jjcarr2 2:0351277ee5dd 113 if(location[0]< 75){
jjcarr2 2:0351277ee5dd 114 turnRight();
jjcarr2 2:0351277ee5dd 115 current=wall_follow(LEFT,FORWARD);
jjcarr2 2:0351277ee5dd 116 if(current == 0)turnLeft();
jjcarr2 2:0351277ee5dd 117 else{
jjcarr2 2:0351277ee5dd 118 direction[0]= RIGHT;
jjcarr2 2:0351277ee5dd 119 turnLeft();
jjcarr2 2:0351277ee5dd 120 overBump();
jjcarr2 2:0351277ee5dd 121 }
jjcarr2 2:0351277ee5dd 122 }
jjcarr2 2:0351277ee5dd 123 else if(location[0]>=75 || current == 0){
jjcarr2 2:0351277ee5dd 124 turnLeft();
jjcarr2 2:0351277ee5dd 125 wall_follow2(RIGHT,FORWARD);
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
jjcarr2 2:0351277ee5dd 133 // left or right
jjcarr2 2:0351277ee5dd 134
jjcarr2 2:0351277ee5dd 135
jjcarr2 2:0351277ee5dd 136 location[1]=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
jjcarr2 2:0351277ee5dd 137
jjcarr2 2:0351277ee5dd 138
jjcarr2 2:0351277ee5dd 139
jjcarr2 2:0351277ee5dd 140
jjcarr2 2:0351277ee5dd 141
jjcarr2 2:0351277ee5dd 142
jjcarr2 2:0351277ee5dd 143
Fairy_Paolina 1:801f0b9a862a 144 leftTurn();
jjcarr2 2:0351277ee5dd 145 //wall_follow2(RIGHT);
Fairy_Paolina 1:801f0b9a862a 146 rightTurn();
Fairy_Paolina 0:ff94cc47fef7 147
jjcarr2 2:0351277ee5dd 148
jjcarr2 2:0351277ee5dd 149
jjcarr2 2:0351277ee5dd 150 bt.printf("LOCATION %f\n\r",location);
jjcarr2 2:0351277ee5dd 151
jjcarr2 2:0351277ee5dd 152 motors.stopBothMotors();
jjcarr2 2:0351277ee5dd 153 // leftTurn();
jjcarr2 2:0351277ee5dd 154 // wait(1);
jjcarr2 2:0351277ee5dd 155 // rightTurn();
jjcarr2 2:0351277ee5dd 156 */
jjcarr2 2:0351277ee5dd 157
Fairy_Paolina 0:ff94cc47fef7 158 }
jjcarr2 2:0351277ee5dd 159
jjcarr2 2:0351277ee5dd 160 void errFunction(void)
jjcarr2 2:0351277ee5dd 161 {
jjcarr2 2:0351277ee5dd 162 //Nothing
Fairy_Paolina 0:ff94cc47fef7 163 }
jjcarr2 2:0351277ee5dd 164
Fairy_Paolina 0:ff94cc47fef7 165 void us_distance(void)
Fairy_Paolina 0:ff94cc47fef7 166 {
jjcarr2 2:0351277ee5dd 167 pc.printf("Ultra Sonic\n\r");
jjcarr2 2:0351277ee5dd 168 rangeFinderLeft.startMeas();
jjcarr2 2:0351277ee5dd 169 wait_us(20);
jjcarr2 2:0351277ee5dd 170 if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID)) {
jjcarr2 2:0351277ee5dd 171 pc.printf("Range = %f\n\r", range);
jjcarr2 2:0351277ee5dd 172 }
Fairy_Paolina 0:ff94cc47fef7 173 }
jjcarr2 2:0351277ee5dd 174
jjcarr2 2:0351277ee5dd 175 float wall_follow(int side, int direction, int section)
Fairy_Paolina 0:ff94cc47fef7 176 {
jjcarr2 2:0351277ee5dd 177 float location, wavegap;
jjcarr2 2:0351277ee5dd 178 int dir=1, set=5;
jjcarr2 2:0351277ee5dd 179
jjcarr2 2:0351277ee5dd 180 pid1.reset();
jjcarr2 2:0351277ee5dd 181
jjcarr2 2:0351277ee5dd 182 if(direction == BACKWARD) dir=-1;
jjcarr2 2:0351277ee5dd 183 if(section == TOOLS)set= 5;
jjcarr2 2:0351277ee5dd 184
jjcarr2 2:0351277ee5dd 185 leftEncoder.reset();
jjcarr2 2:0351277ee5dd 186 rightEncoder.reset();
jjcarr2 2:0351277ee5dd 187
jjcarr2 2:0351277ee5dd 188 location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
jjcarr2 2:0351277ee5dd 189
jjcarr2 2:0351277ee5dd 190 while(location< 75) {
jjcarr2 2:0351277ee5dd 191 location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
jjcarr2 2:0351277ee5dd 192
jjcarr2 2:0351277ee5dd 193 pid1.setInputLimits(0, set);
jjcarr2 2:0351277ee5dd 194 pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
jjcarr2 2:0351277ee5dd 195 pid1.setSetPoint(set);
jjcarr2 2:0351277ee5dd 196 if(side){ //Side = 1 for Left
jjcarr2 2:0351277ee5dd 197 rangeFinderLeft.startMeas();
jjcarr2 2:0351277ee5dd 198 wait_ms(20);
jjcarr2 2:0351277ee5dd 199 rangeFinderLeft.getMeas(range);
jjcarr2 2:0351277ee5dd 200 }
jjcarr2 2:0351277ee5dd 201 else{ //Use right Ultrasonic
jjcarr2 2:0351277ee5dd 202 rangeFinderRight.startMeas();
jjcarr2 2:0351277ee5dd 203 wait_ms(20);
jjcarr2 2:0351277ee5dd 204 rangeFinderRight.getMeas(range);
jjcarr2 2:0351277ee5dd 205 pc.printf("%d\r\n",range);
jjcarr2 2:0351277ee5dd 206 }
jjcarr2 2:0351277ee5dd 207
jjcarr2 2:0351277ee5dd 208 if(range > 20) {
jjcarr2 2:0351277ee5dd 209 wavegap=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
jjcarr2 2:0351277ee5dd 210 bt.printf("wavegap %f\r\n",wavegap);
jjcarr2 2:0351277ee5dd 211 // AT WAVE OPENING!!!!
jjcarr2 2:0351277ee5dd 212 motors.setMotor0Speed(dir*MAX_SPEED);//right
jjcarr2 2:0351277ee5dd 213 motors.setMotor1Speed(dir*MAX_SPEED);//left
jjcarr2 2:0351277ee5dd 214
jjcarr2 2:0351277ee5dd 215 } else {
Fairy_Paolina 0:ff94cc47fef7 216
jjcarr2 2:0351277ee5dd 217 pid1.setProcessValue(range);
jjcarr2 2:0351277ee5dd 218 pid_return = pid1.compute();
jjcarr2 2:0351277ee5dd 219
jjcarr2 2:0351277ee5dd 220 if(pid_return > 0) {
jjcarr2 2:0351277ee5dd 221 if(side){
jjcarr2 2:0351277ee5dd 222 motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
jjcarr2 2:0351277ee5dd 223 motors.setMotor1Speed(dir*MAX_SPEED);//left
jjcarr2 2:0351277ee5dd 224 }
jjcarr2 2:0351277ee5dd 225 else{
jjcarr2 2:0351277ee5dd 226 motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
jjcarr2 2:0351277ee5dd 227 motors.setMotor0Speed(dir*MAX_SPEED);//right
jjcarr2 2:0351277ee5dd 228 }
jjcarr2 2:0351277ee5dd 229 }else if(pid_return < 0) {
jjcarr2 2:0351277ee5dd 230 if(side){
jjcarr2 2:0351277ee5dd 231 motors.setMotor0Speed(dir*MAX_SPEED);//right
jjcarr2 2:0351277ee5dd 232 motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
jjcarr2 2:0351277ee5dd 233 }
jjcarr2 2:0351277ee5dd 234 else{
jjcarr2 2:0351277ee5dd 235 motors.setMotor1Speed(dir*MAX_SPEED);//left
jjcarr2 2:0351277ee5dd 236 motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
jjcarr2 2:0351277ee5dd 237 }
jjcarr2 2:0351277ee5dd 238 }else {
jjcarr2 2:0351277ee5dd 239 motors.setMotor0Speed(dir*MAX_SPEED);//right
jjcarr2 2:0351277ee5dd 240 motors.setMotor1Speed(dir*MAX_SPEED);//left
jjcarr2 2:0351277ee5dd 241 }
jjcarr2 2:0351277ee5dd 242 }}
jjcarr2 2:0351277ee5dd 243 return wavegap;
jjcarr2 2:0351277ee5dd 244 }
jjcarr2 2:0351277ee5dd 245
jjcarr2 2:0351277ee5dd 246 /* MODIFIED WALL_FOLLOW FOR NAVIGATION */
jjcarr2 2:0351277ee5dd 247
jjcarr2 2:0351277ee5dd 248 void wall_follow2(int side, int direction, int section)
jjcarr2 2:0351277ee5dd 249 {
jjcarr2 2:0351277ee5dd 250 int SeeWaveGap = false, count=0, dir=1, set=5;
jjcarr2 2:0351277ee5dd 251
jjcarr2 2:0351277ee5dd 252 pid1.reset();
jjcarr2 2:0351277ee5dd 253
jjcarr2 2:0351277ee5dd 254 if(direction == BACKWARD) dir=-1;
jjcarr2 2:0351277ee5dd 255 if(section == TOOLS)set= 5;
jjcarr2 2:0351277ee5dd 256
jjcarr2 2:0351277ee5dd 257 leftEncoder.reset();
jjcarr2 2:0351277ee5dd 258 rightEncoder.reset();
jjcarr2 2:0351277ee5dd 259
jjcarr2 2:0351277ee5dd 260 while(1) {
jjcarr2 2:0351277ee5dd 261
jjcarr2 2:0351277ee5dd 262 pid1.setInputLimits(0.0, set);
Fairy_Paolina 1:801f0b9a862a 263 pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
jjcarr2 2:0351277ee5dd 264 pid1.setSetPoint(set);
jjcarr2 2:0351277ee5dd 265
jjcarr2 2:0351277ee5dd 266 if(side){
jjcarr2 2:0351277ee5dd 267 rangeFinderLeft.startMeas();
jjcarr2 2:0351277ee5dd 268 wait_ms(20);
jjcarr2 2:0351277ee5dd 269 rangeFinderLeft.getMeas(range);
jjcarr2 2:0351277ee5dd 270 }
jjcarr2 2:0351277ee5dd 271 else{
jjcarr2 2:0351277ee5dd 272 rangeFinderRight.startMeas();
jjcarr2 2:0351277ee5dd 273 wait_ms(20);
jjcarr2 2:0351277ee5dd 274 rangeFinderRight.getMeas(range);
jjcarr2 2:0351277ee5dd 275 }
jjcarr2 2:0351277ee5dd 276
jjcarr2 2:0351277ee5dd 277
jjcarr2 2:0351277ee5dd 278 /*************CHECK FOR WAVE OPENING*****************/
jjcarr2 2:0351277ee5dd 279 /* If after 20 ms the ultrasonic still sees 20+ cm */
jjcarr2 2:0351277ee5dd 280 /* then robot is at wave opening */
jjcarr2 2:0351277ee5dd 281
jjcarr2 2:0351277ee5dd 282 pc.printf("range %f\r\n",range);
jjcarr2 2:0351277ee5dd 283 if(range > 20) {
jjcarr2 2:0351277ee5dd 284 motors.stopBothMotors();
jjcarr2 2:0351277ee5dd 285 bt.printf("wavegap\r\n");
jjcarr2 2:0351277ee5dd 286 // AT WAVE OPENING!!!!
jjcarr2 2:0351277ee5dd 287 break;
jjcarr2 2:0351277ee5dd 288 }
jjcarr2 2:0351277ee5dd 289
Fairy_Paolina 0:ff94cc47fef7 290 pid1.setProcessValue(range);
jjcarr2 2:0351277ee5dd 291 pid_return = pid1.compute();
jjcarr2 2:0351277ee5dd 292 //bt.printf("Range: %f\n PID: %f\r\n", range, pid_return);
jjcarr2 2:0351277ee5dd 293
jjcarr2 2:0351277ee5dd 294 if(pid_return > 0) {
jjcarr2 2:0351277ee5dd 295 if(side){
jjcarr2 2:0351277ee5dd 296 motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
jjcarr2 2:0351277ee5dd 297 motors.setMotor1Speed(dir*MAX_SPEED);//left
jjcarr2 2:0351277ee5dd 298 }
jjcarr2 2:0351277ee5dd 299 else{
jjcarr2 2:0351277ee5dd 300 motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
jjcarr2 2:0351277ee5dd 301 motors.setMotor0Speed(dir*MAX_SPEED);//right
jjcarr2 2:0351277ee5dd 302 }
jjcarr2 2:0351277ee5dd 303 }else if(pid_return < 0) {
jjcarr2 2:0351277ee5dd 304 if(side){
jjcarr2 2:0351277ee5dd 305 motors.setMotor0Speed(dir*MAX_SPEED);//right
jjcarr2 2:0351277ee5dd 306 motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
jjcarr2 2:0351277ee5dd 307 }
jjcarr2 2:0351277ee5dd 308 else{
jjcarr2 2:0351277ee5dd 309 motors.setMotor1Speed(dir*MAX_SPEED);//left
jjcarr2 2:0351277ee5dd 310 motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
jjcarr2 2:0351277ee5dd 311 }
jjcarr2 2:0351277ee5dd 312 } else {
jjcarr2 2:0351277ee5dd 313 motors.setMotor0Speed(dir*MAX_SPEED);
jjcarr2 2:0351277ee5dd 314 motors.setMotor1Speed(dir*MAX_SPEED);
jjcarr2 2:0351277ee5dd 315 }
jjcarr2 2:0351277ee5dd 316 }
jjcarr2 2:0351277ee5dd 317 }
jjcarr2 2:0351277ee5dd 318
jjcarr2 2:0351277ee5dd 319
jjcarr2 2:0351277ee5dd 320 /* MODIFIED WALL_FOLLOW FOR NAVIGATION WITH WAVE OPENINGS PASSED IN */
jjcarr2 2:0351277ee5dd 321 /* MEANT FOR RETURNING FROM OIL RIGS */
jjcarr2 2:0351277ee5dd 322
jjcarr2 2:0351277ee5dd 323 void wall_follow3(int &currentLocation, int &WaveOpening)
jjcarr2 2:0351277ee5dd 324 {
jjcarr2 2:0351277ee5dd 325 while(1) {
jjcarr2 2:0351277ee5dd 326
jjcarr2 2:0351277ee5dd 327
jjcarr2 2:0351277ee5dd 328 pid1.setInputLimits(0, 5);
jjcarr2 2:0351277ee5dd 329 pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
jjcarr2 2:0351277ee5dd 330 pid1.setSetPoint(5.0);
jjcarr2 2:0351277ee5dd 331
jjcarr2 2:0351277ee5dd 332 rangeFinderLeft.startMeas();
jjcarr2 2:0351277ee5dd 333 wait_ms(100);
jjcarr2 2:0351277ee5dd 334 if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID) && (range < 100.0) && (range > 3.0)) {
jjcarr2 2:0351277ee5dd 335 //bt.printf("Range = %f\n", range);
jjcarr2 2:0351277ee5dd 336 }
jjcarr2 2:0351277ee5dd 337
jjcarr2 2:0351277ee5dd 338 /*************CHECK FOR WAVE OPENING*****************/
jjcarr2 2:0351277ee5dd 339 /* If after 100 ms the ultrasonic still sees 20+ cm */
jjcarr2 2:0351277ee5dd 340 /* then robot is at wave opening */
jjcarr2 2:0351277ee5dd 341
jjcarr2 2:0351277ee5dd 342
jjcarr2 2:0351277ee5dd 343 if(range > 20 ) {
jjcarr2 2:0351277ee5dd 344 currentLocation--;
jjcarr2 2:0351277ee5dd 345 }
jjcarr2 2:0351277ee5dd 346
jjcarr2 2:0351277ee5dd 347 if( currentLocation == WaveOpening) {
jjcarr2 2:0351277ee5dd 348 // AT WAVE OPENING!!!!
jjcarr2 2:0351277ee5dd 349
jjcarr2 2:0351277ee5dd 350 break;
jjcarr2 2:0351277ee5dd 351 }
jjcarr2 2:0351277ee5dd 352
jjcarr2 2:0351277ee5dd 353
jjcarr2 2:0351277ee5dd 354 pid1.setProcessValue(range);
jjcarr2 2:0351277ee5dd 355 pid_return = pid1.compute();
jjcarr2 2:0351277ee5dd 356 bt.printf("Range: %f\n PID: %f", range, pid_return);
jjcarr2 2:0351277ee5dd 357
jjcarr2 2:0351277ee5dd 358 if(pid_return > 0) {
jjcarr2 2:0351277ee5dd 359 motors.setMotor0Speed(MAX_SPEED - pid_return);
Fairy_Paolina 0:ff94cc47fef7 360 motors.setMotor1Speed(MAX_SPEED);
jjcarr2 2:0351277ee5dd 361 } else if(pid_return < 0) {
Fairy_Paolina 0:ff94cc47fef7 362 motors.setMotor0Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 363 motors.setMotor1Speed(MAX_SPEED + pid_return);
jjcarr2 2:0351277ee5dd 364 } else {
Fairy_Paolina 0:ff94cc47fef7 365 motors.setMotor0Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 366 motors.setMotor1Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 367 }
jjcarr2 2:0351277ee5dd 368 }
Fairy_Paolina 0:ff94cc47fef7 369 }
jjcarr2 2:0351277ee5dd 370
Fairy_Paolina 1:801f0b9a862a 371 void rightTurn(void)
Fairy_Paolina 1:801f0b9a862a 372 {
jjcarr2 2:0351277ee5dd 373 float speedL, speedR;
jjcarr2 2:0351277ee5dd 374
jjcarr2 2:0351277ee5dd 375 speedL=speedR= 0.4;
jjcarr2 2:0351277ee5dd 376
Fairy_Paolina 1:801f0b9a862a 377 leftEncoder.reset();
jjcarr2 2:0351277ee5dd 378 rightEncoder.reset();
jjcarr2 2:0351277ee5dd 379 motors.setMotor0Speed(-speedR*127);//right
jjcarr2 2:0351277ee5dd 380 motors.setMotor1Speed(speedL*127);//left
jjcarr2 2:0351277ee5dd 381 while(leftEncoder.getPulses()<1050 || rightEncoder.getPulses()>-1050);
Fairy_Paolina 1:801f0b9a862a 382 motors.stopBothMotors();
Fairy_Paolina 1:801f0b9a862a 383 }
jjcarr2 2:0351277ee5dd 384
Fairy_Paolina 1:801f0b9a862a 385 void leftTurn(void)
Fairy_Paolina 1:801f0b9a862a 386 {
Fairy_Paolina 1:801f0b9a862a 387 leftEncoder.reset();
jjcarr2 2:0351277ee5dd 388 rightEncoder.reset();
jjcarr2 2:0351277ee5dd 389 motors.setMotor0Speed(0.4*127);// right
jjcarr2 2:0351277ee5dd 390 motors.setMotor1Speed(-0.4*127);// left
jjcarr2 2:0351277ee5dd 391 while(leftEncoder.getPulses()>-1200 || rightEncoder.getPulses()<1200);
jjcarr2 2:0351277ee5dd 392 motors.stopBothMotors();
jjcarr2 2:0351277ee5dd 393 }
jjcarr2 2:0351277ee5dd 394
jjcarr2 2:0351277ee5dd 395 void overBump(void){
jjcarr2 2:0351277ee5dd 396
jjcarr2 2:0351277ee5dd 397 leftEncoder.reset();
jjcarr2 2:0351277ee5dd 398 rightEncoder.reset();
jjcarr2 2:0351277ee5dd 399 motors.setMotor0Speed(MAX_SPEED); //right
jjcarr2 2:0351277ee5dd 400 motors.setMotor1Speed(MAX_SPEED); //left
jjcarr2 2:0351277ee5dd 401 while(abs(leftEncoder.getPulses()/PPR) + abs(rightEncoder.getPulses()/PPR)/2 < 1);
jjcarr2 2:0351277ee5dd 402 /*
jjcarr2 2:0351277ee5dd 403 leftEncoder.reset();
jjcarr2 2:0351277ee5dd 404 rightEncoder.reset();
jjcarr2 2:0351277ee5dd 405 motors.setMotor0Speed(MAX_SPEED); //right
jjcarr2 2:0351277ee5dd 406 motors.setMotor1Speed(MAX_SPEED); //left
jjcarr2 2:0351277ee5dd 407 while(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR)/2 <3);
jjcarr2 2:0351277ee5dd 408 */
Fairy_Paolina 1:801f0b9a862a 409 motors.stopBothMotors();
Fairy_Paolina 1:801f0b9a862a 410 }
jjcarr2 2:0351277ee5dd 411
jjcarr2 2:0351277ee5dd 412 void tools_section(float *location, float &current){
jjcarr2 2:0351277ee5dd 413
jjcarr2 2:0351277ee5dd 414 location[0]=wall_follow(LEFT,FORWARD, TOOLS); //location from the left edge of the field
jjcarr2 2:0351277ee5dd 415 current=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
jjcarr2 2:0351277ee5dd 416 bt.printf("left Encoder : %d \t right Encoder %d \r\n",leftEncoder.getPulses(),rightEncoder.getPulses());
jjcarr2 2:0351277ee5dd 417
jjcarr2 2:0351277ee5dd 418 bt.printf("wavegap %f \t current %f \r\n",location[0],current);
jjcarr2 2:0351277ee5dd 419
jjcarr2 2:0351277ee5dd 420 motors.stopBothMotors();
jjcarr2 2:0351277ee5dd 421 ////////////////////////////////////////// determine shape and pick up tool ///////////////////////////////////////////////////////
jjcarr2 2:0351277ee5dd 422
jjcarr2 2:0351277ee5dd 423 if(current >location[0]){
jjcarr2 2:0351277ee5dd 424 wall_follow2(LEFT,BACKWARD,TOOLS);
jjcarr2 2:0351277ee5dd 425 wait_ms(100);
jjcarr2 2:0351277ee5dd 426 // forward
jjcarr2 2:0351277ee5dd 427
jjcarr2 2:0351277ee5dd 428 /* leftEncoder.reset();
jjcarr2 2:0351277ee5dd 429 rightEncoder.reset();
jjcarr2 2:0351277ee5dd 430 motors.setMotor0Speed(-MAX_SPEED); //right
jjcarr2 2:0351277ee5dd 431 motors.setMotor1Speed(-MAX_SPEED); //left
jjcarr2 2:0351277ee5dd 432 while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300);
jjcarr2 2:0351277ee5dd 433 motors.stopBothMotors();
jjcarr2 2:0351277ee5dd 434 wait_ms(500);*/
jjcarr2 2:0351277ee5dd 435 current = (abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
jjcarr2 2:0351277ee5dd 436 // REMOVED += because current should just be current pulses?
jjcarr2 2:0351277ee5dd 437 }
jjcarr2 2:0351277ee5dd 438 else{/*
jjcarr2 2:0351277ee5dd 439 wall_follow2(LEFT,FORWARD);
jjcarr2 2:0351277ee5dd 440 // backward
jjcarr2 2:0351277ee5dd 441 leftEncoder.reset();
jjcarr2 2:0351277ee5dd 442 rightEncoder.reset();
jjcarr2 2:0351277ee5dd 443 motors.setMotor0Speed(MAX_SPEED); //right
jjcarr2 2:0351277ee5dd 444 motors.setMotor1Speed(MAX_SPEED); //left
jjcarr2 2:0351277ee5dd 445 while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300);
jjcarr2 2:0351277ee5dd 446 motors.stopBothMotors();
jjcarr2 2:0351277ee5dd 447 current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;*/
jjcarr2 2:0351277ee5dd 448 }
jjcarr2 2:0351277ee5dd 449
jjcarr2 2:0351277ee5dd 450 leftTurn();
jjcarr2 2:0351277ee5dd 451
jjcarr2 2:0351277ee5dd 452 //Go over
jjcarr2 2:0351277ee5dd 453 overBump();
jjcarr2 2:0351277ee5dd 454
jjcarr2 2:0351277ee5dd 455 }
jjcarr2 2:0351277ee5dd 456