Paolina Povolotskaya / NavigationTest

revise

Dependencies:   HCSR04 PID PololuQik2 QEI mbed-rtos Sharp

main.cpp@11:d67a3958127a, 2014-03-29 (annotated)

Committer:
Fairy_Paolina
Date:
Sat Mar 29 20:21:55 2014 +0000
Revision:
11:d67a3958127a
Parent:
10:c57f6a5042d7
Child:
12:168cb595f98e
revise;

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