Paolina Povolotskaya / NavigationTest

revise

Dependencies:   HCSR04 PID PololuQik2 QEI mbed-rtos Sharp

main.cpp@8:11ef93eebe07, 2014-03-22 (annotated)

Committer:
Fairy_Paolina
Date:
Sat Mar 22 21:47:14 2014 +0000
Revision:
8:11ef93eebe07
Parent:
7:78745a518957
Child:
9:f34700716f1d
new library;

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 5:70ccef3734ae 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 3:58726d2e11f0 23 #define FORWARD (1)
Fairy_Paolina 3:58726d2e11f0 24 #define BACKWARD (0)
Fairy_Paolina 3:58726d2e11f0 25 #define TOOLS (0)
Fairy_Paolina 3:58726d2e11f0 26 #define MID (1)
Fairy_Paolina 8:11ef93eebe07 27 #define RIGS (2)
Fairy_Paolina 6:f5c26372b2d0 28 #define FIRST_WAVE (0)
Fairy_Paolina 6:f5c26372b2d0 29 #define FAR (1)
Fairy_Paolina 8:11ef93eebe07 30
Fairy_Paolina 5:70ccef3734ae 31
Fairy_Paolina 0:ff94cc47fef7 32 float range, pid_return;
Fairy_Paolina 0:ff94cc47fef7 33 void errFunction(void);
Fairy_Paolina 0:ff94cc47fef7 34 bool cRc;
Fairy_Paolina 5:70ccef3734ae 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 5:70ccef3734ae 47
Fairy_Paolina 5:70ccef3734ae 48
Fairy_Paolina 0:ff94cc47fef7 49 //Functions
Fairy_Paolina 5:70ccef3734ae 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 1:801f0b9a862a 55 void rightTurn(void);
Fairy_Paolina 0:ff94cc47fef7 56 void us_distance(void);
Fairy_Paolina 3:58726d2e11f0 57 void tools_section(float* location, float &current);
Fairy_Paolina 6:f5c26372b2d0 58 void mid_section(float* location, float &current, int* direction);
Fairy_Paolina 6:f5c26372b2d0 59 void mid_section2(float* location, float &current, int* direction);
Fairy_Paolina 6:f5c26372b2d0 60 void rig_section(float* location, float &current, int* direction, int rig);
Fairy_Paolina 8:11ef93eebe07 61 void overBump(int section);
Fairy_Paolina 6:f5c26372b2d0 62 void alignWithWall(int section);
Fairy_Paolina 5:70ccef3734ae 63
Fairy_Paolina 1:801f0b9a862a 64 //Variables
Fairy_Paolina 5:70ccef3734ae 65
Fairy_Paolina 2:3d0be48abcf2 66 int main(void)
Fairy_Paolina 2:3d0be48abcf2 67 {
Fairy_Paolina 3:58726d2e11f0 68 float location[3], current=0;
Fairy_Paolina 3:58726d2e11f0 69 int direction[3];
Fairy_Paolina 8:11ef93eebe07 70 double distance;
Fairy_Paolina 5:70ccef3734ae 71
Fairy_Paolina 0:ff94cc47fef7 72 pc.baud(115200);
Fairy_Paolina 1:801f0b9a862a 73 bt.baud(115200);
Fairy_Paolina 2:3d0be48abcf2 74 motors.begin();
Fairy_Paolina 2:3d0be48abcf2 75
Fairy_Paolina 7:78745a518957 76 bt.printf("START\r\n");
Fairy_Paolina 3:58726d2e11f0 77 //Go to tools
Fairy_Paolina 4:f2333e66ec2c 78 tools_section(location, current);
Fairy_Paolina 6:f5c26372b2d0 79 mid_section(location, current, direction);
Fairy_Paolina 6:f5c26372b2d0 80 mid_section2(location, current, direction);
Fairy_Paolina 8:11ef93eebe07 81 /*while(1){
Fairy_Paolina 8:11ef93eebe07 82 bt.printf("IR %f\r\n", IR.getDistance());
Fairy_Paolina 8:11ef93eebe07 83 wait_ms(200);
Fairy_Paolina 8:11ef93eebe07 84 }
Fairy_Paolina 8:11ef93eebe07 85 */
Fairy_Paolina 5:70ccef3734ae 86
Fairy_Paolina 0:ff94cc47fef7 87 }
Fairy_Paolina 5:70ccef3734ae 88
Fairy_Paolina 2:3d0be48abcf2 89 void errFunction(void)
Fairy_Paolina 2:3d0be48abcf2 90 {
Fairy_Paolina 2:3d0be48abcf2 91 //Nothing
Fairy_Paolina 0:ff94cc47fef7 92 }
Fairy_Paolina 5:70ccef3734ae 93
Fairy_Paolina 0:ff94cc47fef7 94 void us_distance(void)
Fairy_Paolina 0:ff94cc47fef7 95 {
Fairy_Paolina 2:3d0be48abcf2 96 pc.printf("Ultra Sonic\n\r");
Fairy_Paolina 2:3d0be48abcf2 97 rangeFinderLeft.startMeas();
Fairy_Paolina 2:3d0be48abcf2 98 wait_us(20);
Fairy_Paolina 2:3d0be48abcf2 99 if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID)) {
Fairy_Paolina 2:3d0be48abcf2 100 pc.printf("Range = %f\n\r", range);
Fairy_Paolina 2:3d0be48abcf2 101 }
Fairy_Paolina 0:ff94cc47fef7 102 }
Fairy_Paolina 5:70ccef3734ae 103
Fairy_Paolina 3:58726d2e11f0 104 float wall_follow(int side, int direction, int section)
Fairy_Paolina 0:ff94cc47fef7 105 {
Fairy_Paolina 8:11ef93eebe07 106 float location, wavegap=0, set=5;
Fairy_Paolina 5:70ccef3734ae 107 int dir=1;
Fairy_Paolina 3:58726d2e11f0 108
Fairy_Paolina 4:f2333e66ec2c 109 pid1.reset();
Fairy_Paolina 4:f2333e66ec2c 110
Fairy_Paolina 3:58726d2e11f0 111 if(direction == BACKWARD) dir=-1;
Fairy_Paolina 5:70ccef3734ae 112 if(section == TOOLS)set= 10;
Fairy_Paolina 3:58726d2e11f0 113
Fairy_Paolina 3:58726d2e11f0 114 leftEncoder.reset();
Fairy_Paolina 3:58726d2e11f0 115 rightEncoder.reset();
Fairy_Paolina 3:58726d2e11f0 116
Fairy_Paolina 4:f2333e66ec2c 117 location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
Fairy_Paolina 3:58726d2e11f0 118
Fairy_Paolina 6:f5c26372b2d0 119 while(location< 78) {
Fairy_Paolina 4:f2333e66ec2c 120 location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
Fairy_Paolina 3:58726d2e11f0 121
Fairy_Paolina 3:58726d2e11f0 122 pid1.setInputLimits(0, set);
Fairy_Paolina 1:801f0b9a862a 123 pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
Fairy_Paolina 3:58726d2e11f0 124 pid1.setSetPoint(set);
Fairy_Paolina 2:3d0be48abcf2 125 if(side){
Fairy_Paolina 2:3d0be48abcf2 126 rangeFinderLeft.startMeas();
Fairy_Paolina 8:11ef93eebe07 127 wait_ms(38);
Fairy_Paolina 2:3d0be48abcf2 128 rangeFinderLeft.getMeas(range);
Fairy_Paolina 2:3d0be48abcf2 129 }
Fairy_Paolina 2:3d0be48abcf2 130 else{
Fairy_Paolina 2:3d0be48abcf2 131 rangeFinderRight.startMeas();
Fairy_Paolina 8:11ef93eebe07 132 wait_ms(38);
Fairy_Paolina 2:3d0be48abcf2 133 rangeFinderRight.getMeas(range);
Fairy_Paolina 2:3d0be48abcf2 134 pc.printf("%d\r\n",range);
Fairy_Paolina 2:3d0be48abcf2 135 }
Fairy_Paolina 0:ff94cc47fef7 136
Fairy_Paolina 3:58726d2e11f0 137 if(range > 20) {
Fairy_Paolina 4:f2333e66ec2c 138 wavegap=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
Fairy_Paolina 7:78745a518957 139 //bt.printf("wavegap %f\r\n",wavegap);
Fairy_Paolina 3:58726d2e11f0 140 // AT WAVE OPENING!!!!
Fairy_Paolina 6:f5c26372b2d0 141 motors.setMotor1Speed(dir*0.3*127);//left
Fairy_Paolina 6:f5c26372b2d0 142 motors.setMotor0Speed(dir*0.3*127);//right
Fairy_Paolina 3:58726d2e11f0 143 }
Fairy_Paolina 6:f5c26372b2d0 144 else{
Fairy_Paolina 3:58726d2e11f0 145
Fairy_Paolina 6:f5c26372b2d0 146 pid1.setProcessValue(range);
Fairy_Paolina 6:f5c26372b2d0 147 pid_return = pid1.compute();
Fairy_Paolina 5:70ccef3734ae 148
Fairy_Paolina 6:f5c26372b2d0 149 if(pid_return > 0) {
Fairy_Paolina 6:f5c26372b2d0 150 if(side){
Fairy_Paolina 6:f5c26372b2d0 151 motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
Fairy_Paolina 6:f5c26372b2d0 152 motors.setMotor1Speed(dir*MAX_SPEED);//left
Fairy_Paolina 6:f5c26372b2d0 153 }
Fairy_Paolina 6:f5c26372b2d0 154 else{
Fairy_Paolina 6:f5c26372b2d0 155 motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
Fairy_Paolina 6:f5c26372b2d0 156 motors.setMotor0Speed(dir*MAX_SPEED);//right
Fairy_Paolina 6:f5c26372b2d0 157 }
Fairy_Paolina 6:f5c26372b2d0 158 }else if(pid_return < 0) {
Fairy_Paolina 6:f5c26372b2d0 159 if(side){
Fairy_Paolina 6:f5c26372b2d0 160 motors.setMotor0Speed(dir*MAX_SPEED);//right
Fairy_Paolina 6:f5c26372b2d0 161 motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
Fairy_Paolina 6:f5c26372b2d0 162 }
Fairy_Paolina 6:f5c26372b2d0 163 else{
Fairy_Paolina 6:f5c26372b2d0 164 motors.setMotor1Speed(dir*MAX_SPEED);//left
Fairy_Paolina 6:f5c26372b2d0 165 motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
Fairy_Paolina 6:f5c26372b2d0 166 }
Fairy_Paolina 6:f5c26372b2d0 167 }else {
Fairy_Paolina 6:f5c26372b2d0 168 motors.setMotor0Speed(dir*MAX_SPEED);//right
Fairy_Paolina 3:58726d2e11f0 169 motors.setMotor1Speed(dir*MAX_SPEED);//left
Fairy_Paolina 2:3d0be48abcf2 170 }
Fairy_Paolina 0:ff94cc47fef7 171 }
Fairy_Paolina 2:3d0be48abcf2 172 }
Fairy_Paolina 3:58726d2e11f0 173 return wavegap;
Fairy_Paolina 0:ff94cc47fef7 174 }
Fairy_Paolina 5:70ccef3734ae 175
Fairy_Paolina 0:ff94cc47fef7 176 /* MODIFIED WALL_FOLLOW FOR NAVIGATION */
Fairy_Paolina 5:70ccef3734ae 177
Fairy_Paolina 6:f5c26372b2d0 178 void wall_follow2(int side, int direction, int section, float location)
Fairy_Paolina 0:ff94cc47fef7 179 {
Fairy_Paolina 5:70ccef3734ae 180 int SeeWaveGap = false, dir=1;
Fairy_Paolina 8:11ef93eebe07 181 float set=5, loc=0;
Fairy_Paolina 4:f2333e66ec2c 182
Fairy_Paolina 4:f2333e66ec2c 183 pid1.reset();
Fairy_Paolina 3:58726d2e11f0 184
Fairy_Paolina 3:58726d2e11f0 185 if(direction == BACKWARD) dir=-1;
Fairy_Paolina 6:f5c26372b2d0 186 if(section == TOOLS)set= 5;
Fairy_Paolina 3:58726d2e11f0 187
Fairy_Paolina 3:58726d2e11f0 188 leftEncoder.reset();
Fairy_Paolina 3:58726d2e11f0 189 rightEncoder.reset();
Fairy_Paolina 7:78745a518957 190
Fairy_Paolina 8:11ef93eebe07 191 while(loc + dir*location < 80) {
Fairy_Paolina 7:78745a518957 192 loc=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
Fairy_Paolina 6:f5c26372b2d0 193
Fairy_Paolina 5:70ccef3734ae 194 pid1.setInputLimits(0.0, set);
Fairy_Paolina 1:801f0b9a862a 195 pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
Fairy_Paolina 5:70ccef3734ae 196 pid1.setSetPoint(set);
Fairy_Paolina 5:70ccef3734ae 197
Fairy_Paolina 2:3d0be48abcf2 198 if(side){
Fairy_Paolina 2:3d0be48abcf2 199 rangeFinderLeft.startMeas();
Fairy_Paolina 8:11ef93eebe07 200 wait_ms(38);
Fairy_Paolina 2:3d0be48abcf2 201 rangeFinderLeft.getMeas(range);
Fairy_Paolina 2:3d0be48abcf2 202 }
Fairy_Paolina 2:3d0be48abcf2 203 else{
Fairy_Paolina 2:3d0be48abcf2 204 rangeFinderRight.startMeas();
Fairy_Paolina 8:11ef93eebe07 205 wait_ms(38);
Fairy_Paolina 2:3d0be48abcf2 206 rangeFinderRight.getMeas(range);
Fairy_Paolina 2:3d0be48abcf2 207 }
Fairy_Paolina 5:70ccef3734ae 208
Fairy_Paolina 5:70ccef3734ae 209
Fairy_Paolina 0:ff94cc47fef7 210 /*************CHECK FOR WAVE OPENING*****************/
Fairy_Paolina 3:58726d2e11f0 211 /* If after 20 ms the ultrasonic still sees 20+ cm */
Fairy_Paolina 0:ff94cc47fef7 212 /* then robot is at wave opening */
Fairy_Paolina 5:70ccef3734ae 213
Fairy_Paolina 8:11ef93eebe07 214 bt.printf("wall follow 2 range %f\r\n",range);
Fairy_Paolina 8:11ef93eebe07 215 bt.printf("loc+location = %f\r\n", loc+location);
Fairy_Paolina 2:3d0be48abcf2 216 if(range > 20) {
Fairy_Paolina 2:3d0be48abcf2 217 motors.stopBothMotors();
Fairy_Paolina 2:3d0be48abcf2 218 bt.printf("wavegap\r\n");
Fairy_Paolina 2:3d0be48abcf2 219 // AT WAVE OPENING!!!!
Fairy_Paolina 2:3d0be48abcf2 220 break;
Fairy_Paolina 0:ff94cc47fef7 221 }
Fairy_Paolina 5:70ccef3734ae 222
Fairy_Paolina 0:ff94cc47fef7 223 pid1.setProcessValue(range);
Fairy_Paolina 2:3d0be48abcf2 224 pid_return = pid1.compute();
Fairy_Paolina 2:3d0be48abcf2 225 //bt.printf("Range: %f\n PID: %f\r\n", range, pid_return);
Fairy_Paolina 5:70ccef3734ae 226
Fairy_Paolina 2:3d0be48abcf2 227 if(pid_return > 0) {
Fairy_Paolina 2:3d0be48abcf2 228 if(side){
Fairy_Paolina 3:58726d2e11f0 229 motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
Fairy_Paolina 3:58726d2e11f0 230 motors.setMotor1Speed(dir*MAX_SPEED);//left
Fairy_Paolina 2:3d0be48abcf2 231 }
Fairy_Paolina 2:3d0be48abcf2 232 else{
Fairy_Paolina 3:58726d2e11f0 233 motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
Fairy_Paolina 3:58726d2e11f0 234 motors.setMotor0Speed(dir*MAX_SPEED);//right
Fairy_Paolina 2:3d0be48abcf2 235 }
Fairy_Paolina 2:3d0be48abcf2 236 }else if(pid_return < 0) {
Fairy_Paolina 2:3d0be48abcf2 237 if(side){
Fairy_Paolina 3:58726d2e11f0 238 motors.setMotor0Speed(dir*MAX_SPEED);//right
Fairy_Paolina 3:58726d2e11f0 239 motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
Fairy_Paolina 2:3d0be48abcf2 240 }
Fairy_Paolina 2:3d0be48abcf2 241 else{
Fairy_Paolina 3:58726d2e11f0 242 motors.setMotor1Speed(dir*MAX_SPEED);//left
Fairy_Paolina 3:58726d2e11f0 243 motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
Fairy_Paolina 2:3d0be48abcf2 244 }
Fairy_Paolina 2:3d0be48abcf2 245 } else {
Fairy_Paolina 3:58726d2e11f0 246 motors.setMotor0Speed(dir*MAX_SPEED);
Fairy_Paolina 3:58726d2e11f0 247 motors.setMotor1Speed(dir*MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 248 }
Fairy_Paolina 2:3d0be48abcf2 249 }
Fairy_Paolina 6:f5c26372b2d0 250 motors.stopBothMotors();
Fairy_Paolina 0:ff94cc47fef7 251 }
Fairy_Paolina 5:70ccef3734ae 252
Fairy_Paolina 5:70ccef3734ae 253
Fairy_Paolina 6:f5c26372b2d0 254 void alignWithWall(int section){
Fairy_Paolina 6:f5c26372b2d0 255 float usValue = 0;
Fairy_Paolina 6:f5c26372b2d0 256
Fairy_Paolina 6:f5c26372b2d0 257 if(section == TOOLS){
Fairy_Paolina 6:f5c26372b2d0 258 // turn at an angle
Fairy_Paolina 6:f5c26372b2d0 259 leftEncoder.reset();
Fairy_Paolina 6:f5c26372b2d0 260 rightEncoder.reset();
Fairy_Paolina 6:f5c26372b2d0 261 motors.setMotor0Speed(-MAX_SPEED); //right
Fairy_Paolina 6:f5c26372b2d0 262 motors.setMotor1Speed(0.4*MAX_SPEED); //left
Fairy_Paolina 6:f5c26372b2d0 263 while(rightEncoder.getPulses()>-1000);
Fairy_Paolina 6:f5c26372b2d0 264 motors.stopBothMotors();
Fairy_Paolina 6:f5c26372b2d0 265
Fairy_Paolina 6:f5c26372b2d0 266 //go backwards toward wall
Fairy_Paolina 6:f5c26372b2d0 267 leftEncoder.reset();
Fairy_Paolina 6:f5c26372b2d0 268 rightEncoder.reset();
Fairy_Paolina 6:f5c26372b2d0 269 motors.setMotor0Speed(-MAX_SPEED); //right
Fairy_Paolina 6:f5c26372b2d0 270 motors.setMotor1Speed(-MAX_SPEED); //left
Fairy_Paolina 7:78745a518957 271 while(abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300);
Fairy_Paolina 6:f5c26372b2d0 272
Fairy_Paolina 6:f5c26372b2d0 273 motors.stopBothMotors();
Fairy_Paolina 6:f5c26372b2d0 274
Fairy_Paolina 6:f5c26372b2d0 275 // turn left towards wall
Fairy_Paolina 6:f5c26372b2d0 276 leftEncoder.reset();
Fairy_Paolina 6:f5c26372b2d0 277 rightEncoder.reset();
Fairy_Paolina 6:f5c26372b2d0 278 motors.setMotor0Speed(MAX_SPEED); //right
Fairy_Paolina 6:f5c26372b2d0 279 motors.setMotor1Speed(-MAX_SPEED); //left
Fairy_Paolina 6:f5c26372b2d0 280 while(rightEncoder.getPulses() < 100);
Fairy_Paolina 6:f5c26372b2d0 281
Fairy_Paolina 6:f5c26372b2d0 282 motors.stopBothMotors();
Fairy_Paolina 7:78745a518957 283
Fairy_Paolina 7:78745a518957 284 motors.setMotor0Speed(0.7*MAX_SPEED); //right
Fairy_Paolina 7:78745a518957 285 motors.setMotor1Speed(-0.7*MAX_SPEED); //left
Fairy_Paolina 7:78745a518957 286 } else {
Fairy_Paolina 8:11ef93eebe07 287 rightTurn();
Fairy_Paolina 7:78745a518957 288 motors.setMotor0Speed(-0.7*MAX_SPEED); //right
Fairy_Paolina 7:78745a518957 289 motors.setMotor1Speed(0.7*MAX_SPEED); //left
Fairy_Paolina 6:f5c26372b2d0 290 }
Fairy_Paolina 6:f5c26372b2d0 291
Fairy_Paolina 6:f5c26372b2d0 292 usValue = 0;
Fairy_Paolina 6:f5c26372b2d0 293 while(1){
Fairy_Paolina 2:3d0be48abcf2 294 rangeFinderLeft.startMeas();
Fairy_Paolina 6:f5c26372b2d0 295 wait_ms(20);
Fairy_Paolina 6:f5c26372b2d0 296 rangeFinderLeft.getMeas(range);
Fairy_Paolina 7:78745a518957 297 //bt.printf("Range %f \t OldValue %f\n\r",range, usValue);
Fairy_Paolina 6:f5c26372b2d0 298 if(range > usValue && usValue != 0 && range < 25){
Fairy_Paolina 0:ff94cc47fef7 299 break;
Fairy_Paolina 2:3d0be48abcf2 300 } else {
Fairy_Paolina 6:f5c26372b2d0 301 usValue = range;
Fairy_Paolina 0:ff94cc47fef7 302 }
Fairy_Paolina 2:3d0be48abcf2 303 }
Fairy_Paolina 6:f5c26372b2d0 304 motors.stopBothMotors();
Fairy_Paolina 5:70ccef3734ae 305 }
Fairy_Paolina 5:70ccef3734ae 306
Fairy_Paolina 1:801f0b9a862a 307 void rightTurn(void)
Fairy_Paolina 1:801f0b9a862a 308 {
Fairy_Paolina 1:801f0b9a862a 309 leftEncoder.reset();
Fairy_Paolina 2:3d0be48abcf2 310 rightEncoder.reset();
Fairy_Paolina 5:70ccef3734ae 311 motors.setMotor0Speed(-0.4*127);//right
Fairy_Paolina 5:70ccef3734ae 312 motors.setMotor1Speed(0.4*127);//left
Fairy_Paolina 6:f5c26372b2d0 313 while(leftEncoder.getPulses()<1000 || rightEncoder.getPulses()>-1000);
Fairy_Paolina 1:801f0b9a862a 314 motors.stopBothMotors();
Fairy_Paolina 1:801f0b9a862a 315 }
Fairy_Paolina 5:70ccef3734ae 316
Fairy_Paolina 1:801f0b9a862a 317 void leftTurn(void)
Fairy_Paolina 5:70ccef3734ae 318 {
Fairy_Paolina 1:801f0b9a862a 319 leftEncoder.reset();
Fairy_Paolina 2:3d0be48abcf2 320 rightEncoder.reset();
Fairy_Paolina 2:3d0be48abcf2 321 motors.setMotor0Speed(0.4*127);// right
Fairy_Paolina 2:3d0be48abcf2 322 motors.setMotor1Speed(-0.4*127);// left
Fairy_Paolina 8:11ef93eebe07 323 while(abs(leftEncoder.getPulses())<1000 || rightEncoder.getPulses()<1000);
Fairy_Paolina 5:70ccef3734ae 324 motors.stopBothMotors();
Fairy_Paolina 5:70ccef3734ae 325 }
Fairy_Paolina 5:70ccef3734ae 326
Fairy_Paolina 8:11ef93eebe07 327
Fairy_Paolina 8:11ef93eebe07 328 void overBump(int section){
Fairy_Paolina 6:f5c26372b2d0 329 int preLeft=5000, preRight=5000 ;
Fairy_Paolina 3:58726d2e11f0 330
Fairy_Paolina 3:58726d2e11f0 331 leftEncoder.reset();
Fairy_Paolina 3:58726d2e11f0 332 rightEncoder.reset();
Fairy_Paolina 6:f5c26372b2d0 333 motors.setMotor0Speed(0.15*127); //right
Fairy_Paolina 6:f5c26372b2d0 334 motors.setMotor1Speed(0.15*127); //left
Fairy_Paolina 8:11ef93eebe07 335 while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.getDistance() >20 && preLeft!=0){
Fairy_Paolina 8:11ef93eebe07 336 preLeft=leftEncoder.getPulses();
Fairy_Paolina 8:11ef93eebe07 337 preRight=rightEncoder.getPulses();
Fairy_Paolina 8:11ef93eebe07 338 wait_ms(200);
Fairy_Paolina 8:11ef93eebe07 339 bt.printf(" first while left %d right %d \r\n", preLeft, preRight);
Fairy_Paolina 8:11ef93eebe07 340 if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
Fairy_Paolina 8:11ef93eebe07 341 }
Fairy_Paolina 8:11ef93eebe07 342
Fairy_Paolina 8:11ef93eebe07 343 motors.stopBothMotors();
Fairy_Paolina 8:11ef93eebe07 344 motors.setMotor0Speed(0.15*127); //right
Fairy_Paolina 8:11ef93eebe07 345 motors.setMotor1Speed(0.15*127); //left
Fairy_Paolina 8:11ef93eebe07 346 preLeft=preRight=5000 ;
Fairy_Paolina 8:11ef93eebe07 347 leftEncoder.reset();
Fairy_Paolina 8:11ef93eebe07 348 rightEncoder.reset();
Fairy_Paolina 8:11ef93eebe07 349
Fairy_Paolina 8:11ef93eebe07 350 while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.getDistance() >20 && preLeft!=0){
Fairy_Paolina 6:f5c26372b2d0 351 preLeft=leftEncoder.getPulses();
Fairy_Paolina 6:f5c26372b2d0 352 preRight=rightEncoder.getPulses();
Fairy_Paolina 8:11ef93eebe07 353 bt.printf("second while left %d right %d \r\n", preLeft, preRight);
Fairy_Paolina 8:11ef93eebe07 354 wait_ms(200);
Fairy_Paolina 8:11ef93eebe07 355 if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
Fairy_Paolina 8:11ef93eebe07 356 }
Fairy_Paolina 8:11ef93eebe07 357
Fairy_Paolina 8:11ef93eebe07 358 motors.stopBothMotors();
Fairy_Paolina 4:f2333e66ec2c 359
Fairy_Paolina 8:11ef93eebe07 360 preLeft=preRight=5000 ;
Fairy_Paolina 8:11ef93eebe07 361 leftEncoder.reset();
Fairy_Paolina 8:11ef93eebe07 362 rightEncoder.reset();
Fairy_Paolina 8:11ef93eebe07 363 motors.setMotor0Speed(MAX_SPEED); //right
Fairy_Paolina 8:11ef93eebe07 364 motors.setMotor1Speed(MAX_SPEED); //left
Fairy_Paolina 8:11ef93eebe07 365
Fairy_Paolina 8:11ef93eebe07 366 if(section == TOOLS){
Fairy_Paolina 8:11ef93eebe07 367 while(IR.getDistance() > 20 ) {
Fairy_Paolina 8:11ef93eebe07 368 bt.printf("IR %f\r\n", IR.getDistance());
Fairy_Paolina 8:11ef93eebe07 369
Fairy_Paolina 6:f5c26372b2d0 370 preLeft=leftEncoder.getPulses();
Fairy_Paolina 6:f5c26372b2d0 371 preRight=rightEncoder.getPulses();
Fairy_Paolina 8:11ef93eebe07 372 bt.printf("third while left %d right %d \r\n", preLeft, preRight);
Fairy_Paolina 6:f5c26372b2d0 373 wait_ms(20);
Fairy_Paolina 8:11ef93eebe07 374 if(leftEncoder.getPulses() == preLeft && rightEncoder.getPulses()== preRight){
Fairy_Paolina 8:11ef93eebe07 375 motors.setMotor0Speed(0.4*127); //right
Fairy_Paolina 8:11ef93eebe07 376 motors.setMotor1Speed(0.4*127); //left
Fairy_Paolina 8:11ef93eebe07 377 }
Fairy_Paolina 8:11ef93eebe07 378 }
Fairy_Paolina 6:f5c26372b2d0 379 }
Fairy_Paolina 8:11ef93eebe07 380 else while((abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses())< 300));
Fairy_Paolina 8:11ef93eebe07 381
Fairy_Paolina 3:58726d2e11f0 382 motors.stopBothMotors();
Fairy_Paolina 8:11ef93eebe07 383 wait(2);
Fairy_Paolina 8:11ef93eebe07 384
Fairy_Paolina 3:58726d2e11f0 385 }
Fairy_Paolina 5:70ccef3734ae 386
Fairy_Paolina 3:58726d2e11f0 387 void tools_section(float* location, float &current){
Fairy_Paolina 3:58726d2e11f0 388
Fairy_Paolina 6:f5c26372b2d0 389 wall_follow(LEFT,FORWARD, TOOLS);
Fairy_Paolina 6:f5c26372b2d0 390 // current position in reference to the starting position
Fairy_Paolina 4:f2333e66ec2c 391 current=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
Fairy_Paolina 6:f5c26372b2d0 392 bt.printf("current %f \r\n",current);
Fairy_Paolina 3:58726d2e11f0 393
Fairy_Paolina 5:70ccef3734ae 394 motors.stopBothMotors();
Fairy_Paolina 5:70ccef3734ae 395
Fairy_Paolina 6:f5c26372b2d0 396 //Tool aquiring
Fairy_Paolina 6:f5c26372b2d0 397 wait(2);
Fairy_Paolina 6:f5c26372b2d0 398 // After tool is aquired
Fairy_Paolina 6:f5c26372b2d0 399
Fairy_Paolina 6:f5c26372b2d0 400 alignWithWall(TOOLS);
Fairy_Paolina 6:f5c26372b2d0 401 current-=8;
Fairy_Paolina 6:f5c26372b2d0 402 wait_ms(100);
Fairy_Paolina 6:f5c26372b2d0 403
Fairy_Paolina 6:f5c26372b2d0 404 rangeFinderLeft.startMeas();
Fairy_Paolina 6:f5c26372b2d0 405 wait_ms(20);
Fairy_Paolina 6:f5c26372b2d0 406 rangeFinderLeft.getMeas(range);
Fairy_Paolina 6:f5c26372b2d0 407
Fairy_Paolina 6:f5c26372b2d0 408 if(range < 20){
Fairy_Paolina 6:f5c26372b2d0 409 wall_follow2(LEFT,BACKWARD,TOOLS, current);
Fairy_Paolina 6:f5c26372b2d0 410 location[0]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
Fairy_Paolina 6:f5c26372b2d0 411 current= location[0];
Fairy_Paolina 6:f5c26372b2d0 412
Fairy_Paolina 6:f5c26372b2d0 413 leftTurn();
Fairy_Paolina 8:11ef93eebe07 414 overBump(TOOLS);
Fairy_Paolina 6:f5c26372b2d0 415 }
Fairy_Paolina 6:f5c26372b2d0 416 else{
Fairy_Paolina 6:f5c26372b2d0 417 location[0]= 77;
Fairy_Paolina 6:f5c26372b2d0 418 leftTurn();
Fairy_Paolina 6:f5c26372b2d0 419 wait_ms(20);
Fairy_Paolina 6:f5c26372b2d0 420 overBump(FIRST_WAVE);
Fairy_Paolina 6:f5c26372b2d0 421 }
Fairy_Paolina 6:f5c26372b2d0 422
Fairy_Paolina 6:f5c26372b2d0 423 bt.printf("wavegap = %f\r\n",location[0]);
Fairy_Paolina 6:f5c26372b2d0 424 }
Fairy_Paolina 8:11ef93eebe07 425
Fairy_Paolina 6:f5c26372b2d0 426 void mid_section(float* location, float &current, int* direction){
Fairy_Paolina 6:f5c26372b2d0 427
Fairy_Paolina 6:f5c26372b2d0 428 motors.begin();
Fairy_Paolina 6:f5c26372b2d0 429 alignWithWall(MID);
Fairy_Paolina 8:11ef93eebe07 430
Fairy_Paolina 6:f5c26372b2d0 431 bt.printf("mid section current = %f\r\n",current);
Fairy_Paolina 6:f5c26372b2d0 432 wall_follow2(LEFT,FORWARD,MID, current);
Fairy_Paolina 6:f5c26372b2d0 433 current=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
Fairy_Paolina 6:f5c26372b2d0 434 bt.printf("after wf2 current = %f\r\n",current);
Fairy_Paolina 6:f5c26372b2d0 435
Fairy_Paolina 6:f5c26372b2d0 436 if(current != 0){
Fairy_Paolina 6:f5c26372b2d0 437 direction[0]= RIGHT;
Fairy_Paolina 6:f5c26372b2d0 438 current+= location[0];
Fairy_Paolina 6:f5c26372b2d0 439 location[1]= current;
Fairy_Paolina 6:f5c26372b2d0 440 }
Fairy_Paolina 6:f5c26372b2d0 441 else{
Fairy_Paolina 6:f5c26372b2d0 442 current=location[0];
Fairy_Paolina 6:f5c26372b2d0 443 direction[0]= LEFT;
Fairy_Paolina 6:f5c26372b2d0 444 wall_follow2(LEFT,BACKWARD,MID,current);
Fairy_Paolina 6:f5c26372b2d0 445 location[1]= location[0]- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
Fairy_Paolina 6:f5c26372b2d0 446 }
Fairy_Paolina 6:f5c26372b2d0 447
Fairy_Paolina 6:f5c26372b2d0 448 bt.printf("wavegap2 = %f\r\n",location[1]);
Fairy_Paolina 6:f5c26372b2d0 449 leftTurn();
Fairy_Paolina 8:11ef93eebe07 450 overBump(TOOLS);
Fairy_Paolina 6:f5c26372b2d0 451 // go forward
Fairy_Paolina 6:f5c26372b2d0 452 leftEncoder.reset();
Fairy_Paolina 6:f5c26372b2d0 453 rightEncoder.reset();
Fairy_Paolina 6:f5c26372b2d0 454 motors.setMotor0Speed(0.2*127); //right
Fairy_Paolina 6:f5c26372b2d0 455 motors.setMotor1Speed(0.2*127); //left
Fairy_Paolina 6:f5c26372b2d0 456 while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300);
Fairy_Paolina 5:70ccef3734ae 457 motors.stopBothMotors();
Fairy_Paolina 5:70ccef3734ae 458
Fairy_Paolina 6:f5c26372b2d0 459 }
Fairy_Paolina 6:f5c26372b2d0 460
Fairy_Paolina 6:f5c26372b2d0 461 void mid_section2(float* location, float &current, int* direction){
Fairy_Paolina 6:f5c26372b2d0 462
Fairy_Paolina 6:f5c26372b2d0 463 motors.begin();
Fairy_Paolina 6:f5c26372b2d0 464 alignWithWall(MID);
Fairy_Paolina 6:f5c26372b2d0 465 wall_follow2(LEFT,FORWARD,MID, current);
Fairy_Paolina 6:f5c26372b2d0 466 current=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
Fairy_Paolina 5:70ccef3734ae 467
Fairy_Paolina 6:f5c26372b2d0 468 if(current != 0){
Fairy_Paolina 6:f5c26372b2d0 469 direction[1]= RIGHT;
Fairy_Paolina 6:f5c26372b2d0 470 current+= location[1];
Fairy_Paolina 6:f5c26372b2d0 471 location[2]= current;
Fairy_Paolina 6:f5c26372b2d0 472 }
Fairy_Paolina 6:f5c26372b2d0 473 else{
Fairy_Paolina 6:f5c26372b2d0 474 current=location[1];
Fairy_Paolina 6:f5c26372b2d0 475 direction[1]= LEFT;
Fairy_Paolina 6:f5c26372b2d0 476 wall_follow2(LEFT,BACKWARD,MID,current);
Fairy_Paolina 6:f5c26372b2d0 477 location[2]= location[1]- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
Fairy_Paolina 6:f5c26372b2d0 478 }
Fairy_Paolina 5:70ccef3734ae 479
Fairy_Paolina 6:f5c26372b2d0 480 leftTurn();
Fairy_Paolina 8:11ef93eebe07 481 overBump(RIGS);
Fairy_Paolina 6:f5c26372b2d0 482 }
Fairy_Paolina 8:11ef93eebe07 483
Fairy_Paolina 6:f5c26372b2d0 484 void rig_section(float* location, float &current, int* direction, int rig){
Fairy_Paolina 5:70ccef3734ae 485
Fairy_Paolina 5:70ccef3734ae 486
Fairy_Paolina 8:11ef93eebe07 487 }