Paolina Povolotskaya / NavigationTest

revise

Dependencies:   HCSR04 PID PololuQik2 QEI mbed-rtos Sharp

main.cpp@14:3c8c4efe4786, 2014-03-30 (annotated)

Committer:
Fairy_Paolina
Date:
Sun Mar 30 18:21:06 2014 +0000
Revision:
14:3c8c4efe4786
Parent:
13:9bad7f74833a
Child:
15:a467af795e57
updated;

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