Paolina Povolotskaya / NavigationTest

revise

Dependencies:   HCSR04 PID PololuQik2 QEI mbed-rtos Sharp

main.cpp@12:168cb595f98e, 2014-03-29 (annotated)

Committer:
Fairy_Paolina
Date:
Sat Mar 29 22:20:44 2014 +0000
Revision:
12:168cb595f98e
Parent:
11:d67a3958127a
Child:
13:9bad7f74833a
fixed;

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