Justin Carr / NavigationTest_

Yeah

Dependencies:   HCSR04 PID PololuQik2 QEI Sharp mbed-rtos

Fork of NavigationTest by Paolina Povolotskaya

main.cpp@13:b6480275c445, 2014-04-01 (annotated)

Committer:
jjcarr2
Date:
Tue Apr 01 20:26:15 2014 +0000
Revision:
13:b6480275c445
Parent:
12:081b8fc654af 
slightMove instead of wall_follow;

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 8:11ef93eebe07 9 #include "Sharp.h"
Fairy_Paolina 9:f34700716f1d 10
Fairy_Paolina 2:3d0be48abcf2 11 #define PIN_TRIGGERL (p12)
Fairy_Paolina 2:3d0be48abcf2 12 #define PIN_ECHOL (p11)
Fairy_Paolina 2:3d0be48abcf2 13 #define PIN_TRIGGERR (p29)
Fairy_Paolina 2:3d0be48abcf2 14 #define PIN_ECHOR (p30)
Fairy_Paolina 0:ff94cc47fef7 15 #define PULSE_PER_REV (1192)
Fairy_Paolina 0:ff94cc47fef7 16 #define WHEEL_CIRCUM (12.56637)
Fairy_Paolina 0:ff94cc47fef7 17 #define DIST_PER_PULSE (0.01054225722682)
Fairy_Paolina 0:ff94cc47fef7 18 #define MTRS_TO_INCH (39.3701)
Fairy_Paolina 1:801f0b9a862a 19 #define MAX_SPEED (0.3*127)
Fairy_Paolina 4:f2333e66ec2c 20 #define PPR (4331/4)
Fairy_Paolina 2:3d0be48abcf2 21 #define LEFT (1)
Fairy_Paolina 2:3d0be48abcf2 22 #define RIGHT (0)
jjcarr2 12:081b8fc654af 23 #define STRAIGHT (2)
Fairy_Paolina 3:58726d2e11f0 24 #define FORWARD (1)
Fairy_Paolina 9:f34700716f1d 25 #define BACKWARD (0)
Fairy_Paolina 3:58726d2e11f0 26 #define TOOLS (0)
Fairy_Paolina 3:58726d2e11f0 27 #define MID (1)
Fairy_Paolina 9:f34700716f1d 28 #define RIGS (2)
jjcarr2 13:b6480275c445 29 #define MID2 (3)
jjcarr2 13:b6480275c445 30 #define RETURN (4)
Fairy_Paolina 6:f5c26372b2d0 31 #define FAR (1)
Fairy_Paolina 9:f34700716f1d 32
Fairy_Paolina 9:f34700716f1d 33
Fairy_Paolina 9:f34700716f1d 34 float range, range2, pid_return;
Fairy_Paolina 0:ff94cc47fef7 35 void errFunction(void);
Fairy_Paolina 0:ff94cc47fef7 36 bool cRc;
Fairy_Paolina 9:f34700716f1d 37
Fairy_Paolina 0:ff94cc47fef7 38 //Hardware Initialization
Fairy_Paolina 0:ff94cc47fef7 39 Serial bt(p13,p14);
Fairy_Paolina 0:ff94cc47fef7 40 Serial pc(USBTX,USBRX);
Fairy_Paolina 2:3d0be48abcf2 41 HCSR04 rangeFinderLeft( PIN_TRIGGERL, PIN_ECHOL );
Fairy_Paolina 2:3d0be48abcf2 42 HCSR04 rangeFinderRight( PIN_TRIGGERR, PIN_ECHOR );
Fairy_Paolina 2:3d0be48abcf2 43 PID pid1(15.0,0.0,4.0,0.02);
Fairy_Paolina 0:ff94cc47fef7 44 PololuQik2 motors(p9, p10, p8, p15, errFunction, cRc);
Fairy_Paolina 4:f2333e66ec2c 45 QEI rightEncoder(p17,p18,NC,PPR,QEI::X4_ENCODING);
Fairy_Paolina 4:f2333e66ec2c 46 QEI leftEncoder(p16,p15,NC,PPR,QEI::X4_ENCODING);
Fairy_Paolina 8:11ef93eebe07 47 Sharp IR(p20);
Fairy_Paolina 1:801f0b9a862a 48 //InterruptIn encoder(p29);
Fairy_Paolina 9:f34700716f1d 49
Fairy_Paolina 9:f34700716f1d 50
Fairy_Paolina 0:ff94cc47fef7 51 //Functions
jjcarr2 13:b6480275c445 52 void test();
jjcarr2 13:b6480275c445 53 void wall_follow(int side, int direction, int section, float location);
jjcarr2 13:b6480275c445 54 void wall_follow_to_mid(int side, int direction, int section);
jjcarr2 13:b6480275c445 55 void wall_follow2(int side, int direction, int section, float location, int rig);
Fairy_Paolina 1:801f0b9a862a 56 void leftTurn(void);
Fairy_Paolina 9:f34700716f1d 57 void slightleft(void);
jjcarr2 13:b6480275c445 58 void slightright(void);
Fairy_Paolina 1:801f0b9a862a 59 void rightTurn(void);
jjcarr2 13:b6480275c445 60 void slightMove(int direction, float pulses);
Fairy_Paolina 0:ff94cc47fef7 61 void us_distance(void);
Fairy_Paolina 3:58726d2e11f0 62 void tools_section(float* location, float &current);
Fairy_Paolina 6:f5c26372b2d0 63 void mid_section(float* location, float &current, int* direction);
Fairy_Paolina 6:f5c26372b2d0 64 void mid_section2(float* location, float &current, int* direction);
Fairy_Paolina 6:f5c26372b2d0 65 void rig_section(float* location, float &current, int* direction, int rig);
jjcarr2 12:081b8fc654af 66 void tools_section_return(float* location, float &current);
jjcarr2 12:081b8fc654af 67 void mid_section_return(float* location, float &current, int* direction);
jjcarr2 12:081b8fc654af 68 void mid_section2_return(float* location, float &current, int* direction);
jjcarr2 13:b6480275c445 69 void rig_section_return(float* location, float &current, int* direction);
Fairy_Paolina 8:11ef93eebe07 70 void overBump(int section);
Fairy_Paolina 6:f5c26372b2d0 71 void alignWithWall(int section);
jjcarr2 12:081b8fc654af 72 void UntilWall(int dir);
jjcarr2 12:081b8fc654af 73
Fairy_Paolina 9:f34700716f1d 74
Fairy_Paolina 1:801f0b9a862a 75 //Variables
Fairy_Paolina 9:f34700716f1d 76
Fairy_Paolina 2:3d0be48abcf2 77 int main(void)
Fairy_Paolina 2:3d0be48abcf2 78 {
jjcarr2 13:b6480275c445 79 float location[3], current=4;
Fairy_Paolina 3:58726d2e11f0 80 int direction[3];
Fairy_Paolina 8:11ef93eebe07 81 double distance;
Fairy_Paolina 9:f34700716f1d 82
Fairy_Paolina 0:ff94cc47fef7 83 pc.baud(115200);
jjcarr2 13:b6480275c445 84 pc.baud(115200);
Fairy_Paolina 2:3d0be48abcf2 85 motors.begin();
Fairy_Paolina 9:f34700716f1d 86
jjcarr2 13:b6480275c445 87 pc.printf("START\r\n");
Fairy_Paolina 3:58726d2e11f0 88 //Go to tools
jjcarr2 13:b6480275c445 89 slightMove(FORWARD,3200);
jjcarr2 13:b6480275c445 90
jjcarr2 13:b6480275c445 91 wait(2);
jjcarr2 13:b6480275c445 92 slightMove(FORWARD,3200);
jjcarr2 13:b6480275c445 93
jjcarr2 13:b6480275c445 94 while(1);
Fairy_Paolina 4:f2333e66ec2c 95 tools_section(location, current);
Fairy_Paolina 6:f5c26372b2d0 96 mid_section(location, current, direction);
jjcarr2 13:b6480275c445 97 mid_section2(location, current, direction);
jjcarr2 13:b6480275c445 98 rig_section(location, current, direction, 3);
jjcarr2 13:b6480275c445 99 // fire putting out
jjcarr2 13:b6480275c445 100 wait(2);
jjcarr2 13:b6480275c445 101 //
jjcarr2 13:b6480275c445 102 rig_section_return(location, current, direction);
jjcarr2 13:b6480275c445 103 mid_section2_return(location, current, direction);
jjcarr2 13:b6480275c445 104 mid_section_return(location, current, direction);
jjcarr2 13:b6480275c445 105 tools_section_return(location,current);
Fairy_Paolina 9:f34700716f1d 106
jjcarr2 13:b6480275c445 107 /*while(1) {
jjcarr2 13:b6480275c445 108 pc.printf("IR %f\r\n", IR.getDistance());
jjcarr2 13:b6480275c445 109 /*rangeFinderLeft.startMeas();
jjcarr2 13:b6480275c445 110 rangeFinderRight.startMeas();
jjcarr2 13:b6480275c445 111 wait_ms(38);
jjcarr2 13:b6480275c445 112 rangeFinderLeft.getMeas(range);
jjcarr2 13:b6480275c445 113 rangeFinderRight.getMeas(range);
jjcarr2 13:b6480275c445 114 pc.printf("leftRange = %f\n\r", range);
jjcarr2 13:b6480275c445 115 pc.printf("rightRange = %f\n\r", range);
Fairy_Paolina 9:f34700716f1d 116
jjcarr2 13:b6480275c445 117 wait_ms(200);
jjcarr2 13:b6480275c445 118 }*/
Fairy_Paolina 9:f34700716f1d 119
jjcarr2 13:b6480275c445 120 //wall_follow2(LEFT,FORWARD,MID,0);
Fairy_Paolina 9:f34700716f1d 121 //leftTurn();
Fairy_Paolina 9:f34700716f1d 122 //rightTurn();
Fairy_Paolina 9:f34700716f1d 123
Fairy_Paolina 9:f34700716f1d 124
Fairy_Paolina 0:ff94cc47fef7 125 }
Fairy_Paolina 9:f34700716f1d 126
jjcarr2 13:b6480275c445 127 void test( ){
jjcarr2 13:b6480275c445 128 /*
jjcarr2 13:b6480275c445 129 float set=6, loc=0;
jjcarr2 13:b6480275c445 130 int dir=1;
jjcarr2 13:b6480275c445 131
jjcarr2 13:b6480275c445 132 pid1.reset();
jjcarr2 13:b6480275c445 133
jjcarr2 13:b6480275c445 134 leftEncoder.reset();
jjcarr2 13:b6480275c445 135 rightEncoder.reset();
jjcarr2 13:b6480275c445 136
jjcarr2 13:b6480275c445 137 while(loc < 10) {
jjcarr2 13:b6480275c445 138 loc = (abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
jjcarr2 13:b6480275c445 139
jjcarr2 13:b6480275c445 140 pid1.setInputLimits(0, set);
jjcarr2 13:b6480275c445 141 pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
jjcarr2 13:b6480275c445 142 pid1.setSetPoint(set);
jjcarr2 13:b6480275c445 143 if(side) {
jjcarr2 13:b6480275c445 144 rangeFinderLeft.startMeas();
jjcarr2 13:b6480275c445 145 wait_ms(20);
jjcarr2 13:b6480275c445 146 rangeFinderLeft.getMeas(range);
jjcarr2 13:b6480275c445 147 } else {
jjcarr2 13:b6480275c445 148 rangeFinderRight.startMeas();
jjcarr2 13:b6480275c445 149 wait_ms(20);
jjcarr2 13:b6480275c445 150 rangeFinderRight.getMeas(range);
jjcarr2 13:b6480275c445 151 pc.printf("%d\r\n",range);
jjcarr2 13:b6480275c445 152 }
jjcarr2 13:b6480275c445 153
jjcarr2 13:b6480275c445 154 if(range > 15) {
jjcarr2 13:b6480275c445 155 //pc.printf("wavegap %f\r\n",wavegap);
jjcarr2 13:b6480275c445 156 // AT WAVE OPENING!!!!
jjcarr2 13:b6480275c445 157 motors.setMotor1Speed(dir*0.25*127);//left
jjcarr2 13:b6480275c445 158 motors.setMotor0Speed(dir*0.25*127);//right
jjcarr2 13:b6480275c445 159 } else {
jjcarr2 13:b6480275c445 160
jjcarr2 13:b6480275c445 161 pid1.setProcessValue(range);
jjcarr2 13:b6480275c445 162 pid_return = pid1.compute();
jjcarr2 13:b6480275c445 163
jjcarr2 13:b6480275c445 164 if(pid_return > 0) {
jjcarr2 13:b6480275c445 165 if(side) {
jjcarr2 13:b6480275c445 166 motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
jjcarr2 13:b6480275c445 167 motors.setMotor1Speed(dir*MAX_SPEED);//left
jjcarr2 13:b6480275c445 168 } else {
jjcarr2 13:b6480275c445 169 motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
jjcarr2 13:b6480275c445 170 motors.setMotor0Speed(dir*MAX_SPEED);//right
jjcarr2 13:b6480275c445 171 }
jjcarr2 13:b6480275c445 172 } else if(pid_return < 0) {
jjcarr2 13:b6480275c445 173 if(side) {
jjcarr2 13:b6480275c445 174 motors.setMotor0Speed(dir*MAX_SPEED);//right
jjcarr2 13:b6480275c445 175 motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
jjcarr2 13:b6480275c445 176 } else {
jjcarr2 13:b6480275c445 177 motors.setMotor1Speed(dir*MAX_SPEED);//left
jjcarr2 13:b6480275c445 178 motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
jjcarr2 13:b6480275c445 179 }
jjcarr2 13:b6480275c445 180 } else {
jjcarr2 13:b6480275c445 181 motors.setMotor0Speed(dir*MAX_SPEED);//right
jjcarr2 13:b6480275c445 182 motors.setMotor1Speed(dir*MAX_SPEED);//left
jjcarr2 13:b6480275c445 183 }
jjcarr2 13:b6480275c445 184 }
jjcarr2 13:b6480275c445 185 }
jjcarr2 13:b6480275c445 186
jjcarr2 13:b6480275c445 187 //STOP
jjcarr2 13:b6480275c445 188 motors.setMotor0Speed(dir*-0.3*127); //right
jjcarr2 13:b6480275c445 189 motors.setMotor1Speed(dir*-0.3*127); //left
jjcarr2 13:b6480275c445 190 wait_ms(10);
jjcarr2 13:b6480275c445 191 motors.stopBothMotors(0);
jjcarr2 13:b6480275c445 192 */
jjcarr2 13:b6480275c445 193 }
jjcarr2 13:b6480275c445 194
Fairy_Paolina 2:3d0be48abcf2 195 void errFunction(void)
Fairy_Paolina 2:3d0be48abcf2 196 {
Fairy_Paolina 2:3d0be48abcf2 197 //Nothing
Fairy_Paolina 0:ff94cc47fef7 198 }
Fairy_Paolina 9:f34700716f1d 199
Fairy_Paolina 0:ff94cc47fef7 200 void us_distance(void)
Fairy_Paolina 0:ff94cc47fef7 201 {
Fairy_Paolina 2:3d0be48abcf2 202 pc.printf("Ultra Sonic\n\r");
Fairy_Paolina 2:3d0be48abcf2 203 rangeFinderLeft.startMeas();
Fairy_Paolina 2:3d0be48abcf2 204 wait_us(20);
Fairy_Paolina 2:3d0be48abcf2 205 if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID)) {
Fairy_Paolina 2:3d0be48abcf2 206 pc.printf("Range = %f\n\r", range);
Fairy_Paolina 2:3d0be48abcf2 207 }
Fairy_Paolina 0:ff94cc47fef7 208 }
Fairy_Paolina 9:f34700716f1d 209
jjcarr2 13:b6480275c445 210 void wall_follow(int side, int direction, int section, float location)
Fairy_Paolina 0:ff94cc47fef7 211 {
jjcarr2 13:b6480275c445 212 float set=6, loc=0;
jjcarr2 13:b6480275c445 213 int dir=1;
jjcarr2 13:b6480275c445 214
jjcarr2 13:b6480275c445 215 pid1.reset();
jjcarr2 13:b6480275c445 216
jjcarr2 13:b6480275c445 217 if(direction == BACKWARD) dir=-1;
jjcarr2 13:b6480275c445 218 if(section == TOOLS)set= 10;
jjcarr2 13:b6480275c445 219
jjcarr2 13:b6480275c445 220 leftEncoder.reset();
jjcarr2 13:b6480275c445 221 rightEncoder.reset();
jjcarr2 13:b6480275c445 222
jjcarr2 13:b6480275c445 223 while(loc + location< 66.5) {
jjcarr2 13:b6480275c445 224 loc = (abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
jjcarr2 13:b6480275c445 225
jjcarr2 13:b6480275c445 226 pid1.setInputLimits(0, set);
jjcarr2 13:b6480275c445 227 pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
jjcarr2 13:b6480275c445 228 pid1.setSetPoint(set);
jjcarr2 13:b6480275c445 229 if(side) {
jjcarr2 13:b6480275c445 230 rangeFinderLeft.startMeas();
jjcarr2 13:b6480275c445 231 wait_ms(20);
jjcarr2 13:b6480275c445 232 rangeFinderLeft.getMeas(range);
jjcarr2 13:b6480275c445 233 } else {
jjcarr2 13:b6480275c445 234 rangeFinderRight.startMeas();
jjcarr2 13:b6480275c445 235 wait_ms(20);
jjcarr2 13:b6480275c445 236 rangeFinderRight.getMeas(range);
jjcarr2 13:b6480275c445 237 pc.printf("%d\r\n",range);
jjcarr2 13:b6480275c445 238 }
jjcarr2 13:b6480275c445 239
jjcarr2 13:b6480275c445 240 if(range > 15) {
jjcarr2 13:b6480275c445 241 //pc.printf("wavegap %f\r\n",wavegap);
jjcarr2 13:b6480275c445 242 // AT WAVE OPENING!!!!
jjcarr2 13:b6480275c445 243 motors.setMotor1Speed(dir*0.28*127);//left
jjcarr2 13:b6480275c445 244 motors.setMotor0Speed(dir*0.25*127);//right
jjcarr2 13:b6480275c445 245 } else {
jjcarr2 13:b6480275c445 246
jjcarr2 13:b6480275c445 247 pid1.setProcessValue(range);
jjcarr2 13:b6480275c445 248 pid_return = pid1.compute();
jjcarr2 13:b6480275c445 249
jjcarr2 13:b6480275c445 250 if(pid_return > 0) {
jjcarr2 13:b6480275c445 251 if(side) {
jjcarr2 13:b6480275c445 252 motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
jjcarr2 13:b6480275c445 253 motors.setMotor1Speed(dir*MAX_SPEED);//left
jjcarr2 13:b6480275c445 254 } else {
jjcarr2 13:b6480275c445 255 motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
jjcarr2 13:b6480275c445 256 motors.setMotor0Speed(dir*MAX_SPEED);//right
jjcarr2 13:b6480275c445 257 }
jjcarr2 13:b6480275c445 258 } else if(pid_return < 0) {
jjcarr2 13:b6480275c445 259 if(side) {
jjcarr2 13:b6480275c445 260 motors.setMotor0Speed(dir*MAX_SPEED);//right
jjcarr2 13:b6480275c445 261 motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
jjcarr2 13:b6480275c445 262 } else {
jjcarr2 13:b6480275c445 263 motors.setMotor1Speed(dir*MAX_SPEED);//left
jjcarr2 13:b6480275c445 264 motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
jjcarr2 13:b6480275c445 265 }
jjcarr2 13:b6480275c445 266 } else {
jjcarr2 13:b6480275c445 267 motors.setMotor0Speed(dir*MAX_SPEED);//right
jjcarr2 13:b6480275c445 268 motors.setMotor1Speed(dir*MAX_SPEED);//left
jjcarr2 13:b6480275c445 269 }
jjcarr2 13:b6480275c445 270 }
jjcarr2 13:b6480275c445 271 }
jjcarr2 13:b6480275c445 272
jjcarr2 13:b6480275c445 273 //STOP
jjcarr2 13:b6480275c445 274 motors.setMotor0Speed(dir*-0.3*127); //right
jjcarr2 13:b6480275c445 275 motors.setMotor1Speed(dir*-0.3*127); //left
jjcarr2 13:b6480275c445 276 wait_ms(10);
jjcarr2 13:b6480275c445 277 motors.stopBothMotors(0);
jjcarr2 13:b6480275c445 278 }
jjcarr2 13:b6480275c445 279
jjcarr2 13:b6480275c445 280 void wall_follow_to_mid(int side, int direction, int section)
jjcarr2 13:b6480275c445 281 {
jjcarr2 13:b6480275c445 282 float location, set=6;
Fairy_Paolina 5:70ccef3734ae 283 int dir=1;
Fairy_Paolina 9:f34700716f1d 284
Fairy_Paolina 4:f2333e66ec2c 285 pid1.reset();
Fairy_Paolina 9:f34700716f1d 286
Fairy_Paolina 3:58726d2e11f0 287 if(direction == BACKWARD) dir=-1;
Fairy_Paolina 5:70ccef3734ae 288 if(section == TOOLS)set= 10;
Fairy_Paolina 9:f34700716f1d 289
Fairy_Paolina 3:58726d2e11f0 290 leftEncoder.reset();
Fairy_Paolina 3:58726d2e11f0 291 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 292
Fairy_Paolina 4:f2333e66ec2c 293 location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
Fairy_Paolina 9:f34700716f1d 294
jjcarr2 13:b6480275c445 295 while(location< 34) {
Fairy_Paolina 4:f2333e66ec2c 296 location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
Fairy_Paolina 9:f34700716f1d 297
Fairy_Paolina 3:58726d2e11f0 298 pid1.setInputLimits(0, set);
Fairy_Paolina 1:801f0b9a862a 299 pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
Fairy_Paolina 3:58726d2e11f0 300 pid1.setSetPoint(set);
Fairy_Paolina 9:f34700716f1d 301 if(side) {
Fairy_Paolina 2:3d0be48abcf2 302 rangeFinderLeft.startMeas();
jjcarr2 13:b6480275c445 303 wait_ms(20);
Fairy_Paolina 2:3d0be48abcf2 304 rangeFinderLeft.getMeas(range);
Fairy_Paolina 9:f34700716f1d 305 } else {
Fairy_Paolina 2:3d0be48abcf2 306 rangeFinderRight.startMeas();
jjcarr2 13:b6480275c445 307 wait_ms(20);
Fairy_Paolina 2:3d0be48abcf2 308 rangeFinderRight.getMeas(range);
Fairy_Paolina 2:3d0be48abcf2 309 pc.printf("%d\r\n",range);
Fairy_Paolina 2:3d0be48abcf2 310 }
Fairy_Paolina 9:f34700716f1d 311
jjcarr2 13:b6480275c445 312 if(range > 15) {
jjcarr2 13:b6480275c445 313 //pc.printf("wavegap %f\r\n",wavegap);
Fairy_Paolina 3:58726d2e11f0 314 // AT WAVE OPENING!!!!
jjcarr2 13:b6480275c445 315 motors.setMotor1Speed(dir*0.28*127);//left
jjcarr2 13:b6480275c445 316 motors.setMotor0Speed(dir*0.25*127);//right
Fairy_Paolina 9:f34700716f1d 317 } else {
Fairy_Paolina 9:f34700716f1d 318
Fairy_Paolina 6:f5c26372b2d0 319 pid1.setProcessValue(range);
Fairy_Paolina 6:f5c26372b2d0 320 pid_return = pid1.compute();
Fairy_Paolina 9:f34700716f1d 321
Fairy_Paolina 6:f5c26372b2d0 322 if(pid_return > 0) {
Fairy_Paolina 9:f34700716f1d 323 if(side) {
Fairy_Paolina 6:f5c26372b2d0 324 motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
Fairy_Paolina 6:f5c26372b2d0 325 motors.setMotor1Speed(dir*MAX_SPEED);//left
Fairy_Paolina 9:f34700716f1d 326 } else {
Fairy_Paolina 6:f5c26372b2d0 327 motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
Fairy_Paolina 6:f5c26372b2d0 328 motors.setMotor0Speed(dir*MAX_SPEED);//right
Fairy_Paolina 6:f5c26372b2d0 329 }
Fairy_Paolina 9:f34700716f1d 330 } else if(pid_return < 0) {
Fairy_Paolina 9:f34700716f1d 331 if(side) {
Fairy_Paolina 6:f5c26372b2d0 332 motors.setMotor0Speed(dir*MAX_SPEED);//right
Fairy_Paolina 6:f5c26372b2d0 333 motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
Fairy_Paolina 9:f34700716f1d 334 } else {
Fairy_Paolina 6:f5c26372b2d0 335 motors.setMotor1Speed(dir*MAX_SPEED);//left
Fairy_Paolina 6:f5c26372b2d0 336 motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
Fairy_Paolina 6:f5c26372b2d0 337 }
Fairy_Paolina 9:f34700716f1d 338 } else {
Fairy_Paolina 6:f5c26372b2d0 339 motors.setMotor0Speed(dir*MAX_SPEED);//right
Fairy_Paolina 3:58726d2e11f0 340 motors.setMotor1Speed(dir*MAX_SPEED);//left
Fairy_Paolina 2:3d0be48abcf2 341 }
Fairy_Paolina 0:ff94cc47fef7 342 }
Fairy_Paolina 2:3d0be48abcf2 343 }
jjcarr2 13:b6480275c445 344
jjcarr2 13:b6480275c445 345 //STOP
jjcarr2 13:b6480275c445 346 motors.setMotor0Speed(dir*-0.3*127); //right
jjcarr2 13:b6480275c445 347 motors.setMotor1Speed(dir*-0.3*127); //left
jjcarr2 13:b6480275c445 348 wait_ms(10);
jjcarr2 13:b6480275c445 349 motors.stopBothMotors(0);
Fairy_Paolina 0:ff94cc47fef7 350 }
Fairy_Paolina 9:f34700716f1d 351
Fairy_Paolina 0:ff94cc47fef7 352 /* MODIFIED WALL_FOLLOW FOR NAVIGATION */
Fairy_Paolina 9:f34700716f1d 353
jjcarr2 13:b6480275c445 354 void wall_follow2(int side, int direction, int section, float location, int rig)
Fairy_Paolina 0:ff94cc47fef7 355 {
jjcarr2 13:b6480275c445 356 int dir=1, limit=80, lowlim=5;
jjcarr2 13:b6480275c445 357 float set=6, loc=0, Rigloc=0;
jjcarr2 13:b6480275c445 358 bool SeeWaveGap = false;
jjcarr2 13:b6480275c445 359
jjcarr2 13:b6480275c445 360 if(rig == 1) Rigloc= 16;
jjcarr2 13:b6480275c445 361 else if(rig == 2) Rigloc= 45;
jjcarr2 13:b6480275c445 362 else if(rig== 3) Rigloc = 70;
Fairy_Paolina 9:f34700716f1d 363
Fairy_Paolina 4:f2333e66ec2c 364 pid1.reset();
Fairy_Paolina 9:f34700716f1d 365
jjcarr2 13:b6480275c445 366 if(direction == BACKWARD) {
jjcarr2 13:b6480275c445 367 dir=-1;
jjcarr2 13:b6480275c445 368 limit = 100;
jjcarr2 13:b6480275c445 369 }
jjcarr2 12:081b8fc654af 370 if(section == TOOLS) {
jjcarr2 13:b6480275c445 371 set= 6;
jjcarr2 13:b6480275c445 372 limit = 86;
jjcarr2 11:12ce7600f2f9 373 }
jjcarr2 13:b6480275c445 374 if(section == RETURN) lowlim=15;
jjcarr2 12:081b8fc654af 375
Fairy_Paolina 3:58726d2e11f0 376 leftEncoder.reset();
Fairy_Paolina 3:58726d2e11f0 377 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 378
jjcarr2 13:b6480275c445 379 //pc.printf("before %f\r\n", location);
jjcarr2 13:b6480275c445 380
jjcarr2 13:b6480275c445 381 pc.printf("dir*loc+location %f\r\n",dir*loc + location );
jjcarr2 13:b6480275c445 382 pc.printf("limit %d \r\n", limit);
jjcarr2 13:b6480275c445 383
jjcarr2 13:b6480275c445 384 while((dir*loc + location <= limit) && (dir*loc + location >= lowlim)) {
jjcarr2 13:b6480275c445 385
Fairy_Paolina 7:78745a518957 386 loc=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
jjcarr2 13:b6480275c445 387 //pc.printf("loc %f \r\n", loc);
Fairy_Paolina 9:f34700716f1d 388
Fairy_Paolina 5:70ccef3734ae 389 pid1.setInputLimits(0.0, set);
Fairy_Paolina 1:801f0b9a862a 390 pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
Fairy_Paolina 5:70ccef3734ae 391 pid1.setSetPoint(set);
Fairy_Paolina 9:f34700716f1d 392
Fairy_Paolina 9:f34700716f1d 393 if(side) {
Fairy_Paolina 2:3d0be48abcf2 394 rangeFinderLeft.startMeas();
jjcarr2 13:b6480275c445 395 wait_ms(20);
Fairy_Paolina 2:3d0be48abcf2 396 rangeFinderLeft.getMeas(range);
Fairy_Paolina 9:f34700716f1d 397 } else {
Fairy_Paolina 2:3d0be48abcf2 398 rangeFinderRight.startMeas();
jjcarr2 13:b6480275c445 399 wait_ms(20);
Fairy_Paolina 2:3d0be48abcf2 400 rangeFinderRight.getMeas(range);
Fairy_Paolina 2:3d0be48abcf2 401 }
Fairy_Paolina 9:f34700716f1d 402
jjcarr2 12:081b8fc654af 403 if(section == RIGS) {
jjcarr2 13:b6480275c445 404 rangeFinderLeft.startMeas();
jjcarr2 13:b6480275c445 405 wait_ms(20);
jjcarr2 13:b6480275c445 406 rangeFinderLeft.getMeas(range2);
jjcarr2 13:b6480275c445 407
jjcarr2 13:b6480275c445 408 if(range2< 20) {
jjcarr2 13:b6480275c445 409 if( abs(dir*loc + location - Rigloc) < 10) {
jjcarr2 13:b6480275c445 410 //STOP
jjcarr2 13:b6480275c445 411 motors.setMotor0Speed(dir*-0.25*127); //right
jjcarr2 13:b6480275c445 412 motors.setMotor1Speed(dir*-0.25*127); //left
jjcarr2 13:b6480275c445 413 wait_ms(5);
jjcarr2 13:b6480275c445 414 motors.stopBothMotors(0);
jjcarr2 13:b6480275c445 415 break;
jjcarr2 13:b6480275c445 416 }
jjcarr2 12:081b8fc654af 417 }
jjcarr2 12:081b8fc654af 418 }
jjcarr2 12:081b8fc654af 419
Fairy_Paolina 9:f34700716f1d 420
jjcarr2 13:b6480275c445 421 //pc.printf("wall follow 2 range %f\r\n",range);
jjcarr2 13:b6480275c445 422 //pc.printf("loc+location = %f\r\n", loc+location);
jjcarr2 13:b6480275c445 423 if(range > 20 ) {
jjcarr2 13:b6480275c445 424 if(section == RIGS || section == RETURN) {
jjcarr2 13:b6480275c445 425 motors.setMotor0Speed(dir*0.25*127); //right
jjcarr2 13:b6480275c445 426 motors.setMotor1Speed(dir*0.25*127); //left
jjcarr2 13:b6480275c445 427 } else {
jjcarr2 13:b6480275c445 428 if(!SeeWaveGap) {
jjcarr2 13:b6480275c445 429 SeeWaveGap=true;
jjcarr2 13:b6480275c445 430 } else {
jjcarr2 13:b6480275c445 431 //STOP
jjcarr2 13:b6480275c445 432 motors.setMotor0Speed(dir*-0.25*127); //right
jjcarr2 13:b6480275c445 433 motors.setMotor1Speed(dir*-0.25*127); //left
jjcarr2 13:b6480275c445 434 wait_ms(5);
jjcarr2 13:b6480275c445 435 motors.stopBothMotors(0);
jjcarr2 13:b6480275c445 436
jjcarr2 13:b6480275c445 437 pc.printf("wavegap\r\n");
jjcarr2 13:b6480275c445 438 // AT WAVE OPENING!!!!
jjcarr2 13:b6480275c445 439 break;
jjcarr2 13:b6480275c445 440 }
jjcarr2 13:b6480275c445 441 }
jjcarr2 11:12ce7600f2f9 442 } else {
jjcarr2 13:b6480275c445 443 SeeWaveGap = false;
jjcarr2 12:081b8fc654af 444 pid1.setProcessValue(range);
jjcarr2 12:081b8fc654af 445 pid_return = pid1.compute();
jjcarr2 13:b6480275c445 446 //pc.printf("Range: %f\n PID: %f\r\n", range, pid_return);
Fairy_Paolina 9:f34700716f1d 447
jjcarr2 12:081b8fc654af 448 if(pid_return > 0) {
jjcarr2 12:081b8fc654af 449 if(side) {
jjcarr2 12:081b8fc654af 450 motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
jjcarr2 12:081b8fc654af 451 motors.setMotor1Speed(dir*MAX_SPEED);//left
jjcarr2 12:081b8fc654af 452 } else {
jjcarr2 12:081b8fc654af 453 motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
jjcarr2 12:081b8fc654af 454 motors.setMotor0Speed(dir*MAX_SPEED);//right
jjcarr2 12:081b8fc654af 455 }
jjcarr2 12:081b8fc654af 456 } else if(pid_return < 0) {
jjcarr2 12:081b8fc654af 457 if(side) {
jjcarr2 12:081b8fc654af 458 motors.setMotor0Speed(dir*MAX_SPEED);//right
jjcarr2 12:081b8fc654af 459 motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
jjcarr2 12:081b8fc654af 460 } else {
jjcarr2 12:081b8fc654af 461 motors.setMotor1Speed(dir*MAX_SPEED);//left
jjcarr2 12:081b8fc654af 462 motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
jjcarr2 12:081b8fc654af 463 }
Fairy_Paolina 9:f34700716f1d 464 } else {
jjcarr2 12:081b8fc654af 465 motors.setMotor0Speed(dir*MAX_SPEED);
jjcarr2 12:081b8fc654af 466 motors.setMotor1Speed(dir*MAX_SPEED);
Fairy_Paolina 2:3d0be48abcf2 467 }
Fairy_Paolina 0:ff94cc47fef7 468 }
jjcarr2 12:081b8fc654af 469 }
jjcarr2 13:b6480275c445 470
jjcarr2 13:b6480275c445 471 //STOP
jjcarr2 13:b6480275c445 472 motors.setMotor0Speed(dir*-0.3*127); //right
jjcarr2 13:b6480275c445 473 motors.setMotor1Speed(dir*-0.3*127); //left
jjcarr2 13:b6480275c445 474 wait_ms(5);
jjcarr2 13:b6480275c445 475 motors.stopBothMotors(0);
Fairy_Paolina 0:ff94cc47fef7 476 }
Fairy_Paolina 9:f34700716f1d 477
Fairy_Paolina 9:f34700716f1d 478
Fairy_Paolina 9:f34700716f1d 479 void alignWithWall(int section)
Fairy_Paolina 9:f34700716f1d 480 {
Fairy_Paolina 6:f5c26372b2d0 481 float usValue = 0;
Fairy_Paolina 9:f34700716f1d 482
Fairy_Paolina 9:f34700716f1d 483 if(section == TOOLS) {
jjcarr2 13:b6480275c445 484 pc.printf("tools section align\r\n");
Fairy_Paolina 6:f5c26372b2d0 485 // turn at an angle
Fairy_Paolina 6:f5c26372b2d0 486 leftEncoder.reset();
Fairy_Paolina 9:f34700716f1d 487 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 488 motors.setMotor0Speed(-1.2*MAX_SPEED); //right
Fairy_Paolina 6:f5c26372b2d0 489 motors.setMotor1Speed(0.4*MAX_SPEED); //left
Fairy_Paolina 6:f5c26372b2d0 490 while(rightEncoder.getPulses()>-1000);
jjcarr2 13:b6480275c445 491 motors.stopBothMotors(0);
Fairy_Paolina 9:f34700716f1d 492
Fairy_Paolina 6:f5c26372b2d0 493 //go backwards toward wall
Fairy_Paolina 6:f5c26372b2d0 494 leftEncoder.reset();
Fairy_Paolina 9:f34700716f1d 495 rightEncoder.reset();
jjcarr2 13:b6480275c445 496 motors.setMotor0Speed(-0.25*127); //right
jjcarr2 13:b6480275c445 497 motors.setMotor1Speed(-0.25*127); //left
Fairy_Paolina 7:78745a518957 498 while(abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300);
jjcarr2 13:b6480275c445 499 motors.stopBothMotors(0);
Fairy_Paolina 9:f34700716f1d 500
Fairy_Paolina 9:f34700716f1d 501 // turn left towards wall
Fairy_Paolina 6:f5c26372b2d0 502 leftEncoder.reset();
Fairy_Paolina 6:f5c26372b2d0 503 rightEncoder.reset();
Fairy_Paolina 6:f5c26372b2d0 504 motors.setMotor0Speed(MAX_SPEED); //right
Fairy_Paolina 6:f5c26372b2d0 505 motors.setMotor1Speed(-MAX_SPEED); //left
jjcarr2 13:b6480275c445 506 while(rightEncoder.getPulses() < 20 || abs(leftEncoder.getPulses()) < 20);
jjcarr2 13:b6480275c445 507
jjcarr2 13:b6480275c445 508 motors.stopBothMotors(0);
jjcarr2 13:b6480275c445 509
jjcarr2 13:b6480275c445 510 // turning left
jjcarr2 13:b6480275c445 511 motors.setMotor0Speed(0.9*MAX_SPEED); //right
jjcarr2 13:b6480275c445 512 motors.setMotor1Speed(-0.9*MAX_SPEED); //left
Fairy_Paolina 9:f34700716f1d 513
jjcarr2 13:b6480275c445 514 } else if( section == RIGS) {
jjcarr2 13:b6480275c445 515 // check distance to wall
jjcarr2 13:b6480275c445 516 rangeFinderRight.startMeas();
jjcarr2 13:b6480275c445 517 wait_ms(20);
jjcarr2 13:b6480275c445 518 rangeFinderRight.getMeas(range);
jjcarr2 13:b6480275c445 519
jjcarr2 13:b6480275c445 520 if(range < 4 || range > 20) return;
jjcarr2 13:b6480275c445 521
jjcarr2 13:b6480275c445 522 // turn at an angle
jjcarr2 13:b6480275c445 523 leftEncoder.reset();
jjcarr2 13:b6480275c445 524 rightEncoder.reset();
jjcarr2 13:b6480275c445 525 motors.setMotor1Speed(-1.2*MAX_SPEED); //left
jjcarr2 13:b6480275c445 526 motors.setMotor0Speed(0.4*MAX_SPEED); //right
jjcarr2 13:b6480275c445 527 while(abs(leftEncoder.getPulses())<1000);
jjcarr2 13:b6480275c445 528 motors.stopBothMotors(0);
Fairy_Paolina 9:f34700716f1d 529
jjcarr2 13:b6480275c445 530 //go backwards toward wall
jjcarr2 13:b6480275c445 531 leftEncoder.reset();
jjcarr2 13:b6480275c445 532 rightEncoder.reset();
jjcarr2 13:b6480275c445 533 motors.setMotor0Speed(-0.25*127); //right
jjcarr2 13:b6480275c445 534 motors.setMotor1Speed(-0.25*127); //left
jjcarr2 13:b6480275c445 535 while(abs(leftEncoder.getPulses()) < 150 || abs(rightEncoder.getPulses()) < 150);
jjcarr2 13:b6480275c445 536 motors.stopBothMotors(0);
jjcarr2 13:b6480275c445 537
jjcarr2 13:b6480275c445 538 // turn left towards wall
jjcarr2 13:b6480275c445 539 leftEncoder.reset();
jjcarr2 13:b6480275c445 540 rightEncoder.reset();
jjcarr2 13:b6480275c445 541 motors.setMotor0Speed(-MAX_SPEED); //right
jjcarr2 13:b6480275c445 542 motors.setMotor1Speed(MAX_SPEED); //left
jjcarr2 13:b6480275c445 543 while(abs(rightEncoder.getPulses()) < 20 || abs(leftEncoder.getPulses()) < 20);
jjcarr2 13:b6480275c445 544
jjcarr2 13:b6480275c445 545 motors.stopBothMotors(0);
jjcarr2 13:b6480275c445 546
jjcarr2 13:b6480275c445 547 // turning left
jjcarr2 13:b6480275c445 548 motors.setMotor0Speed(-0.9*MAX_SPEED); //right
jjcarr2 13:b6480275c445 549 motors.setMotor1Speed(0.9*MAX_SPEED); //left
Fairy_Paolina 7:78745a518957 550 } else {
jjcarr2 13:b6480275c445 551 pc.printf("in mid section align\r\n");
jjcarr2 13:b6480275c445 552 // turn right towards wall
Fairy_Paolina 8:11ef93eebe07 553 rightTurn();
jjcarr2 13:b6480275c445 554 // turning left towards wall
jjcarr2 13:b6480275c445 555 motors.setMotor0Speed(0.9*MAX_SPEED); //right
jjcarr2 13:b6480275c445 556 motors.setMotor1Speed(-0.9*MAX_SPEED); //left
Fairy_Paolina 6:f5c26372b2d0 557 }
Fairy_Paolina 9:f34700716f1d 558
Fairy_Paolina 6:f5c26372b2d0 559 usValue = 0;
Fairy_Paolina 9:f34700716f1d 560 while(1) {
jjcarr2 13:b6480275c445 561 if(section == RIGS) {
jjcarr2 13:b6480275c445 562 rangeFinderRight.startMeas();
jjcarr2 13:b6480275c445 563 wait_ms(20);
jjcarr2 13:b6480275c445 564 rangeFinderRight.getMeas(range);
jjcarr2 13:b6480275c445 565 } else {
jjcarr2 13:b6480275c445 566 rangeFinderLeft.startMeas();
jjcarr2 13:b6480275c445 567 wait_ms(20);
jjcarr2 13:b6480275c445 568 rangeFinderLeft.getMeas(range);
jjcarr2 13:b6480275c445 569 }
jjcarr2 13:b6480275c445 570 pc.printf("Range %f \t OldValue %f\n\r",range, usValue);
Fairy_Paolina 9:f34700716f1d 571 if(range > usValue && usValue != 0 && range < 25) {
Fairy_Paolina 0:ff94cc47fef7 572 break;
Fairy_Paolina 2:3d0be48abcf2 573 } else {
Fairy_Paolina 9:f34700716f1d 574 usValue = range;
Fairy_Paolina 0:ff94cc47fef7 575 }
Fairy_Paolina 2:3d0be48abcf2 576 }
jjcarr2 13:b6480275c445 577 motors.stopBothMotors(0);
Fairy_Paolina 5:70ccef3734ae 578 }
Fairy_Paolina 9:f34700716f1d 579
Fairy_Paolina 1:801f0b9a862a 580 void rightTurn(void)
Fairy_Paolina 1:801f0b9a862a 581 {
Fairy_Paolina 9:f34700716f1d 582 motors.begin();
Fairy_Paolina 1:801f0b9a862a 583 leftEncoder.reset();
Fairy_Paolina 2:3d0be48abcf2 584 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 585 motors.setMotor0Speed(-0.5*127);//right
Fairy_Paolina 9:f34700716f1d 586 motors.setMotor1Speed(0.5*127);//left
jjcarr2 13:b6480275c445 587 while(abs(leftEncoder.getPulses())<950 || abs(rightEncoder.getPulses())<950);
jjcarr2 13:b6480275c445 588 motors.stopBothMotors(0);
Fairy_Paolina 1:801f0b9a862a 589 }
Fairy_Paolina 9:f34700716f1d 590
Fairy_Paolina 1:801f0b9a862a 591 void leftTurn(void)
Fairy_Paolina 9:f34700716f1d 592 {
Fairy_Paolina 9:f34700716f1d 593 motors.begin();
Fairy_Paolina 9:f34700716f1d 594 leftEncoder.reset();
Fairy_Paolina 9:f34700716f1d 595 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 596 motors.setMotor0Speed(0.5*127);// right
Fairy_Paolina 9:f34700716f1d 597 motors.setMotor1Speed(-0.5*127);// left
jjcarr2 11:12ce7600f2f9 598 while(abs(leftEncoder.getPulses())<1100 || rightEncoder.getPulses()<1100);
jjcarr2 13:b6480275c445 599 motors.stopBothMotors(0);
Fairy_Paolina 5:70ccef3734ae 600 }
jjcarr2 11:12ce7600f2f9 601
jjcarr2 12:081b8fc654af 602 void slightleft(void)
jjcarr2 12:081b8fc654af 603 {
jjcarr2 12:081b8fc654af 604
Fairy_Paolina 9:f34700716f1d 605 leftEncoder.reset();
Fairy_Paolina 9:f34700716f1d 606 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 607 motors.setMotor0Speed(0.5*127);// right
Fairy_Paolina 9:f34700716f1d 608 motors.setMotor1Speed(-0.5*127);// left
jjcarr2 13:b6480275c445 609 while(abs(leftEncoder.getPulses())<90 || rightEncoder.getPulses()<90);
jjcarr2 13:b6480275c445 610 motors.stopBothMotors(0);
jjcarr2 13:b6480275c445 611 }
jjcarr2 13:b6480275c445 612
jjcarr2 13:b6480275c445 613 void slightright(void)
jjcarr2 13:b6480275c445 614 {
jjcarr2 13:b6480275c445 615
jjcarr2 13:b6480275c445 616 leftEncoder.reset();
jjcarr2 13:b6480275c445 617 rightEncoder.reset();
jjcarr2 13:b6480275c445 618 motors.setMotor0Speed(-0.4*127);// right
jjcarr2 13:b6480275c445 619 motors.setMotor1Speed(0.4*127);// left
jjcarr2 13:b6480275c445 620 while(abs(leftEncoder.getPulses())<90 || abs(rightEncoder.getPulses())<90);
jjcarr2 13:b6480275c445 621 motors.stopBothMotors(0);
jjcarr2 13:b6480275c445 622 }
jjcarr2 13:b6480275c445 623
jjcarr2 13:b6480275c445 624 void slightMove(int direction, float pulses)
jjcarr2 13:b6480275c445 625 {
jjcarr2 13:b6480275c445 626 int dir=1;
jjcarr2 13:b6480275c445 627
jjcarr2 13:b6480275c445 628 if(direction == BACKWARD) dir= -1;
jjcarr2 13:b6480275c445 629
jjcarr2 13:b6480275c445 630 leftEncoder.reset();
jjcarr2 13:b6480275c445 631 rightEncoder.reset();
jjcarr2 13:b6480275c445 632 motors.setMotor0Speed(dir*0.22*127); //right
jjcarr2 13:b6480275c445 633 motors.setMotor1Speed(dir*0.25*127); //left
jjcarr2 13:b6480275c445 634 while(abs(leftEncoder.getPulses()) < pulses || abs(rightEncoder.getPulses()) < pulses);
jjcarr2 13:b6480275c445 635 /*
jjcarr2 13:b6480275c445 636 motors.setMotor0Speed(dir*-0.25*127); //right
jjcarr2 13:b6480275c445 637 motors.setMotor1Speed(dir*-0.25*127); //left
jjcarr2 13:b6480275c445 638 wait_ms(10);
jjcarr2 13:b6480275c445 639 */
jjcarr2 13:b6480275c445 640 motors.stopBothMotors(127);
jjcarr2 13:b6480275c445 641
Fairy_Paolina 9:f34700716f1d 642 }
Fairy_Paolina 9:f34700716f1d 643
jjcarr2 12:081b8fc654af 644 void UntilWall(int dir)
jjcarr2 12:081b8fc654af 645 {
jjcarr2 12:081b8fc654af 646
jjcarr2 12:081b8fc654af 647 if(dir == BACKWARD) dir=-1;
jjcarr2 12:081b8fc654af 648
jjcarr2 12:081b8fc654af 649 leftEncoder.reset();
jjcarr2 12:081b8fc654af 650 rightEncoder.reset();
jjcarr2 12:081b8fc654af 651 motors.setMotor0Speed(dir*0.2*127); //right
jjcarr2 12:081b8fc654af 652 motors.setMotor1Speed(dir*0.2*127); //left
jjcarr2 12:081b8fc654af 653
jjcarr2 12:081b8fc654af 654 range = 30;
jjcarr2 12:081b8fc654af 655
jjcarr2 12:081b8fc654af 656 while(range > 20) {
jjcarr2 12:081b8fc654af 657 rangeFinderRight.startMeas();
jjcarr2 12:081b8fc654af 658 wait_ms(20);
jjcarr2 12:081b8fc654af 659 rangeFinderRight.getMeas(range);
jjcarr2 12:081b8fc654af 660 }
jjcarr2 12:081b8fc654af 661
jjcarr2 12:081b8fc654af 662 motors.setMotor0Speed(dir*-0.2*127); //right
jjcarr2 12:081b8fc654af 663 motors.setMotor1Speed(dir*-0.2*127); //left
jjcarr2 12:081b8fc654af 664 wait_ms(5);
jjcarr2 13:b6480275c445 665 motors.stopBothMotors(0);
jjcarr2 12:081b8fc654af 666 }
Fairy_Paolina 9:f34700716f1d 667
Fairy_Paolina 9:f34700716f1d 668 void overBump(int section)
Fairy_Paolina 9:f34700716f1d 669 {
Fairy_Paolina 9:f34700716f1d 670 int preLeft=5000, preRight=5000, out=0;
jjcarr2 12:081b8fc654af 671
Fairy_Paolina 9:f34700716f1d 672 motors.begin();
jjcarr2 13:b6480275c445 673 // slight backwards
Fairy_Paolina 3:58726d2e11f0 674 leftEncoder.reset();
Fairy_Paolina 3:58726d2e11f0 675 rightEncoder.reset();
jjcarr2 13:b6480275c445 676 motors.setMotor0Speed(-0.25*127); //right
jjcarr2 13:b6480275c445 677 motors.setMotor1Speed(-0.25*127); //left
jjcarr2 13:b6480275c445 678 while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses()) < 50);
jjcarr2 13:b6480275c445 679 motors.stopBothMotors(0);
jjcarr2 12:081b8fc654af 680
jjcarr2 13:b6480275c445 681 pc.printf("slight backwards\r\n");
jjcarr2 13:b6480275c445 682 wait_ms(200);
jjcarr2 12:081b8fc654af 683
Fairy_Paolina 8:11ef93eebe07 684 leftEncoder.reset();
Fairy_Paolina 8:11ef93eebe07 685 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 686 motors.setMotor0Speed(0.3*127); //right
Fairy_Paolina 9:f34700716f1d 687 motors.setMotor1Speed(0.3*127); //left
jjcarr2 13:b6480275c445 688 while((abs(leftEncoder.getPulses()) < 800 || abs(rightEncoder.getPulses()) < 800) /*&& preLeft!=0*/ && IR.getDistance() >15 ) {
jjcarr2 13:b6480275c445 689 /*preLeft=leftEncoder.getPulses();
Fairy_Paolina 6:f5c26372b2d0 690 preRight=rightEncoder.getPulses();
jjcarr2 13:b6480275c445 691 wait_ms(200);
jjcarr2 13:b6480275c445 692 if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;*/
Fairy_Paolina 8:11ef93eebe07 693 }
Fairy_Paolina 9:f34700716f1d 694
jjcarr2 13:b6480275c445 695 pc.printf("forward \r\n");
jjcarr2 13:b6480275c445 696 wait_ms(200);
jjcarr2 13:b6480275c445 697 /*
jjcarr2 13:b6480275c445 698 motors.stopBothMotors(0);
jjcarr2 13:b6480275c445 699 motors.begin();
jjcarr2 13:b6480275c445 700
jjcarr2 13:b6480275c445 701 leftEncoder.reset();
jjcarr2 13:b6480275c445 702 rightEncoder.reset();
jjcarr2 13:b6480275c445 703 motors.setMotor0Speed(0.3*127); //right
jjcarr2 13:b6480275c445 704 motors.setMotor1Speed(0.3*127); //left
jjcarr2 13:b6480275c445 705
jjcarr2 13:b6480275c445 706 while(!out) {
jjcarr2 13:b6480275c445 707 preLeft=leftEncoder.getPulses();
jjcarr2 13:b6480275c445 708 preRight=rightEncoder.getPulses();
jjcarr2 13:b6480275c445 709
jjcarr2 13:b6480275c445 710 rangeFinderLeft.startMeas();
jjcarr2 13:b6480275c445 711 rangeFinderRight.startMeas();
jjcarr2 13:b6480275c445 712 wait_ms(20);
jjcarr2 13:b6480275c445 713 rangeFinderLeft.getMeas(range);
jjcarr2 13:b6480275c445 714 rangeFinderRight.getMeas(range2);
jjcarr2 13:b6480275c445 715 if(range < 10 || range2 < 10) out=1;
jjcarr2 13:b6480275c445 716
jjcarr2 13:b6480275c445 717 if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) {
jjcarr2 13:b6480275c445 718 motors.setMotor0Speed(0.4*127); //right
jjcarr2 13:b6480275c445 719 motors.setMotor1Speed(0.4*127); //left
jjcarr2 13:b6480275c445 720 wait_ms(50);
jjcarr2 13:b6480275c445 721 out=1;
jjcarr2 13:b6480275c445 722 }
jjcarr2 13:b6480275c445 723 if(abs(leftEncoder.getPulses()) >1000 || abs(leftEncoder.getPulses())>1000) out=1;
jjcarr2 13:b6480275c445 724 }
jjcarr2 13:b6480275c445 725 */
jjcarr2 13:b6480275c445 726
jjcarr2 13:b6480275c445 727 motors.stopBothMotors(0);
Fairy_Paolina 9:f34700716f1d 728 motors.begin();
jjcarr2 12:081b8fc654af 729
Fairy_Paolina 8:11ef93eebe07 730 preLeft=preRight=5000 ;
Fairy_Paolina 8:11ef93eebe07 731 leftEncoder.reset();
Fairy_Paolina 8:11ef93eebe07 732 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 733 motors.setMotor0Speed(.25*127); //right
Fairy_Paolina 9:f34700716f1d 734 motors.setMotor1Speed(.25*127); //left
Fairy_Paolina 9:f34700716f1d 735
jjcarr2 13:b6480275c445 736 if(section == TOOLS) {
jjcarr2 13:b6480275c445 737 while(IR.getDistance() > 10 && (abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) {
jjcarr2 13:b6480275c445 738
jjcarr2 13:b6480275c445 739 if(IR.getDistance() > 38) break;
jjcarr2 13:b6480275c445 740
jjcarr2 13:b6480275c445 741 preLeft=leftEncoder.getPulses();
jjcarr2 13:b6480275c445 742 preRight=rightEncoder.getPulses();
jjcarr2 13:b6480275c445 743 wait_ms(200);
jjcarr2 13:b6480275c445 744 }
jjcarr2 13:b6480275c445 745 } else if(section == MID || section == MID2) {
jjcarr2 13:b6480275c445 746 if(section == MID2) while(IR.getDistance() > 15 && (abs(leftEncoder.getPulses()) < 400 || abs(rightEncoder.getPulses()) < 400));
jjcarr2 13:b6480275c445 747 while(IR.getDistance() > 15 && (abs(leftEncoder.getPulses()) < 400 || abs(rightEncoder.getPulses()) < 400) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) {
jjcarr2 13:b6480275c445 748
jjcarr2 13:b6480275c445 749 if(IR.getDistance() > 38) break;
jjcarr2 13:b6480275c445 750
jjcarr2 13:b6480275c445 751 preLeft=leftEncoder.getPulses();
jjcarr2 13:b6480275c445 752 preRight=rightEncoder.getPulses();
jjcarr2 13:b6480275c445 753 wait_ms(200);
Fairy_Paolina 9:f34700716f1d 754 }
jjcarr2 13:b6480275c445 755
jjcarr2 13:b6480275c445 756 } else {
jjcarr2 13:b6480275c445 757 while(abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100);
jjcarr2 13:b6480275c445 758
jjcarr2 13:b6480275c445 759 leftEncoder.reset();
jjcarr2 13:b6480275c445 760 rightEncoder.reset();
jjcarr2 13:b6480275c445 761
jjcarr2 13:b6480275c445 762 motors.setMotor0Speed(-.15*127); //right
jjcarr2 13:b6480275c445 763 motors.setMotor1Speed(-.15*127); //left
jjcarr2 13:b6480275c445 764 while((abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100) && preLeft!=0 ) {
jjcarr2 13:b6480275c445 765 preLeft = leftEncoder.getPulses();
jjcarr2 13:b6480275c445 766 preRight = rightEncoder.getPulses();
jjcarr2 13:b6480275c445 767 wait_ms(200);
jjcarr2 13:b6480275c445 768 if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
jjcarr2 13:b6480275c445 769 }
jjcarr2 13:b6480275c445 770
jjcarr2 13:b6480275c445 771 leftEncoder.reset();
jjcarr2 13:b6480275c445 772 rightEncoder.reset();
jjcarr2 13:b6480275c445 773
jjcarr2 13:b6480275c445 774 motors.setMotor0Speed(0.25*127); //right
jjcarr2 13:b6480275c445 775 motors.setMotor1Speed(0.25*127); //left
jjcarr2 13:b6480275c445 776 while((abs(leftEncoder.getPulses()) < 10 || abs(rightEncoder.getPulses()) < 10));
jjcarr2 13:b6480275c445 777
jjcarr2 13:b6480275c445 778 motors.stopBothMotors(0);
jjcarr2 13:b6480275c445 779
jjcarr2 13:b6480275c445 780 return;
jjcarr2 13:b6480275c445 781 }
jjcarr2 13:b6480275c445 782
jjcarr2 13:b6480275c445 783 leftEncoder.reset();
jjcarr2 13:b6480275c445 784 rightEncoder.reset();
Fairy_Paolina 8:11ef93eebe07 785
Fairy_Paolina 9:f34700716f1d 786 motors.setMotor0Speed(-.25*127); //right
Fairy_Paolina 9:f34700716f1d 787 motors.setMotor1Speed(-.25*127); //left
jjcarr2 13:b6480275c445 788 while((abs(leftEncoder.getPulses()) < 10 || abs(rightEncoder.getPulses()) < 10));
jjcarr2 13:b6480275c445 789
jjcarr2 13:b6480275c445 790 motors.stopBothMotors(0);
jjcarr2 13:b6480275c445 791 wait_ms(20);
Fairy_Paolina 9:f34700716f1d 792 motors.begin();
Fairy_Paolina 9:f34700716f1d 793
Fairy_Paolina 3:58726d2e11f0 794 }
Fairy_Paolina 9:f34700716f1d 795
Fairy_Paolina 9:f34700716f1d 796 void tools_section(float* location, float &current)
Fairy_Paolina 9:f34700716f1d 797 {
jjcarr2 13:b6480275c445 798 slightMove(FORWARD,3200);
jjcarr2 13:b6480275c445 799 current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
Fairy_Paolina 9:f34700716f1d 800
jjcarr2 13:b6480275c445 801 wait(2);
jjcarr2 13:b6480275c445 802 slightMove(FORWARD,3200);
jjcarr2 13:b6480275c445 803 current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
jjcarr2 13:b6480275c445 804 /*
jjcarr2 13:b6480275c445 805 wall_follow_to_mid(LEFT,FORWARD, TOOLS);
jjcarr2 13:b6480275c445 806 // current position in reference to the starting position
jjcarr2 13:b6480275c445 807 current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
jjcarr2 12:081b8fc654af 808
jjcarr2 13:b6480275c445 809 wait(2);
Fairy_Paolina 9:f34700716f1d 810
jjcarr2 13:b6480275c445 811 wall_follow(LEFT,FORWARD, TOOLS, current);
jjcarr2 13:b6480275c445 812 // current position in reference to the starting position
jjcarr2 13:b6480275c445 813 current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
jjcarr2 13:b6480275c445 814 */
jjcarr2 13:b6480275c445 815 //////////////////////////////// determine tool
jjcarr2 13:b6480275c445 816 wait(2);
jjcarr2 13:b6480275c445 817 ///////////////////////////////////////////////////////////////////////////////////////
jjcarr2 13:b6480275c445 818 // Move Forward
jjcarr2 13:b6480275c445 819 slightMove(FORWARD, 100);
jjcarr2 12:081b8fc654af 820
jjcarr2 13:b6480275c445 821 //////////////////////////////////////////Tool aquiring
Fairy_Paolina 6:f5c26372b2d0 822 wait(2);
jjcarr2 13:b6480275c445 823 //////////////////////////////////////////////////////////////////// After tool is aquired
Fairy_Paolina 9:f34700716f1d 824
Fairy_Paolina 6:f5c26372b2d0 825 alignWithWall(TOOLS);
jjcarr2 13:b6480275c445 826 pc.printf("align\r\n");
Fairy_Paolina 6:f5c26372b2d0 827 wait_ms(100);
jjcarr2 12:081b8fc654af 828
jjcarr2 13:b6480275c445 829 //wall_follow2(LEFT,FORWARD,MID, current);
jjcarr2 13:b6480275c445 830 //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
jjcarr2 12:081b8fc654af 831
Fairy_Paolina 6:f5c26372b2d0 832 rangeFinderLeft.startMeas();
Fairy_Paolina 6:f5c26372b2d0 833 wait_ms(20);
Fairy_Paolina 6:f5c26372b2d0 834 rangeFinderLeft.getMeas(range);
Fairy_Paolina 9:f34700716f1d 835
Fairy_Paolina 9:f34700716f1d 836 if(range < 20) {
jjcarr2 13:b6480275c445 837 wall_follow2(LEFT,BACKWARD,TOOLS, current,0);
jjcarr2 13:b6480275c445 838 pc.printf("wall follow\r\n");
jjcarr2 11:12ce7600f2f9 839 location[0]= current - ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
jjcarr2 11:12ce7600f2f9 840 current= location[0];
jjcarr2 13:b6480275c445 841 pc.printf("current %f \r\n",current);
jjcarr2 13:b6480275c445 842 // go backwards
Fairy_Paolina 9:f34700716f1d 843 leftEncoder.reset();
Fairy_Paolina 9:f34700716f1d 844 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 845 motors.setMotor0Speed(-MAX_SPEED); //right
Fairy_Paolina 9:f34700716f1d 846 motors.setMotor1Speed(-MAX_SPEED); //left
Fairy_Paolina 9:f34700716f1d 847 while(abs(leftEncoder.getPulses()) < 120 || abs(rightEncoder.getPulses())< 120);
jjcarr2 13:b6480275c445 848 // hard stop
Fairy_Paolina 9:f34700716f1d 849 leftEncoder.reset();
Fairy_Paolina 9:f34700716f1d 850 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 851 motors.setMotor0Speed(MAX_SPEED); //right
Fairy_Paolina 9:f34700716f1d 852 motors.setMotor1Speed(MAX_SPEED); //left
jjcarr2 13:b6480275c445 853 while(abs(leftEncoder.getPulses()) < 10 || abs(rightEncoder.getPulses())< 10);
jjcarr2 13:b6480275c445 854 motors.stopBothMotors(0);
Fairy_Paolina 9:f34700716f1d 855
jjcarr2 13:b6480275c445 856 wait_ms(100);
Fairy_Paolina 6:f5c26372b2d0 857 leftTurn();
Fairy_Paolina 8:11ef93eebe07 858 overBump(TOOLS);
Fairy_Paolina 9:f34700716f1d 859 } else {
jjcarr2 13:b6480275c445 860 pc.printf("else greater than 20\r\n");
jjcarr2 13:b6480275c445 861 location[0]= current;
Fairy_Paolina 6:f5c26372b2d0 862 leftTurn();
jjcarr2 13:b6480275c445 863 overBump(TOOLS);
Fairy_Paolina 9:f34700716f1d 864 }
Fairy_Paolina 9:f34700716f1d 865
jjcarr2 13:b6480275c445 866 pc.printf("First Wavegap = %f\r\n",location[0]);
Fairy_Paolina 6:f5c26372b2d0 867 }
Fairy_Paolina 9:f34700716f1d 868
Fairy_Paolina 9:f34700716f1d 869 void mid_section(float* location, float &current, int* direction)
Fairy_Paolina 9:f34700716f1d 870 {
Fairy_Paolina 9:f34700716f1d 871 motors.begin();
jjcarr2 12:081b8fc654af 872
jjcarr2 13:b6480275c445 873 if(IR.getDistance() > 38) {
jjcarr2 12:081b8fc654af 874 direction[0]= STRAIGHT;
jjcarr2 13:b6480275c445 875 overBump(MID);
jjcarr2 12:081b8fc654af 876 return;
jjcarr2 12:081b8fc654af 877 }
jjcarr2 13:b6480275c445 878 pc.printf("before align with wall \r\n");
jjcarr2 13:b6480275c445 879 alignWithWall(MID);
jjcarr2 13:b6480275c445 880 wait_ms(100);
jjcarr2 13:b6480275c445 881
jjcarr2 13:b6480275c445 882 pc.printf("mid section current = %f\r\n",current);
jjcarr2 13:b6480275c445 883 wall_follow2(LEFT,FORWARD,MID, current,0);
jjcarr2 13:b6480275c445 884 current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
jjcarr2 13:b6480275c445 885 pc.printf("after wf2 current = %f\r\n",current);
jjcarr2 13:b6480275c445 886
jjcarr2 13:b6480275c445 887 wait_ms(500);
jjcarr2 13:b6480275c445 888 rangeFinderLeft.startMeas();
jjcarr2 13:b6480275c445 889 wait_ms(20);
jjcarr2 13:b6480275c445 890 rangeFinderLeft.getMeas(range);
jjcarr2 13:b6480275c445 891
jjcarr2 13:b6480275c445 892 if(range > 20 ) {
jjcarr2 13:b6480275c445 893 direction[0]= RIGHT;
jjcarr2 13:b6480275c445 894 location[1]= current;
jjcarr2 13:b6480275c445 895 slightMove(FORWARD,75);
jjcarr2 13:b6480275c445 896 //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
jjcarr2 13:b6480275c445 897 } else {
jjcarr2 13:b6480275c445 898 direction[0]= LEFT;
jjcarr2 13:b6480275c445 899 wall_follow2(LEFT,BACKWARD,MID,current,0);
jjcarr2 13:b6480275c445 900 location[1]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
jjcarr2 13:b6480275c445 901 current= location[1];
jjcarr2 13:b6480275c445 902
jjcarr2 13:b6480275c445 903 if(location[1] < 18) {
jjcarr2 13:b6480275c445 904 slightMove(FORWARD, 50);
jjcarr2 13:b6480275c445 905 current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
jjcarr2 13:b6480275c445 906 }
jjcarr2 13:b6480275c445 907
jjcarr2 13:b6480275c445 908 }
jjcarr2 12:081b8fc654af 909
jjcarr2 13:b6480275c445 910 pc.printf("wavegap2 = %f\r\n",location[1]);
jjcarr2 13:b6480275c445 911 leftTurn();
Fairy_Paolina 9:f34700716f1d 912
jjcarr2 13:b6480275c445 913 wait_ms(100);
Fairy_Paolina 9:f34700716f1d 914
jjcarr2 13:b6480275c445 915 overBump(MID);
Fairy_Paolina 9:f34700716f1d 916
Fairy_Paolina 6:f5c26372b2d0 917 }
Fairy_Paolina 9:f34700716f1d 918
Fairy_Paolina 9:f34700716f1d 919 void mid_section2(float* location, float &current, int* direction)
Fairy_Paolina 9:f34700716f1d 920 {
Fairy_Paolina 9:f34700716f1d 921 motors.begin();
jjcarr2 12:081b8fc654af 922
jjcarr2 13:b6480275c445 923 pc.printf("mid section 2\r\n");
jjcarr2 13:b6480275c445 924
jjcarr2 13:b6480275c445 925 if(IR.getDistance() > 38) {
jjcarr2 13:b6480275c445 926 direction[1]= STRAIGHT;
jjcarr2 13:b6480275c445 927 overBump(RIGS);
jjcarr2 12:081b8fc654af 928 return;
jjcarr2 12:081b8fc654af 929 }
jjcarr2 12:081b8fc654af 930
Fairy_Paolina 6:f5c26372b2d0 931 alignWithWall(MID);
jjcarr2 13:b6480275c445 932 pc.printf("midsection 2 alignt with wall mid \r\n");
jjcarr2 13:b6480275c445 933
jjcarr2 13:b6480275c445 934 wall_follow2(LEFT,FORWARD,MID, current,0);
jjcarr2 13:b6480275c445 935 current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
jjcarr2 13:b6480275c445 936
jjcarr2 13:b6480275c445 937 wait_ms(500);
Fairy_Paolina 9:f34700716f1d 938
jjcarr2 13:b6480275c445 939 pc.printf("midseection 2 after wf2 %f",current);
jjcarr2 13:b6480275c445 940 rangeFinderLeft.startMeas();
jjcarr2 13:b6480275c445 941 wait_ms(20);
jjcarr2 13:b6480275c445 942 rangeFinderLeft.getMeas(range);
jjcarr2 13:b6480275c445 943
jjcarr2 13:b6480275c445 944 if(range > 20 ) {
Fairy_Paolina 6:f5c26372b2d0 945 direction[1]= RIGHT;
Fairy_Paolina 6:f5c26372b2d0 946 location[2]= current;
jjcarr2 13:b6480275c445 947 slightMove(FORWARD,75);
jjcarr2 13:b6480275c445 948 //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
Fairy_Paolina 9:f34700716f1d 949 } else {
Fairy_Paolina 6:f5c26372b2d0 950 direction[1]= LEFT;
jjcarr2 13:b6480275c445 951 wall_follow2(LEFT,BACKWARD,MID,current,0);
jjcarr2 13:b6480275c445 952 location[2]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
jjcarr2 13:b6480275c445 953 current=location[2];
jjcarr2 13:b6480275c445 954 //slightMove(FORWARD,500);
Fairy_Paolina 6:f5c26372b2d0 955 }
Fairy_Paolina 9:f34700716f1d 956
Fairy_Paolina 9:f34700716f1d 957 leftTurn();
Fairy_Paolina 8:11ef93eebe07 958 overBump(RIGS);
jjcarr2 13:b6480275c445 959 pc.printf("overbump rigs\r\n");
Fairy_Paolina 9:f34700716f1d 960 }
Fairy_Paolina 9:f34700716f1d 961
Fairy_Paolina 9:f34700716f1d 962 void rig_section(float* location, float &current, int* direction, int rig)
Fairy_Paolina 9:f34700716f1d 963 {
jjcarr2 12:081b8fc654af 964 float loc;
jjcarr2 12:081b8fc654af 965
jjcarr2 13:b6480275c445 966 if(rig == 1) loc= 15;
jjcarr2 13:b6480275c445 967 else if(rig == 2) loc= 45;
jjcarr2 13:b6480275c445 968 else loc = 75;
jjcarr2 12:081b8fc654af 969
jjcarr2 12:081b8fc654af 970 rightTurn();
jjcarr2 13:b6480275c445 971 slightright();
jjcarr2 12:081b8fc654af 972
jjcarr2 12:081b8fc654af 973 if(current > loc) {
jjcarr2 13:b6480275c445 974 pc.printf("RIG section %f\r\n",current);
jjcarr2 13:b6480275c445 975 wall_follow2(RIGHT, BACKWARD, RIGS, current, rig);
jjcarr2 12:081b8fc654af 976 current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
jjcarr2 12:081b8fc654af 977 } else {
jjcarr2 13:b6480275c445 978 pc.printf("RIG section %f\r\n",current);
jjcarr2 13:b6480275c445 979 wall_follow2(RIGHT, FORWARD, RIGS, current, rig);
jjcarr2 12:081b8fc654af 980 current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
jjcarr2 12:081b8fc654af 981 }
jjcarr2 12:081b8fc654af 982 }
jjcarr2 12:081b8fc654af 983
jjcarr2 12:081b8fc654af 984 void tools_section_return(float* location, float &current)
jjcarr2 12:081b8fc654af 985 {
jjcarr2 12:081b8fc654af 986 if(location[0] > 16) {
jjcarr2 13:b6480275c445 987 leftTurn();
jjcarr2 13:b6480275c445 988 wall_follow2(LEFT, BACKWARD, RETURN, location[0], 0);
jjcarr2 12:081b8fc654af 989 }
jjcarr2 13:b6480275c445 990 motors.stopBothMotors(0);
jjcarr2 12:081b8fc654af 991
jjcarr2 12:081b8fc654af 992 }
jjcarr2 12:081b8fc654af 993
jjcarr2 12:081b8fc654af 994 void mid_section_return(float* location, float &current, int* direction)
jjcarr2 12:081b8fc654af 995 {
jjcarr2 13:b6480275c445 996 if(direction[0] == RIGHT) {
jjcarr2 12:081b8fc654af 997 leftTurn();
jjcarr2 13:b6480275c445 998 alignWithWall(MID);
jjcarr2 13:b6480275c445 999 wall_follow2(LEFT, BACKWARD, MID, current,0);
jjcarr2 13:b6480275c445 1000 current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
jjcarr2 12:081b8fc654af 1001 rightTurn();
jjcarr2 13:b6480275c445 1002 } else if(direction[0] == LEFT) {
jjcarr2 13:b6480275c445 1003 leftTurn();
jjcarr2 13:b6480275c445 1004 wall_follow2(RIGHT, FORWARD, MID, current,0);
jjcarr2 13:b6480275c445 1005 current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
jjcarr2 13:b6480275c445 1006 rightTurn();
jjcarr2 12:081b8fc654af 1007 }
jjcarr2 13:b6480275c445 1008 //ELSE and GO FORWARD
jjcarr2 12:081b8fc654af 1009 overBump(RIGS);
jjcarr2 12:081b8fc654af 1010 }
jjcarr2 12:081b8fc654af 1011
jjcarr2 12:081b8fc654af 1012 void mid_section2_return(float* location, float &current, int* direction)
jjcarr2 12:081b8fc654af 1013 {
jjcarr2 13:b6480275c445 1014 if(direction[1] == RIGHT) {
jjcarr2 12:081b8fc654af 1015 leftTurn();
jjcarr2 13:b6480275c445 1016 wall_follow2(LEFT, BACKWARD, MID, current,0);
jjcarr2 13:b6480275c445 1017 current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
jjcarr2 12:081b8fc654af 1018 rightTurn();
jjcarr2 13:b6480275c445 1019 } else if(direction[1] == LEFT) {
jjcarr2 13:b6480275c445 1020 leftTurn();
jjcarr2 13:b6480275c445 1021 wall_follow2(RIGHT, FORWARD, MID, current,0);
jjcarr2 13:b6480275c445 1022 current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
jjcarr2 13:b6480275c445 1023 rightTurn();
jjcarr2 12:081b8fc654af 1024 }
jjcarr2 13:b6480275c445 1025 //ELSE and GO FORWARD
jjcarr2 12:081b8fc654af 1026 overBump(MID);
jjcarr2 12:081b8fc654af 1027 }
jjcarr2 12:081b8fc654af 1028
jjcarr2 13:b6480275c445 1029 void rig_section_return(float* location, float &current, int* direction)
jjcarr2 12:081b8fc654af 1030 {
jjcarr2 13:b6480275c445 1031 alignWithWall(RIGS);
jjcarr2 12:081b8fc654af 1032 if(location[2] > current) {
jjcarr2 13:b6480275c445 1033 wall_follow2(RIGHT, FORWARD, MID, current,0);
jjcarr2 13:b6480275c445 1034 current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
jjcarr2 12:081b8fc654af 1035 } else {
jjcarr2 13:b6480275c445 1036 wall_follow2(RIGHT, BACKWARD, MID, current,0);
jjcarr2 13:b6480275c445 1037 current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
jjcarr2 12:081b8fc654af 1038 }
jjcarr2 12:081b8fc654af 1039 rightTurn();
jjcarr2 13:b6480275c445 1040 overBump(MID2);
Fairy_Paolina 8:11ef93eebe07 1041 }