Justin Carr / NavigationTest_

Yeah

Dependencies:   HCSR04 PID PololuQik2 QEI Sharp mbed-rtos

Fork of NavigationTest by Paolina Povolotskaya

main.cpp@7:78745a518957, 2014-03-21 (annotated)

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