Paolina Povolotskaya / NavigationTest

revise

Dependencies:   HCSR04 PID PololuQik2 QEI mbed-rtos Sharp

main.cpp@10:c57f6a5042d7, 2014-03-27 (annotated)

Committer:
Fairy_Paolina
Date:
Thu Mar 27 22:40:22 2014 +0000
Revision:
10:c57f6a5042d7
Parent:
9:f34700716f1d
Child:
11:d67a3958127a
revised;

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 10:c57f6a5042d7 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 6:f5c26372b2d0 29 #define FIRST_WAVE (0)
Fairy_Paolina 6:f5c26372b2d0 30 #define FAR (1)
Fairy_Paolina 10:c57f6a5042d7 31
Fairy_Paolina 10:c57f6a5042d7 32
Fairy_Paolina 9:f34700716f1d 33 float range, range2, pid_return;
Fairy_Paolina 0:ff94cc47fef7 34 void errFunction(void);
Fairy_Paolina 0:ff94cc47fef7 35 bool cRc;
Fairy_Paolina 10:c57f6a5042d7 36
Fairy_Paolina 0:ff94cc47fef7 37 //Hardware Initialization
Fairy_Paolina 0:ff94cc47fef7 38 Serial bt(p13,p14);
Fairy_Paolina 0:ff94cc47fef7 39 Serial pc(USBTX,USBRX);
Fairy_Paolina 2:3d0be48abcf2 40 HCSR04 rangeFinderLeft( PIN_TRIGGERL, PIN_ECHOL );
Fairy_Paolina 2:3d0be48abcf2 41 HCSR04 rangeFinderRight( PIN_TRIGGERR, PIN_ECHOR );
Fairy_Paolina 2:3d0be48abcf2 42 PID pid1(15.0,0.0,4.0,0.02);
Fairy_Paolina 0:ff94cc47fef7 43 PololuQik2 motors(p9, p10, p8, p15, errFunction, cRc);
Fairy_Paolina 4:f2333e66ec2c 44 QEI rightEncoder(p17,p18,NC,PPR,QEI::X4_ENCODING);
Fairy_Paolina 4:f2333e66ec2c 45 QEI leftEncoder(p16,p15,NC,PPR,QEI::X4_ENCODING);
Fairy_Paolina 8:11ef93eebe07 46 Sharp IR(p20);
Fairy_Paolina 1:801f0b9a862a 47 //InterruptIn encoder(p29);
Fairy_Paolina 10:c57f6a5042d7 48
Fairy_Paolina 10:c57f6a5042d7 49
Fairy_Paolina 0:ff94cc47fef7 50 //Functions
Fairy_Paolina 10:c57f6a5042d7 51
Fairy_Paolina 3:58726d2e11f0 52 float wall_follow(int side, int direction, int section);
Fairy_Paolina 6:f5c26372b2d0 53 void wall_follow2(int side, int direction, int section, float location);
Fairy_Paolina 0:ff94cc47fef7 54 void wall_follow3(int &currentLocation, int &WaveOpening);
Fairy_Paolina 1:801f0b9a862a 55 void leftTurn(void);
Fairy_Paolina 9:f34700716f1d 56 void slightleft(void);
Fairy_Paolina 1:801f0b9a862a 57 void rightTurn(void);
Fairy_Paolina 0:ff94cc47fef7 58 void us_distance(void);
Fairy_Paolina 3:58726d2e11f0 59 void tools_section(float* location, float &current);
Fairy_Paolina 6:f5c26372b2d0 60 void mid_section(float* location, float &current, int* direction);
Fairy_Paolina 6:f5c26372b2d0 61 void mid_section2(float* location, float &current, int* direction);
Fairy_Paolina 6:f5c26372b2d0 62 void rig_section(float* location, float &current, int* direction, int rig);
Fairy_Paolina 8:11ef93eebe07 63 void overBump(int section);
Fairy_Paolina 6:f5c26372b2d0 64 void alignWithWall(int section);
Fairy_Paolina 10:c57f6a5042d7 65 void UntilWall(int dir);
Fairy_Paolina 9:f34700716f1d 66
Fairy_Paolina 10:c57f6a5042d7 67
Fairy_Paolina 1:801f0b9a862a 68 //Variables
Fairy_Paolina 10:c57f6a5042d7 69
Fairy_Paolina 2:3d0be48abcf2 70 int main(void)
Fairy_Paolina 2:3d0be48abcf2 71 {
Fairy_Paolina 3:58726d2e11f0 72 float location[3], current=0;
Fairy_Paolina 3:58726d2e11f0 73 int direction[3];
Fairy_Paolina 8:11ef93eebe07 74 double distance;
Fairy_Paolina 10:c57f6a5042d7 75
Fairy_Paolina 0:ff94cc47fef7 76 pc.baud(115200);
Fairy_Paolina 1:801f0b9a862a 77 bt.baud(115200);
Fairy_Paolina 2:3d0be48abcf2 78 motors.begin();
Fairy_Paolina 10:c57f6a5042d7 79
Fairy_Paolina 7:78745a518957 80 bt.printf("START\r\n");
Fairy_Paolina 3:58726d2e11f0 81 //Go to tools
Fairy_Paolina 4:f2333e66ec2c 82 tools_section(location, current);
Fairy_Paolina 6:f5c26372b2d0 83 mid_section(location, current, direction);
Fairy_Paolina 9:f34700716f1d 84 //mid_section2(location, current, direction);
Fairy_Paolina 9:f34700716f1d 85 /* while(1) {
Fairy_Paolina 9:f34700716f1d 86 //bt.printf("IR %f\r\n", US.getDistance());
Fairy_Paolina 9:f34700716f1d 87 rangeFinderLeft.startMeas();
Fairy_Paolina 9:f34700716f1d 88 wait_ms(20);
Fairy_Paolina 9:f34700716f1d 89 rangeFinderLeft.getMeas(range);
Fairy_Paolina 9:f34700716f1d 90 bt.printf("Range = %f\n\r", range);
Fairy_Paolina 10:c57f6a5042d7 91
Fairy_Paolina 8:11ef93eebe07 92 wait_ms(200);
Fairy_Paolina 9:f34700716f1d 93 }*/
Fairy_Paolina 10:c57f6a5042d7 94
Fairy_Paolina 10:c57f6a5042d7 95
Fairy_Paolina 9:f34700716f1d 96 //leftTurn();
Fairy_Paolina 9:f34700716f1d 97 //rightTurn();
Fairy_Paolina 10:c57f6a5042d7 98
Fairy_Paolina 10:c57f6a5042d7 99
Fairy_Paolina 0:ff94cc47fef7 100 }
Fairy_Paolina 10:c57f6a5042d7 101
Fairy_Paolina 2:3d0be48abcf2 102 void errFunction(void)
Fairy_Paolina 2:3d0be48abcf2 103 {
Fairy_Paolina 2:3d0be48abcf2 104 //Nothing
Fairy_Paolina 0:ff94cc47fef7 105 }
Fairy_Paolina 10:c57f6a5042d7 106
Fairy_Paolina 0:ff94cc47fef7 107 void us_distance(void)
Fairy_Paolina 0:ff94cc47fef7 108 {
Fairy_Paolina 2:3d0be48abcf2 109 pc.printf("Ultra Sonic\n\r");
Fairy_Paolina 2:3d0be48abcf2 110 rangeFinderLeft.startMeas();
Fairy_Paolina 2:3d0be48abcf2 111 wait_us(20);
Fairy_Paolina 2:3d0be48abcf2 112 if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID)) {
Fairy_Paolina 2:3d0be48abcf2 113 pc.printf("Range = %f\n\r", range);
Fairy_Paolina 2:3d0be48abcf2 114 }
Fairy_Paolina 0:ff94cc47fef7 115 }
Fairy_Paolina 10:c57f6a5042d7 116
Fairy_Paolina 3:58726d2e11f0 117 float wall_follow(int side, int direction, int section)
Fairy_Paolina 0:ff94cc47fef7 118 {
Fairy_Paolina 8:11ef93eebe07 119 float location, wavegap=0, set=5;
Fairy_Paolina 5:70ccef3734ae 120 int dir=1;
Fairy_Paolina 10:c57f6a5042d7 121
Fairy_Paolina 4:f2333e66ec2c 122 pid1.reset();
Fairy_Paolina 10:c57f6a5042d7 123
Fairy_Paolina 3:58726d2e11f0 124 if(direction == BACKWARD) dir=-1;
Fairy_Paolina 5:70ccef3734ae 125 if(section == TOOLS)set= 10;
Fairy_Paolina 10:c57f6a5042d7 126
Fairy_Paolina 3:58726d2e11f0 127 leftEncoder.reset();
Fairy_Paolina 3:58726d2e11f0 128 rightEncoder.reset();
Fairy_Paolina 10:c57f6a5042d7 129
Fairy_Paolina 4:f2333e66ec2c 130 location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
Fairy_Paolina 10:c57f6a5042d7 131
Fairy_Paolina 9:f34700716f1d 132 while(location< 73) {
Fairy_Paolina 4:f2333e66ec2c 133 location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
Fairy_Paolina 10:c57f6a5042d7 134
Fairy_Paolina 3:58726d2e11f0 135 pid1.setInputLimits(0, set);
Fairy_Paolina 1:801f0b9a862a 136 pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
Fairy_Paolina 3:58726d2e11f0 137 pid1.setSetPoint(set);
Fairy_Paolina 9:f34700716f1d 138 if(side) {
Fairy_Paolina 2:3d0be48abcf2 139 rangeFinderLeft.startMeas();
Fairy_Paolina 8:11ef93eebe07 140 wait_ms(38);
Fairy_Paolina 2:3d0be48abcf2 141 rangeFinderLeft.getMeas(range);
Fairy_Paolina 9:f34700716f1d 142 } else {
Fairy_Paolina 2:3d0be48abcf2 143 rangeFinderRight.startMeas();
Fairy_Paolina 8:11ef93eebe07 144 wait_ms(38);
Fairy_Paolina 2:3d0be48abcf2 145 rangeFinderRight.getMeas(range);
Fairy_Paolina 2:3d0be48abcf2 146 pc.printf("%d\r\n",range);
Fairy_Paolina 2:3d0be48abcf2 147 }
Fairy_Paolina 10:c57f6a5042d7 148
Fairy_Paolina 3:58726d2e11f0 149 if(range > 20) {
Fairy_Paolina 4:f2333e66ec2c 150 wavegap=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
Fairy_Paolina 7:78745a518957 151 //bt.printf("wavegap %f\r\n",wavegap);
Fairy_Paolina 3:58726d2e11f0 152 // AT WAVE OPENING!!!!
Fairy_Paolina 9:f34700716f1d 153 motors.setMotor1Speed(dir*0.4*127);//left
Fairy_Paolina 9:f34700716f1d 154 motors.setMotor0Speed(dir*0.4*127);//right
Fairy_Paolina 9:f34700716f1d 155 } else {
Fairy_Paolina 10:c57f6a5042d7 156
Fairy_Paolina 6:f5c26372b2d0 157 pid1.setProcessValue(range);
Fairy_Paolina 6:f5c26372b2d0 158 pid_return = pid1.compute();
Fairy_Paolina 10:c57f6a5042d7 159
Fairy_Paolina 6:f5c26372b2d0 160 if(pid_return > 0) {
Fairy_Paolina 9:f34700716f1d 161 if(side) {
Fairy_Paolina 6:f5c26372b2d0 162 motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
Fairy_Paolina 6:f5c26372b2d0 163 motors.setMotor1Speed(dir*MAX_SPEED);//left
Fairy_Paolina 9:f34700716f1d 164 } else {
Fairy_Paolina 6:f5c26372b2d0 165 motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
Fairy_Paolina 6:f5c26372b2d0 166 motors.setMotor0Speed(dir*MAX_SPEED);//right
Fairy_Paolina 6:f5c26372b2d0 167 }
Fairy_Paolina 9:f34700716f1d 168 } else if(pid_return < 0) {
Fairy_Paolina 9:f34700716f1d 169 if(side) {
Fairy_Paolina 6:f5c26372b2d0 170 motors.setMotor0Speed(dir*MAX_SPEED);//right
Fairy_Paolina 6:f5c26372b2d0 171 motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
Fairy_Paolina 9:f34700716f1d 172 } else {
Fairy_Paolina 6:f5c26372b2d0 173 motors.setMotor1Speed(dir*MAX_SPEED);//left
Fairy_Paolina 6:f5c26372b2d0 174 motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
Fairy_Paolina 6:f5c26372b2d0 175 }
Fairy_Paolina 9:f34700716f1d 176 } else {
Fairy_Paolina 6:f5c26372b2d0 177 motors.setMotor0Speed(dir*MAX_SPEED);//right
Fairy_Paolina 3:58726d2e11f0 178 motors.setMotor1Speed(dir*MAX_SPEED);//left
Fairy_Paolina 2:3d0be48abcf2 179 }
Fairy_Paolina 0:ff94cc47fef7 180 }
Fairy_Paolina 2:3d0be48abcf2 181 }
Fairy_Paolina 3:58726d2e11f0 182 return wavegap;
Fairy_Paolina 0:ff94cc47fef7 183 }
Fairy_Paolina 10:c57f6a5042d7 184
Fairy_Paolina 0:ff94cc47fef7 185 /* MODIFIED WALL_FOLLOW FOR NAVIGATION */
Fairy_Paolina 10:c57f6a5042d7 186
Fairy_Paolina 6:f5c26372b2d0 187 void wall_follow2(int side, int direction, int section, float location)
Fairy_Paolina 0:ff94cc47fef7 188 {
Fairy_Paolina 10:c57f6a5042d7 189 bool SeeWaveGap = false, dir=1, limit=83;
Fairy_Paolina 8:11ef93eebe07 190 float set=5, loc=0;
Fairy_Paolina 10:c57f6a5042d7 191
Fairy_Paolina 4:f2333e66ec2c 192 pid1.reset();
Fairy_Paolina 10:c57f6a5042d7 193
Fairy_Paolina 3:58726d2e11f0 194 if(direction == BACKWARD) dir=-1;
Fairy_Paolina 10:c57f6a5042d7 195 if(section == TOOLS){
Fairy_Paolina 10:c57f6a5042d7 196 set= 5;
Fairy_Paolina 10:c57f6a5042d7 197 limit = 78;
Fairy_Paolina 10:c57f6a5042d7 198 }
Fairy_Paolina 9:f34700716f1d 199
Fairy_Paolina 10:c57f6a5042d7 200
Fairy_Paolina 3:58726d2e11f0 201 leftEncoder.reset();
Fairy_Paolina 3:58726d2e11f0 202 rightEncoder.reset();
Fairy_Paolina 10:c57f6a5042d7 203
Fairy_Paolina 10:c57f6a5042d7 204 while(dir*loc + location <= limit) {
Fairy_Paolina 7:78745a518957 205 loc=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
Fairy_Paolina 10:c57f6a5042d7 206
Fairy_Paolina 5:70ccef3734ae 207 pid1.setInputLimits(0.0, set);
Fairy_Paolina 1:801f0b9a862a 208 pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
Fairy_Paolina 5:70ccef3734ae 209 pid1.setSetPoint(set);
Fairy_Paolina 10:c57f6a5042d7 210
Fairy_Paolina 9:f34700716f1d 211 if(side) {
Fairy_Paolina 2:3d0be48abcf2 212 rangeFinderLeft.startMeas();
Fairy_Paolina 8:11ef93eebe07 213 wait_ms(38);
Fairy_Paolina 2:3d0be48abcf2 214 rangeFinderLeft.getMeas(range);
Fairy_Paolina 9:f34700716f1d 215 } else {
Fairy_Paolina 2:3d0be48abcf2 216 rangeFinderRight.startMeas();
Fairy_Paolina 8:11ef93eebe07 217 wait_ms(38);
Fairy_Paolina 2:3d0be48abcf2 218 rangeFinderRight.getMeas(range);
Fairy_Paolina 2:3d0be48abcf2 219 }
Fairy_Paolina 10:c57f6a5042d7 220
Fairy_Paolina 10:c57f6a5042d7 221 if(section == RIGS){
Fairy_Paolina 10:c57f6a5042d7 222 rangeFinderRight.startMeas();
Fairy_Paolina 10:c57f6a5042d7 223 wait_ms(38);
Fairy_Paolina 10:c57f6a5042d7 224 rangeFinderRight.getMeas(range);
Fairy_Paolina 10:c57f6a5042d7 225 if(range< 20){
Fairy_Paolina 10:c57f6a5042d7 226 motors.stopBothMotors();
Fairy_Paolina 10:c57f6a5042d7 227 break;
Fairy_Paolina 10:c57f6a5042d7 228 }
Fairy_Paolina 10:c57f6a5042d7 229 }
Fairy_Paolina 10:c57f6a5042d7 230
Fairy_Paolina 10:c57f6a5042d7 231
Fairy_Paolina 10:c57f6a5042d7 232 //bt.printf("wall follow 2 range %f\r\n",range);
Fairy_Paolina 10:c57f6a5042d7 233 //bt.printf("loc+location = %f\r\n", loc+location);
Fairy_Paolina 10:c57f6a5042d7 234 if(range > 20 && !SeeWaveGap) {
Fairy_Paolina 10:c57f6a5042d7 235 motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
Fairy_Paolina 10:c57f6a5042d7 236 motors.setMotor1Speed(dir*MAX_SPEED);//left
Fairy_Paolina 10:c57f6a5042d7 237 wait_ms(30);
Fairy_Paolina 10:c57f6a5042d7 238 SeeWaveGap = true;
Fairy_Paolina 10:c57f6a5042d7 239 } else if(range > 20 && SeeWaveGap){
Fairy_Paolina 10:c57f6a5042d7 240 motors.stopBothMotors();
Fairy_Paolina 2:3d0be48abcf2 241 bt.printf("wavegap\r\n");
Fairy_Paolina 2:3d0be48abcf2 242 // AT WAVE OPENING!!!!
Fairy_Paolina 2:3d0be48abcf2 243 break;
Fairy_Paolina 10:c57f6a5042d7 244 } else {
Fairy_Paolina 10:c57f6a5042d7 245
Fairy_Paolina 0:ff94cc47fef7 246 pid1.setProcessValue(range);
Fairy_Paolina 2:3d0be48abcf2 247 pid_return = pid1.compute();
Fairy_Paolina 2:3d0be48abcf2 248 //bt.printf("Range: %f\n PID: %f\r\n", range, pid_return);
Fairy_Paolina 10:c57f6a5042d7 249
Fairy_Paolina 9:f34700716f1d 250 if(pid_return > 0) {
Fairy_Paolina 9:f34700716f1d 251 if(side) {
Fairy_Paolina 3:58726d2e11f0 252 motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
Fairy_Paolina 3:58726d2e11f0 253 motors.setMotor1Speed(dir*MAX_SPEED);//left
Fairy_Paolina 9:f34700716f1d 254 } else {
Fairy_Paolina 3:58726d2e11f0 255 motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
Fairy_Paolina 3:58726d2e11f0 256 motors.setMotor0Speed(dir*MAX_SPEED);//right
Fairy_Paolina 2:3d0be48abcf2 257 }
Fairy_Paolina 9:f34700716f1d 258 } else if(pid_return < 0) {
Fairy_Paolina 9:f34700716f1d 259 if(side) {
Fairy_Paolina 3:58726d2e11f0 260 motors.setMotor0Speed(dir*MAX_SPEED);//right
Fairy_Paolina 3:58726d2e11f0 261 motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
Fairy_Paolina 9:f34700716f1d 262 } else {
Fairy_Paolina 3:58726d2e11f0 263 motors.setMotor1Speed(dir*MAX_SPEED);//left
Fairy_Paolina 3:58726d2e11f0 264 motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
Fairy_Paolina 2:3d0be48abcf2 265 }
Fairy_Paolina 2:3d0be48abcf2 266 } else {
Fairy_Paolina 3:58726d2e11f0 267 motors.setMotor0Speed(dir*MAX_SPEED);
Fairy_Paolina 3:58726d2e11f0 268 motors.setMotor1Speed(dir*MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 269 }
Fairy_Paolina 10:c57f6a5042d7 270 }}
Fairy_Paolina 6:f5c26372b2d0 271 motors.stopBothMotors();
Fairy_Paolina 0:ff94cc47fef7 272 }
Fairy_Paolina 10:c57f6a5042d7 273
Fairy_Paolina 10:c57f6a5042d7 274
Fairy_Paolina 9:f34700716f1d 275 void alignWithWall(int section)
Fairy_Paolina 9:f34700716f1d 276 {
Fairy_Paolina 6:f5c26372b2d0 277 float usValue = 0;
Fairy_Paolina 10:c57f6a5042d7 278
Fairy_Paolina 9:f34700716f1d 279 if(section == TOOLS) {
Fairy_Paolina 6:f5c26372b2d0 280 // turn at an angle
Fairy_Paolina 6:f5c26372b2d0 281 leftEncoder.reset();
Fairy_Paolina 9:f34700716f1d 282 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 283 motors.setMotor0Speed(-1.2*MAX_SPEED); //right
Fairy_Paolina 6:f5c26372b2d0 284 motors.setMotor1Speed(0.4*MAX_SPEED); //left
Fairy_Paolina 6:f5c26372b2d0 285 while(rightEncoder.getPulses()>-1000);
Fairy_Paolina 6:f5c26372b2d0 286 motors.stopBothMotors();
Fairy_Paolina 10:c57f6a5042d7 287
Fairy_Paolina 6:f5c26372b2d0 288 //go backwards toward wall
Fairy_Paolina 6:f5c26372b2d0 289 leftEncoder.reset();
Fairy_Paolina 9:f34700716f1d 290 rightEncoder.reset();
Fairy_Paolina 6:f5c26372b2d0 291 motors.setMotor0Speed(-MAX_SPEED); //right
Fairy_Paolina 6:f5c26372b2d0 292 motors.setMotor1Speed(-MAX_SPEED); //left
Fairy_Paolina 7:78745a518957 293 while(abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300);
Fairy_Paolina 10:c57f6a5042d7 294
Fairy_Paolina 6:f5c26372b2d0 295 motors.stopBothMotors();
Fairy_Paolina 10:c57f6a5042d7 296
Fairy_Paolina 9:f34700716f1d 297 // turn left towards wall
Fairy_Paolina 6:f5c26372b2d0 298 leftEncoder.reset();
Fairy_Paolina 6:f5c26372b2d0 299 rightEncoder.reset();
Fairy_Paolina 6:f5c26372b2d0 300 motors.setMotor0Speed(MAX_SPEED); //right
Fairy_Paolina 6:f5c26372b2d0 301 motors.setMotor1Speed(-MAX_SPEED); //left
Fairy_Paolina 9:f34700716f1d 302 while(rightEncoder.getPulses() < 10 || abs(leftEncoder.getPulses()) < 10);
Fairy_Paolina 10:c57f6a5042d7 303
Fairy_Paolina 9:f34700716f1d 304 motors.stopBothMotors();
Fairy_Paolina 10:c57f6a5042d7 305
Fairy_Paolina 7:78745a518957 306 motors.setMotor0Speed(0.7*MAX_SPEED); //right
Fairy_Paolina 7:78745a518957 307 motors.setMotor1Speed(-0.7*MAX_SPEED); //left
Fairy_Paolina 7:78745a518957 308 } else {
Fairy_Paolina 8:11ef93eebe07 309 rightTurn();
Fairy_Paolina 7:78745a518957 310 motors.setMotor0Speed(-0.7*MAX_SPEED); //right
Fairy_Paolina 7:78745a518957 311 motors.setMotor1Speed(0.7*MAX_SPEED); //left
Fairy_Paolina 6:f5c26372b2d0 312 }
Fairy_Paolina 10:c57f6a5042d7 313
Fairy_Paolina 6:f5c26372b2d0 314 usValue = 0;
Fairy_Paolina 9:f34700716f1d 315 while(1) {
Fairy_Paolina 2:3d0be48abcf2 316 rangeFinderLeft.startMeas();
Fairy_Paolina 6:f5c26372b2d0 317 wait_ms(20);
Fairy_Paolina 6:f5c26372b2d0 318 rangeFinderLeft.getMeas(range);
Fairy_Paolina 7:78745a518957 319 //bt.printf("Range %f \t OldValue %f\n\r",range, usValue);
Fairy_Paolina 9:f34700716f1d 320 if(range > usValue && usValue != 0 && range < 25) {
Fairy_Paolina 0:ff94cc47fef7 321 break;
Fairy_Paolina 2:3d0be48abcf2 322 } else {
Fairy_Paolina 9:f34700716f1d 323 usValue = range;
Fairy_Paolina 0:ff94cc47fef7 324 }
Fairy_Paolina 2:3d0be48abcf2 325 }
Fairy_Paolina 6:f5c26372b2d0 326 motors.stopBothMotors();
Fairy_Paolina 5:70ccef3734ae 327 }
Fairy_Paolina 10:c57f6a5042d7 328
Fairy_Paolina 1:801f0b9a862a 329 void rightTurn(void)
Fairy_Paolina 1:801f0b9a862a 330 {
Fairy_Paolina 9:f34700716f1d 331 motors.begin();
Fairy_Paolina 1:801f0b9a862a 332 leftEncoder.reset();
Fairy_Paolina 2:3d0be48abcf2 333 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 334 motors.setMotor0Speed(-0.5*127);//right
Fairy_Paolina 9:f34700716f1d 335 motors.setMotor1Speed(0.5*127);//left
Fairy_Paolina 9:f34700716f1d 336 while(leftEncoder.getPulses()<900 || rightEncoder.getPulses()>-900);
Fairy_Paolina 1:801f0b9a862a 337 motors.stopBothMotors();
Fairy_Paolina 1:801f0b9a862a 338 }
Fairy_Paolina 10:c57f6a5042d7 339
Fairy_Paolina 1:801f0b9a862a 340 void leftTurn(void)
Fairy_Paolina 9:f34700716f1d 341 {
Fairy_Paolina 9:f34700716f1d 342 /*
Fairy_Paolina 1:801f0b9a862a 343 leftEncoder.reset();
Fairy_Paolina 2:3d0be48abcf2 344 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 345 motors.setMotor0Speed(0.4*MAX_SPEED); //right
Fairy_Paolina 9:f34700716f1d 346 motors.setMotor1Speed(-MAX_SPEED); //left
Fairy_Paolina 9:f34700716f1d 347 while(abs(leftEncoder.getPulses())<2500);
Fairy_Paolina 9:f34700716f1d 348 motors.stopBothMotors();
Fairy_Paolina 9:f34700716f1d 349 */
Fairy_Paolina 9:f34700716f1d 350 motors.begin();
Fairy_Paolina 9:f34700716f1d 351 leftEncoder.reset();
Fairy_Paolina 9:f34700716f1d 352 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 353 motors.setMotor0Speed(0.5*127);// right
Fairy_Paolina 9:f34700716f1d 354 motors.setMotor1Speed(-0.5*127);// left
Fairy_Paolina 10:c57f6a5042d7 355 while(abs(leftEncoder.getPulses())<1100 || rightEncoder.getPulses()<1100);
Fairy_Paolina 5:70ccef3734ae 356 motors.stopBothMotors();
Fairy_Paolina 5:70ccef3734ae 357 }
Fairy_Paolina 10:c57f6a5042d7 358
Fairy_Paolina 9:f34700716f1d 359 void slightleft(void){
Fairy_Paolina 9:f34700716f1d 360
Fairy_Paolina 9:f34700716f1d 361 leftEncoder.reset();
Fairy_Paolina 9:f34700716f1d 362 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 363 motors.setMotor0Speed(0.5*127);// right
Fairy_Paolina 9:f34700716f1d 364 motors.setMotor1Speed(-0.5*127);// left
Fairy_Paolina 10:c57f6a5042d7 365 while(abs(leftEncoder.getPulses())<100 || rightEncoder.getPulses()<100);
Fairy_Paolina 9:f34700716f1d 366 motors.stopBothMotors();
Fairy_Paolina 9:f34700716f1d 367 }
Fairy_Paolina 9:f34700716f1d 368
Fairy_Paolina 10:c57f6a5042d7 369 void UntilWall(int dir){
Fairy_Paolina 10:c57f6a5042d7 370
Fairy_Paolina 10:c57f6a5042d7 371 if(dir == BACKWARD) dir=-1;
Fairy_Paolina 10:c57f6a5042d7 372
Fairy_Paolina 10:c57f6a5042d7 373 leftEncoder.reset();
Fairy_Paolina 10:c57f6a5042d7 374 rightEncoder.reset();
Fairy_Paolina 10:c57f6a5042d7 375 motors.setMotor0Speed(dir*0.2*127); //right
Fairy_Paolina 10:c57f6a5042d7 376 motors.setMotor1Speed(dir*0.2*127); //left
Fairy_Paolina 10:c57f6a5042d7 377
Fairy_Paolina 10:c57f6a5042d7 378 range = 30;
Fairy_Paolina 10:c57f6a5042d7 379
Fairy_Paolina 10:c57f6a5042d7 380 while(range > 20){
Fairy_Paolina 10:c57f6a5042d7 381 rangeFinderRight.startMeas();
Fairy_Paolina 10:c57f6a5042d7 382 wait_ms(20);
Fairy_Paolina 10:c57f6a5042d7 383 rangeFinderRight.getMeas(range);
Fairy_Paolina 10:c57f6a5042d7 384 }
Fairy_Paolina 10:c57f6a5042d7 385
Fairy_Paolina 10:c57f6a5042d7 386 motors.setMotor0Speed(dir*-0.2*127); //right
Fairy_Paolina 10:c57f6a5042d7 387 motors.setMotor1Speed(dir*-0.2*127); //left
Fairy_Paolina 10:c57f6a5042d7 388 wait_ms(5);
Fairy_Paolina 10:c57f6a5042d7 389 motors.stopBothMotors();
Fairy_Paolina 10:c57f6a5042d7 390 }
Fairy_Paolina 10:c57f6a5042d7 391
Fairy_Paolina 9:f34700716f1d 392 void overBump(int section)
Fairy_Paolina 9:f34700716f1d 393 {
Fairy_Paolina 9:f34700716f1d 394 int preLeft=5000, preRight=5000, out=0;
Fairy_Paolina 9:f34700716f1d 395
Fairy_Paolina 9:f34700716f1d 396 motors.begin();
Fairy_Paolina 9:f34700716f1d 397
Fairy_Paolina 9:f34700716f1d 398 leftEncoder.reset();
Fairy_Paolina 9:f34700716f1d 399 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 400 motors.setMotor0Speed(-0.2*127); //right
Fairy_Paolina 9:f34700716f1d 401 motors.setMotor1Speed(-0.2*127); //left
Fairy_Paolina 9:f34700716f1d 402 while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses())< 50);
Fairy_Paolina 9:f34700716f1d 403 motors.stopBothMotors();
Fairy_Paolina 3:58726d2e11f0 404
Fairy_Paolina 3:58726d2e11f0 405 leftEncoder.reset();
Fairy_Paolina 3:58726d2e11f0 406 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 407 motors.setMotor0Speed(0.2*127); //right
Fairy_Paolina 9:f34700716f1d 408 motors.setMotor1Speed(0.2*127); //left
Fairy_Paolina 8:11ef93eebe07 409 while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.getDistance() >20 && preLeft!=0){
Fairy_Paolina 9:f34700716f1d 410 preLeft=leftEncoder.getPulses();
Fairy_Paolina 9:f34700716f1d 411 preRight=rightEncoder.getPulses();
Fairy_Paolina 9:f34700716f1d 412 wait_ms(100);
Fairy_Paolina 10:c57f6a5042d7 413 //bt.printf(" first while left %d right %d \r\n", preLeft, preRight);
Fairy_Paolina 9:f34700716f1d 414 if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
Fairy_Paolina 8:11ef93eebe07 415 }
Fairy_Paolina 8:11ef93eebe07 416
Fairy_Paolina 8:11ef93eebe07 417 motors.stopBothMotors();
Fairy_Paolina 9:f34700716f1d 418 motors.begin();
Fairy_Paolina 9:f34700716f1d 419 wait(2);
Fairy_Paolina 9:f34700716f1d 420 /*
Fairy_Paolina 9:f34700716f1d 421 motors.stopBothMotors();
Fairy_Paolina 9:f34700716f1d 422 motors.setMotor0Speed(0.15*127); //right
Fairy_Paolina 9:f34700716f1d 423 motors.setMotor1Speed(0.15*127); //left
Fairy_Paolina 9:f34700716f1d 424 preLeft=preRight=5000 ;
Fairy_Paolina 9:f34700716f1d 425 leftEncoder.reset();
Fairy_Paolina 9:f34700716f1d 426 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 427 */
Fairy_Paolina 9:f34700716f1d 428 // while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.getDistance() >20 && preLeft!=0){
Fairy_Paolina 9:f34700716f1d 429 /* preLeft=leftEncoder.getPulses();
Fairy_Paolina 9:f34700716f1d 430 preRight=rightEncoder.getPulses();
Fairy_Paolina 9:f34700716f1d 431 bt.printf("second while left %d right %d \r\n", preLeft, preRight);
Fairy_Paolina 9:f34700716f1d 432 wait_ms(200);
Fairy_Paolina 9:f34700716f1d 433 if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
Fairy_Paolina 9:f34700716f1d 434 }*/
Fairy_Paolina 9:f34700716f1d 435
Fairy_Paolina 8:11ef93eebe07 436 leftEncoder.reset();
Fairy_Paolina 8:11ef93eebe07 437 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 438 motors.setMotor0Speed(0.3*127); //right
Fairy_Paolina 9:f34700716f1d 439 motors.setMotor1Speed(0.3*127); //left
Fairy_Paolina 10:c57f6a5042d7 440
Fairy_Paolina 9:f34700716f1d 441 while(!out) {
Fairy_Paolina 6:f5c26372b2d0 442 preLeft=leftEncoder.getPulses();
Fairy_Paolina 6:f5c26372b2d0 443 preRight=rightEncoder.getPulses();
Fairy_Paolina 10:c57f6a5042d7 444
Fairy_Paolina 9:f34700716f1d 445 rangeFinderLeft.startMeas();
Fairy_Paolina 9:f34700716f1d 446 rangeFinderRight.startMeas();
Fairy_Paolina 9:f34700716f1d 447 wait_ms(20);
Fairy_Paolina 9:f34700716f1d 448 rangeFinderLeft.getMeas(range);
Fairy_Paolina 9:f34700716f1d 449 rangeFinderRight.getMeas(range2);
Fairy_Paolina 9:f34700716f1d 450 if(range < 10 || range2 < 10) out=1;
Fairy_Paolina 10:c57f6a5042d7 451
Fairy_Paolina 9:f34700716f1d 452 if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) {
Fairy_Paolina 9:f34700716f1d 453 motors.setMotor0Speed(0.4*127); //right
Fairy_Paolina 9:f34700716f1d 454 motors.setMotor1Speed(0.4*127); //left
Fairy_Paolina 9:f34700716f1d 455 }
Fairy_Paolina 10:c57f6a5042d7 456 if(abs(leftEncoder.getPulses()) >1000 || abs(leftEncoder.getPulses())>1000) out=1;
Fairy_Paolina 8:11ef93eebe07 457 }
Fairy_Paolina 10:c57f6a5042d7 458
Fairy_Paolina 8:11ef93eebe07 459 motors.stopBothMotors();
Fairy_Paolina 9:f34700716f1d 460 wait(2);
Fairy_Paolina 9:f34700716f1d 461 motors.begin();
Fairy_Paolina 4:f2333e66ec2c 462
Fairy_Paolina 8:11ef93eebe07 463 preLeft=preRight=5000 ;
Fairy_Paolina 8:11ef93eebe07 464 leftEncoder.reset();
Fairy_Paolina 8:11ef93eebe07 465 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 466 motors.setMotor0Speed(.25*127); //right
Fairy_Paolina 9:f34700716f1d 467 motors.setMotor1Speed(.25*127); //left
Fairy_Paolina 10:c57f6a5042d7 468
Fairy_Paolina 9:f34700716f1d 469 if(section == TOOLS || section == MID) {
Fairy_Paolina 8:11ef93eebe07 470 while(IR.getDistance() > 20 ) {
Fairy_Paolina 10:c57f6a5042d7 471 //bt.printf("IR %f\r\n", IR.getDistance());
Fairy_Paolina 10:c57f6a5042d7 472 //bt.printf("third while left %d right %d \r\n", preLeft, preRight);
Fairy_Paolina 9:f34700716f1d 473 }
Fairy_Paolina 9:f34700716f1d 474 } else while((abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses())< 200));
Fairy_Paolina 10:c57f6a5042d7 475
Fairy_Paolina 9:f34700716f1d 476 motors.setMotor0Speed(-.25*127); //right
Fairy_Paolina 9:f34700716f1d 477 motors.setMotor1Speed(-.25*127); //left
Fairy_Paolina 9:f34700716f1d 478 wait_ms(10);
Fairy_Paolina 3:58726d2e11f0 479 motors.stopBothMotors();
Fairy_Paolina 8:11ef93eebe07 480 wait(2);
Fairy_Paolina 9:f34700716f1d 481 motors.begin();
Fairy_Paolina 10:c57f6a5042d7 482
Fairy_Paolina 3:58726d2e11f0 483 }
Fairy_Paolina 10:c57f6a5042d7 484
Fairy_Paolina 9:f34700716f1d 485 void tools_section(float* location, float &current)
Fairy_Paolina 9:f34700716f1d 486 {
Fairy_Paolina 9:f34700716f1d 487 wall_follow(LEFT,FORWARD, TOOLS);
Fairy_Paolina 6:f5c26372b2d0 488 // current position in reference to the starting position
Fairy_Paolina 4:f2333e66ec2c 489 current=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
Fairy_Paolina 6:f5c26372b2d0 490 bt.printf("current %f \r\n",current);
Fairy_Paolina 10:c57f6a5042d7 491
Fairy_Paolina 10:c57f6a5042d7 492
Fairy_Paolina 10:c57f6a5042d7 493 motors.setMotor0Speed(-.2*127); //right
Fairy_Paolina 10:c57f6a5042d7 494 motors.setMotor1Speed(-.2*127); //left
Fairy_Paolina 10:c57f6a5042d7 495 wait_ms(5);
Fairy_Paolina 5:70ccef3734ae 496 motors.stopBothMotors();
Fairy_Paolina 10:c57f6a5042d7 497
Fairy_Paolina 10:c57f6a5042d7 498 motors.setMotor0Speed(.2*127); //right
Fairy_Paolina 10:c57f6a5042d7 499 motors.setMotor1Speed(.2*127); //left
Fairy_Paolina 10:c57f6a5042d7 500 while(IR.getDistance()>6);
Fairy_Paolina 10:c57f6a5042d7 501
Fairy_Paolina 10:c57f6a5042d7 502 motors.setMotor0Speed(-.2*127); //right
Fairy_Paolina 10:c57f6a5042d7 503 motors.setMotor1Speed(-.2*127); //left
Fairy_Paolina 10:c57f6a5042d7 504 wait_ms(5);
Fairy_Paolina 10:c57f6a5042d7 505 motors.stopBothMotors();
Fairy_Paolina 10:c57f6a5042d7 506
Fairy_Paolina 10:c57f6a5042d7 507
Fairy_Paolina 6:f5c26372b2d0 508 //Tool aquiring
Fairy_Paolina 6:f5c26372b2d0 509 wait(2);
Fairy_Paolina 6:f5c26372b2d0 510 // After tool is aquired
Fairy_Paolina 10:c57f6a5042d7 511
Fairy_Paolina 6:f5c26372b2d0 512 alignWithWall(TOOLS);
Fairy_Paolina 9:f34700716f1d 513
Fairy_Paolina 6:f5c26372b2d0 514 wait_ms(100);
Fairy_Paolina 6:f5c26372b2d0 515
Fairy_Paolina 9:f34700716f1d 516 wall_follow2(LEFT,FORWARD,MID, current);
Fairy_Paolina 9:f34700716f1d 517 current= 78;
Fairy_Paolina 9:f34700716f1d 518
Fairy_Paolina 6:f5c26372b2d0 519 rangeFinderLeft.startMeas();
Fairy_Paolina 6:f5c26372b2d0 520 wait_ms(20);
Fairy_Paolina 6:f5c26372b2d0 521 rangeFinderLeft.getMeas(range);
Fairy_Paolina 10:c57f6a5042d7 522
Fairy_Paolina 9:f34700716f1d 523 if(range < 20) {
Fairy_Paolina 6:f5c26372b2d0 524 wall_follow2(LEFT,BACKWARD,TOOLS, current);
Fairy_Paolina 10:c57f6a5042d7 525
Fairy_Paolina 10:c57f6a5042d7 526 location[0]= current - ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
Fairy_Paolina 10:c57f6a5042d7 527 current= location[0];
Fairy_Paolina 10:c57f6a5042d7 528
Fairy_Paolina 9:f34700716f1d 529 leftEncoder.reset();
Fairy_Paolina 9:f34700716f1d 530 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 531 motors.setMotor0Speed(-MAX_SPEED); //right
Fairy_Paolina 9:f34700716f1d 532 motors.setMotor1Speed(-MAX_SPEED); //left
Fairy_Paolina 9:f34700716f1d 533 while(abs(leftEncoder.getPulses()) < 120 || abs(rightEncoder.getPulses())< 120);
Fairy_Paolina 9:f34700716f1d 534
Fairy_Paolina 9:f34700716f1d 535 leftEncoder.reset();
Fairy_Paolina 9:f34700716f1d 536 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 537 motors.setMotor0Speed(MAX_SPEED); //right
Fairy_Paolina 9:f34700716f1d 538 motors.setMotor1Speed(MAX_SPEED); //left
Fairy_Paolina 9:f34700716f1d 539 while(abs(leftEncoder.getPulses()) < 40 || abs(rightEncoder.getPulses())< 40);
Fairy_Paolina 9:f34700716f1d 540 motors.stopBothMotors();
Fairy_Paolina 10:c57f6a5042d7 541
Fairy_Paolina 9:f34700716f1d 542 wait_ms(500);
Fairy_Paolina 6:f5c26372b2d0 543 leftTurn();
Fairy_Paolina 9:f34700716f1d 544 slightleft();
Fairy_Paolina 8:11ef93eebe07 545 overBump(TOOLS);
Fairy_Paolina 9:f34700716f1d 546 } else {
Fairy_Paolina 9:f34700716f1d 547 location[0]= 77;
Fairy_Paolina 6:f5c26372b2d0 548 leftTurn();
Fairy_Paolina 6:f5c26372b2d0 549 wait_ms(20);
Fairy_Paolina 6:f5c26372b2d0 550 overBump(FIRST_WAVE);
Fairy_Paolina 9:f34700716f1d 551 }
Fairy_Paolina 10:c57f6a5042d7 552
Fairy_Paolina 10:c57f6a5042d7 553 bt.printf("First Wavegap = %f\r\n",location[0]);
Fairy_Paolina 6:f5c26372b2d0 554 }
Fairy_Paolina 10:c57f6a5042d7 555
Fairy_Paolina 9:f34700716f1d 556 void mid_section(float* location, float &current, int* direction)
Fairy_Paolina 9:f34700716f1d 557 {
Fairy_Paolina 10:c57f6a5042d7 558
Fairy_Paolina 9:f34700716f1d 559 motors.begin();
Fairy_Paolina 6:f5c26372b2d0 560
Fairy_Paolina 10:c57f6a5042d7 561 if(IR.getDistance() > 20){
Fairy_Paolina 10:c57f6a5042d7 562 direction[0]= STRAIGHT;
Fairy_Paolina 10:c57f6a5042d7 563 return;
Fairy_Paolina 10:c57f6a5042d7 564 }
Fairy_Paolina 9:f34700716f1d 565
Fairy_Paolina 6:f5c26372b2d0 566 alignWithWall(MID);
Fairy_Paolina 10:c57f6a5042d7 567
Fairy_Paolina 6:f5c26372b2d0 568 bt.printf("mid section current = %f\r\n",current);
Fairy_Paolina 6:f5c26372b2d0 569 wall_follow2(LEFT,FORWARD,MID, current);
Fairy_Paolina 6:f5c26372b2d0 570 current=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
Fairy_Paolina 6:f5c26372b2d0 571 bt.printf("after wf2 current = %f\r\n",current);
Fairy_Paolina 10:c57f6a5042d7 572
Fairy_Paolina 9:f34700716f1d 573 if(current != 0) {
Fairy_Paolina 6:f5c26372b2d0 574 direction[0]= RIGHT;
Fairy_Paolina 6:f5c26372b2d0 575 current+= location[0];
Fairy_Paolina 6:f5c26372b2d0 576 location[1]= current;
Fairy_Paolina 9:f34700716f1d 577 } else {
Fairy_Paolina 9:f34700716f1d 578 current=location[0];
Fairy_Paolina 6:f5c26372b2d0 579 direction[0]= LEFT;
Fairy_Paolina 6:f5c26372b2d0 580 wall_follow2(LEFT,BACKWARD,MID,current);
Fairy_Paolina 6:f5c26372b2d0 581 location[1]= location[0]- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
Fairy_Paolina 6:f5c26372b2d0 582 }
Fairy_Paolina 10:c57f6a5042d7 583
Fairy_Paolina 6:f5c26372b2d0 584 bt.printf("wavegap2 = %f\r\n",location[1]);
Fairy_Paolina 9:f34700716f1d 585 leftTurn();
Fairy_Paolina 8:11ef93eebe07 586 overBump(TOOLS);
Fairy_Paolina 6:f5c26372b2d0 587 // go forward
Fairy_Paolina 6:f5c26372b2d0 588 leftEncoder.reset();
Fairy_Paolina 6:f5c26372b2d0 589 rightEncoder.reset();
Fairy_Paolina 6:f5c26372b2d0 590 motors.setMotor0Speed(0.2*127); //right
Fairy_Paolina 6:f5c26372b2d0 591 motors.setMotor1Speed(0.2*127); //left
Fairy_Paolina 6:f5c26372b2d0 592 while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300);
Fairy_Paolina 5:70ccef3734ae 593 motors.stopBothMotors();
Fairy_Paolina 10:c57f6a5042d7 594
Fairy_Paolina 6:f5c26372b2d0 595 }
Fairy_Paolina 10:c57f6a5042d7 596
Fairy_Paolina 9:f34700716f1d 597 void mid_section2(float* location, float &current, int* direction)
Fairy_Paolina 9:f34700716f1d 598 {
Fairy_Paolina 10:c57f6a5042d7 599
Fairy_Paolina 9:f34700716f1d 600 motors.begin();
Fairy_Paolina 6:f5c26372b2d0 601
Fairy_Paolina 10:c57f6a5042d7 602 if(IR.getDistance() > 20){
Fairy_Paolina 10:c57f6a5042d7 603 direction[0]= STRAIGHT;
Fairy_Paolina 10:c57f6a5042d7 604 return;
Fairy_Paolina 10:c57f6a5042d7 605 }
Fairy_Paolina 6:f5c26372b2d0 606
Fairy_Paolina 6:f5c26372b2d0 607 alignWithWall(MID);
Fairy_Paolina 6:f5c26372b2d0 608 wall_follow2(LEFT,FORWARD,MID, current);
Fairy_Paolina 6:f5c26372b2d0 609 current=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
Fairy_Paolina 10:c57f6a5042d7 610
Fairy_Paolina 9:f34700716f1d 611 if(current != 0) {
Fairy_Paolina 6:f5c26372b2d0 612 direction[1]= RIGHT;
Fairy_Paolina 6:f5c26372b2d0 613 current+= location[1];
Fairy_Paolina 6:f5c26372b2d0 614 location[2]= current;
Fairy_Paolina 9:f34700716f1d 615 } else {
Fairy_Paolina 9:f34700716f1d 616 current=location[1];
Fairy_Paolina 6:f5c26372b2d0 617 direction[1]= LEFT;
Fairy_Paolina 6:f5c26372b2d0 618 wall_follow2(LEFT,BACKWARD,MID,current);
Fairy_Paolina 6:f5c26372b2d0 619 location[2]= location[1]- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
Fairy_Paolina 6:f5c26372b2d0 620 }
Fairy_Paolina 10:c57f6a5042d7 621
Fairy_Paolina 9:f34700716f1d 622 leftTurn();
Fairy_Paolina 8:11ef93eebe07 623 overBump(RIGS);
Fairy_Paolina 9:f34700716f1d 624 }
Fairy_Paolina 10:c57f6a5042d7 625
Fairy_Paolina 9:f34700716f1d 626 void rig_section(float* location, float &current, int* direction, int rig)
Fairy_Paolina 9:f34700716f1d 627 {
Fairy_Paolina 10:c57f6a5042d7 628 float loc;
Fairy_Paolina 10:c57f6a5042d7 629
Fairy_Paolina 10:c57f6a5042d7 630 if(rig == 1) loc= 16;
Fairy_Paolina 10:c57f6a5042d7 631 else if(rig == 2) loc= 37;
Fairy_Paolina 10:c57f6a5042d7 632 else loc = 58;
Fairy_Paolina 10:c57f6a5042d7 633
Fairy_Paolina 10:c57f6a5042d7 634 rightTurn();
Fairy_Paolina 10:c57f6a5042d7 635
Fairy_Paolina 10:c57f6a5042d7 636
Fairy_Paolina 10:c57f6a5042d7 637 if(current > loc){
Fairy_Paolina 10:c57f6a5042d7 638 UntilWall(BACKWARD);
Fairy_Paolina 10:c57f6a5042d7 639 current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
Fairy_Paolina 10:c57f6a5042d7 640 wall_follow2(RIGHT, BACKWARD, RIGS, current);
Fairy_Paolina 10:c57f6a5042d7 641 current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
Fairy_Paolina 10:c57f6a5042d7 642
Fairy_Paolina 10:c57f6a5042d7 643 if((current- loc)>10){
Fairy_Paolina 10:c57f6a5042d7 644 wall_follow2(RIGHT, BACKWARD, RIGS, current);
Fairy_Paolina 10:c57f6a5042d7 645 current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
Fairy_Paolina 10:c57f6a5042d7 646 }
Fairy_Paolina 10:c57f6a5042d7 647 }
Fairy_Paolina 10:c57f6a5042d7 648 else{
Fairy_Paolina 10:c57f6a5042d7 649 UntilWall(FORWARD);
Fairy_Paolina 10:c57f6a5042d7 650 wall_follow2(RIGHT, FORWARD, RIGS, current);
Fairy_Paolina 10:c57f6a5042d7 651 current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
Fairy_Paolina 10:c57f6a5042d7 652
Fairy_Paolina 10:c57f6a5042d7 653 if((current- loc)<-10){
Fairy_Paolina 10:c57f6a5042d7 654 wall_follow2(RIGHT, FORWARD, RIGS, current);
Fairy_Paolina 10:c57f6a5042d7 655 current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
Fairy_Paolina 10:c57f6a5042d7 656 }
Fairy_Paolina 10:c57f6a5042d7 657 }
Fairy_Paolina 10:c57f6a5042d7 658
Fairy_Paolina 10:c57f6a5042d7 659
Fairy_Paolina 8:11ef93eebe07 660 }