Paolina Povolotskaya / NavigationTest

revise

Dependencies:   HCSR04 PID PololuQik2 QEI mbed-rtos Sharp

main.cpp@13:9bad7f74833a, 2014-03-30 (annotated)

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