Paolina Povolotskaya / NavigationTest

revise

Dependencies:   HCSR04 PID PololuQik2 QEI mbed-rtos Sharp

main.cpp@15:a467af795e57, 2014-04-01 (annotated)

Committer:
Fairy_Paolina
Date:
Tue Apr 01 15:42:03 2014 +0000
Revision:
15:a467af795e57
Parent:
14:3c8c4efe4786
Child:
16:0888096bff60
almost!;

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