Paolina Povolotskaya / NavigationTest

revise

Dependencies:   HCSR04 PID PololuQik2 QEI mbed-rtos Sharp

main.cpp@16:0888096bff60, 2014-04-03 (annotated)

Committer:
Fairy_Paolina
Date:
Thu Apr 03 15:25:17 2014 +0000
Revision:
16:0888096bff60
Parent:
15:a467af795e57 
asd;

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