Justin Carr / NavigationTest_

Yeah

Dependencies:   HCSR04 PID PololuQik2 QEI Sharp mbed-rtos

Fork of NavigationTest by Paolina Povolotskaya

main.cpp@6:f5c26372b2d0, 2014-03-21 (annotated)

Committer:
Fairy_Paolina
Date:
Fri Mar 21 21:26:50 2014 +0000
Revision:
6:f5c26372b2d0
Parent:
5:70ccef3734ae
Child:
7:78745a518957
revise 3/21;

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