Paolina Povolotskaya / NavigationTest

revise

Dependencies:   HCSR04 PID PololuQik2 QEI mbed-rtos Sharp

main.cpp@4:f2333e66ec2c, 2014-03-18 (annotated)

Committer:
Fairy_Paolina
Date:
Tue Mar 18 23:11:28 2014 +0000
Revision:
4:f2333e66ec2c
Parent:
3:58726d2e11f0
Child:
5:70ccef3734ae
revise 3/18;

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 0:ff94cc47fef7 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 3:58726d2e11f0 26 #define RIGS (2)
Fairy_Paolina 0:ff94cc47fef7 27
Fairy_Paolina 0:ff94cc47fef7 28 float range, pid_return;
Fairy_Paolina 0:ff94cc47fef7 29 void errFunction(void);
Fairy_Paolina 0:ff94cc47fef7 30 bool cRc;
Fairy_Paolina 0:ff94cc47fef7 31
Fairy_Paolina 0:ff94cc47fef7 32 //Hardware Initialization
Fairy_Paolina 0:ff94cc47fef7 33 Serial bt(p13,p14);
Fairy_Paolina 0:ff94cc47fef7 34 Serial pc(USBTX,USBRX);
Fairy_Paolina 2:3d0be48abcf2 35 HCSR04 rangeFinderLeft( PIN_TRIGGERL, PIN_ECHOL );
Fairy_Paolina 2:3d0be48abcf2 36 HCSR04 rangeFinderRight( PIN_TRIGGERR, PIN_ECHOR );
Fairy_Paolina 2:3d0be48abcf2 37 PID pid1(15.0,0.0,4.0,0.02);
Fairy_Paolina 0:ff94cc47fef7 38 PololuQik2 motors(p9, p10, p8, p15, errFunction, cRc);
Fairy_Paolina 4:f2333e66ec2c 39 QEI rightEncoder(p17,p18,NC,PPR,QEI::X4_ENCODING);
Fairy_Paolina 4:f2333e66ec2c 40 QEI leftEncoder(p16,p15,NC,PPR,QEI::X4_ENCODING);
Fairy_Paolina 1:801f0b9a862a 41 //InterruptIn encoder(p29);
Fairy_Paolina 1:801f0b9a862a 42
Fairy_Paolina 0:ff94cc47fef7 43
Fairy_Paolina 0:ff94cc47fef7 44 //Functions
Fairy_Paolina 0:ff94cc47fef7 45
Fairy_Paolina 3:58726d2e11f0 46 float wall_follow(int side, int direction, int section);
Fairy_Paolina 4:f2333e66ec2c 47 void wall_follow2(int side, int direction, int section);
Fairy_Paolina 0:ff94cc47fef7 48 void wall_follow3(int &currentLocation, int &WaveOpening);
Fairy_Paolina 1:801f0b9a862a 49 void leftTurn(void);
Fairy_Paolina 1:801f0b9a862a 50 void rightTurn(void);
Fairy_Paolina 0:ff94cc47fef7 51 void us_distance(void);
Fairy_Paolina 3:58726d2e11f0 52 void tools_section(float* location, float &current);
Fairy_Paolina 3:58726d2e11f0 53 void overBump(void);
Fairy_Paolina 0:ff94cc47fef7 54
Fairy_Paolina 1:801f0b9a862a 55 //Variables
Fairy_Paolina 0:ff94cc47fef7 56
Fairy_Paolina 2:3d0be48abcf2 57 int main(void)
Fairy_Paolina 2:3d0be48abcf2 58 {
Fairy_Paolina 3:58726d2e11f0 59 float location[3], current=0;
Fairy_Paolina 3:58726d2e11f0 60 int direction[3];
Fairy_Paolina 0:ff94cc47fef7 61
Fairy_Paolina 0:ff94cc47fef7 62 pc.baud(115200);
Fairy_Paolina 1:801f0b9a862a 63 bt.baud(115200);
Fairy_Paolina 2:3d0be48abcf2 64 motors.begin();
Fairy_Paolina 2:3d0be48abcf2 65
Fairy_Paolina 2:3d0be48abcf2 66
Fairy_Paolina 4:f2333e66ec2c 67 //leftEncoder.reset();
Fairy_Paolina 4:f2333e66ec2c 68 //rightEncoder.reset();
Fairy_Paolina 4:f2333e66ec2c 69 //motors.setMotor0Speed(MAX_SPEED); //right
Fairy_Paolina 4:f2333e66ec2c 70 //motors.setMotor1Speed(MAX_SPEED); //left
Fairy_Paolina 3:58726d2e11f0 71
Fairy_Paolina 4:f2333e66ec2c 72 //while((abs(leftEncoder.getPulses())/(PPR) + abs(rightEncoder.getPulses())/(PPR))/2 < 3)
Fairy_Paolina 4:f2333e66ec2c 73
Fairy_Paolina 3:58726d2e11f0 74
Fairy_Paolina 2:3d0be48abcf2 75
Fairy_Paolina 3:58726d2e11f0 76 //Go to tools
Fairy_Paolina 4:f2333e66ec2c 77 tools_section(location, current);
Fairy_Paolina 4:f2333e66ec2c 78 /*
Fairy_Paolina 3:58726d2e11f0 79 //////////////////////////////// without predefined wavegaps//////////////////////////////////////////////
Fairy_Paolina 3:58726d2e11f0 80 current=0;
Fairy_Paolina 3:58726d2e11f0 81 if(location[0]< 75){
Fairy_Paolina 3:58726d2e11f0 82 turnRight();
Fairy_Paolina 3:58726d2e11f0 83 current=wall_follow(LEFT,FORWARD);
Fairy_Paolina 3:58726d2e11f0 84 if(current == 0)turnLeft();
Fairy_Paolina 3:58726d2e11f0 85 else{
Fairy_Paolina 3:58726d2e11f0 86 direction[0]= RIGHT;
Fairy_Paolina 3:58726d2e11f0 87 turnLeft();
Fairy_Paolina 3:58726d2e11f0 88 overBump();
Fairy_Paolina 3:58726d2e11f0 89 }
Fairy_Paolina 3:58726d2e11f0 90 }
Fairy_Paolina 3:58726d2e11f0 91 else if(location[0]>=75 || current == 0){
Fairy_Paolina 3:58726d2e11f0 92 turnLeft();
Fairy_Paolina 3:58726d2e11f0 93 wall_follow2(RIGHT,FORWARD);
Fairy_Paolina 3:58726d2e11f0 94 }
Fairy_Paolina 3:58726d2e11f0 95
Fairy_Paolina 3:58726d2e11f0 96
Fairy_Paolina 3:58726d2e11f0 97
Fairy_Paolina 3:58726d2e11f0 98
Fairy_Paolina 3:58726d2e11f0 99
Fairy_Paolina 3:58726d2e11f0 100
Fairy_Paolina 3:58726d2e11f0 101 // left or right
Fairy_Paolina 3:58726d2e11f0 102
Fairy_Paolina 3:58726d2e11f0 103
Fairy_Paolina 4:f2333e66ec2c 104 location[1]=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
Fairy_Paolina 3:58726d2e11f0 105
Fairy_Paolina 3:58726d2e11f0 106
Fairy_Paolina 3:58726d2e11f0 107
Fairy_Paolina 3:58726d2e11f0 108
Fairy_Paolina 3:58726d2e11f0 109
Fairy_Paolina 3:58726d2e11f0 110
Fairy_Paolina 3:58726d2e11f0 111
Fairy_Paolina 2:3d0be48abcf2 112 leftTurn();
Fairy_Paolina 3:58726d2e11f0 113 //wall_follow2(RIGHT);
Fairy_Paolina 1:801f0b9a862a 114 rightTurn();
Fairy_Paolina 0:ff94cc47fef7 115
Fairy_Paolina 2:3d0be48abcf2 116
Fairy_Paolina 2:3d0be48abcf2 117
Fairy_Paolina 2:3d0be48abcf2 118 bt.printf("LOCATION %f\n\r",location);
Fairy_Paolina 2:3d0be48abcf2 119
Fairy_Paolina 2:3d0be48abcf2 120 motors.stopBothMotors();
Fairy_Paolina 2:3d0be48abcf2 121 // leftTurn();
Fairy_Paolina 2:3d0be48abcf2 122 // wait(1);
Fairy_Paolina 2:3d0be48abcf2 123 // rightTurn();
Fairy_Paolina 3:58726d2e11f0 124 */
Fairy_Paolina 2:3d0be48abcf2 125
Fairy_Paolina 0:ff94cc47fef7 126 }
Fairy_Paolina 0:ff94cc47fef7 127
Fairy_Paolina 2:3d0be48abcf2 128 void errFunction(void)
Fairy_Paolina 2:3d0be48abcf2 129 {
Fairy_Paolina 2:3d0be48abcf2 130 //Nothing
Fairy_Paolina 0:ff94cc47fef7 131 }
Fairy_Paolina 0:ff94cc47fef7 132
Fairy_Paolina 0:ff94cc47fef7 133 void us_distance(void)
Fairy_Paolina 0:ff94cc47fef7 134 {
Fairy_Paolina 2:3d0be48abcf2 135 pc.printf("Ultra Sonic\n\r");
Fairy_Paolina 2:3d0be48abcf2 136 rangeFinderLeft.startMeas();
Fairy_Paolina 2:3d0be48abcf2 137 wait_us(20);
Fairy_Paolina 2:3d0be48abcf2 138 if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID)) {
Fairy_Paolina 2:3d0be48abcf2 139 pc.printf("Range = %f\n\r", range);
Fairy_Paolina 2:3d0be48abcf2 140 }
Fairy_Paolina 0:ff94cc47fef7 141 }
Fairy_Paolina 0:ff94cc47fef7 142
Fairy_Paolina 3:58726d2e11f0 143 float wall_follow(int side, int direction, int section)
Fairy_Paolina 0:ff94cc47fef7 144 {
Fairy_Paolina 3:58726d2e11f0 145 float location, wavegap;
Fairy_Paolina 3:58726d2e11f0 146 int dir=1, set=5;
Fairy_Paolina 3:58726d2e11f0 147
Fairy_Paolina 4:f2333e66ec2c 148 pid1.reset();
Fairy_Paolina 4:f2333e66ec2c 149
Fairy_Paolina 3:58726d2e11f0 150 if(direction == BACKWARD) dir=-1;
Fairy_Paolina 3:58726d2e11f0 151 if(section == TOOLS)set= 9;
Fairy_Paolina 3:58726d2e11f0 152
Fairy_Paolina 3:58726d2e11f0 153 leftEncoder.reset();
Fairy_Paolina 3:58726d2e11f0 154 rightEncoder.reset();
Fairy_Paolina 3:58726d2e11f0 155
Fairy_Paolina 4:f2333e66ec2c 156 location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
Fairy_Paolina 3:58726d2e11f0 157
Fairy_Paolina 3:58726d2e11f0 158 while(location< 75) {
Fairy_Paolina 4:f2333e66ec2c 159 location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
Fairy_Paolina 3:58726d2e11f0 160
Fairy_Paolina 3:58726d2e11f0 161 pid1.setInputLimits(0, set);
Fairy_Paolina 1:801f0b9a862a 162 pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
Fairy_Paolina 3:58726d2e11f0 163 pid1.setSetPoint(set);
Fairy_Paolina 2:3d0be48abcf2 164 if(side){
Fairy_Paolina 2:3d0be48abcf2 165 rangeFinderLeft.startMeas();
Fairy_Paolina 2:3d0be48abcf2 166 wait_ms(20);
Fairy_Paolina 2:3d0be48abcf2 167 rangeFinderLeft.getMeas(range);
Fairy_Paolina 2:3d0be48abcf2 168 }
Fairy_Paolina 2:3d0be48abcf2 169 else{
Fairy_Paolina 2:3d0be48abcf2 170 rangeFinderRight.startMeas();
Fairy_Paolina 2:3d0be48abcf2 171 wait_ms(20);
Fairy_Paolina 2:3d0be48abcf2 172 rangeFinderRight.getMeas(range);
Fairy_Paolina 2:3d0be48abcf2 173 pc.printf("%d\r\n",range);
Fairy_Paolina 2:3d0be48abcf2 174 }
Fairy_Paolina 0:ff94cc47fef7 175
Fairy_Paolina 3:58726d2e11f0 176 if(range > 20) {
Fairy_Paolina 4:f2333e66ec2c 177 wavegap=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
Fairy_Paolina 3:58726d2e11f0 178 bt.printf("wavegap %f\r\n",wavegap);
Fairy_Paolina 3:58726d2e11f0 179 // AT WAVE OPENING!!!!
Fairy_Paolina 3:58726d2e11f0 180 }
Fairy_Paolina 3:58726d2e11f0 181
Fairy_Paolina 0:ff94cc47fef7 182 pid1.setProcessValue(range);
Fairy_Paolina 2:3d0be48abcf2 183 pid_return = pid1.compute();
Fairy_Paolina 2:3d0be48abcf2 184
Fairy_Paolina 2:3d0be48abcf2 185 if(pid_return > 0) {
Fairy_Paolina 2:3d0be48abcf2 186 if(side){
Fairy_Paolina 3:58726d2e11f0 187 motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
Fairy_Paolina 3:58726d2e11f0 188 motors.setMotor1Speed(dir*MAX_SPEED);//left
Fairy_Paolina 2:3d0be48abcf2 189 }
Fairy_Paolina 2:3d0be48abcf2 190 else{
Fairy_Paolina 3:58726d2e11f0 191 motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
Fairy_Paolina 3:58726d2e11f0 192 motors.setMotor0Speed(dir*MAX_SPEED);//right
Fairy_Paolina 2:3d0be48abcf2 193 }
Fairy_Paolina 2:3d0be48abcf2 194 }else if(pid_return < 0) {
Fairy_Paolina 2:3d0be48abcf2 195 if(side){
Fairy_Paolina 3:58726d2e11f0 196 motors.setMotor0Speed(dir*MAX_SPEED);//right
Fairy_Paolina 3:58726d2e11f0 197 motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
Fairy_Paolina 2:3d0be48abcf2 198 }
Fairy_Paolina 2:3d0be48abcf2 199 else{
Fairy_Paolina 3:58726d2e11f0 200 motors.setMotor1Speed(dir*MAX_SPEED);//left
Fairy_Paolina 3:58726d2e11f0 201 motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
Fairy_Paolina 2:3d0be48abcf2 202 }
Fairy_Paolina 2:3d0be48abcf2 203 }else {
Fairy_Paolina 3:58726d2e11f0 204 motors.setMotor0Speed(dir*MAX_SPEED);//right
Fairy_Paolina 3:58726d2e11f0 205 motors.setMotor1Speed(dir*MAX_SPEED);//left
Fairy_Paolina 0:ff94cc47fef7 206 }
Fairy_Paolina 2:3d0be48abcf2 207 }
Fairy_Paolina 3:58726d2e11f0 208 return wavegap;
Fairy_Paolina 0:ff94cc47fef7 209 }
Fairy_Paolina 0:ff94cc47fef7 210
Fairy_Paolina 0:ff94cc47fef7 211 /* MODIFIED WALL_FOLLOW FOR NAVIGATION */
Fairy_Paolina 0:ff94cc47fef7 212
Fairy_Paolina 4:f2333e66ec2c 213 void wall_follow2(int side, int direction, int section)
Fairy_Paolina 0:ff94cc47fef7 214 {
Fairy_Paolina 4:f2333e66ec2c 215 int SeeWaveGap = false, count=0, dir=1, set=5;
Fairy_Paolina 4:f2333e66ec2c 216
Fairy_Paolina 4:f2333e66ec2c 217 pid1.reset();
Fairy_Paolina 3:58726d2e11f0 218
Fairy_Paolina 3:58726d2e11f0 219 if(direction == BACKWARD) dir=-1;
Fairy_Paolina 4:f2333e66ec2c 220 if(section == TOOLS)set= 9;
Fairy_Paolina 3:58726d2e11f0 221
Fairy_Paolina 3:58726d2e11f0 222 leftEncoder.reset();
Fairy_Paolina 3:58726d2e11f0 223 rightEncoder.reset();
Fairy_Paolina 2:3d0be48abcf2 224
Fairy_Paolina 2:3d0be48abcf2 225 while(1) {
Fairy_Paolina 2:3d0be48abcf2 226
Fairy_Paolina 2:3d0be48abcf2 227 pid1.setInputLimits(0.0, 5.0);
Fairy_Paolina 1:801f0b9a862a 228 pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
Fairy_Paolina 2:3d0be48abcf2 229 pid1.setSetPoint(5.0);
Fairy_Paolina 2:3d0be48abcf2 230
Fairy_Paolina 2:3d0be48abcf2 231 if(side){
Fairy_Paolina 2:3d0be48abcf2 232 rangeFinderLeft.startMeas();
Fairy_Paolina 2:3d0be48abcf2 233 wait_ms(20);
Fairy_Paolina 2:3d0be48abcf2 234 rangeFinderLeft.getMeas(range);
Fairy_Paolina 2:3d0be48abcf2 235 }
Fairy_Paolina 2:3d0be48abcf2 236 else{
Fairy_Paolina 2:3d0be48abcf2 237 rangeFinderRight.startMeas();
Fairy_Paolina 2:3d0be48abcf2 238 wait_ms(20);
Fairy_Paolina 2:3d0be48abcf2 239 rangeFinderRight.getMeas(range);
Fairy_Paolina 2:3d0be48abcf2 240 }
Fairy_Paolina 2:3d0be48abcf2 241
Fairy_Paolina 2:3d0be48abcf2 242
Fairy_Paolina 0:ff94cc47fef7 243 /*************CHECK FOR WAVE OPENING*****************/
Fairy_Paolina 3:58726d2e11f0 244 /* If after 20 ms the ultrasonic still sees 20+ cm */
Fairy_Paolina 0:ff94cc47fef7 245 /* then robot is at wave opening */
Fairy_Paolina 2:3d0be48abcf2 246
Fairy_Paolina 1:801f0b9a862a 247 pc.printf("range %f\r\n",range);
Fairy_Paolina 2:3d0be48abcf2 248 if(range > 20) {
Fairy_Paolina 2:3d0be48abcf2 249 motors.stopBothMotors();
Fairy_Paolina 2:3d0be48abcf2 250 bt.printf("wavegap\r\n");
Fairy_Paolina 2:3d0be48abcf2 251 // AT WAVE OPENING!!!!
Fairy_Paolina 2:3d0be48abcf2 252 break;
Fairy_Paolina 0:ff94cc47fef7 253 }
Fairy_Paolina 2:3d0be48abcf2 254
Fairy_Paolina 0:ff94cc47fef7 255 pid1.setProcessValue(range);
Fairy_Paolina 2:3d0be48abcf2 256 pid_return = pid1.compute();
Fairy_Paolina 2:3d0be48abcf2 257 //bt.printf("Range: %f\n PID: %f\r\n", range, pid_return);
Fairy_Paolina 2:3d0be48abcf2 258
Fairy_Paolina 2:3d0be48abcf2 259 if(pid_return > 0) {
Fairy_Paolina 2:3d0be48abcf2 260 if(side){
Fairy_Paolina 3:58726d2e11f0 261 motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
Fairy_Paolina 3:58726d2e11f0 262 motors.setMotor1Speed(dir*MAX_SPEED);//left
Fairy_Paolina 2:3d0be48abcf2 263 }
Fairy_Paolina 2:3d0be48abcf2 264 else{
Fairy_Paolina 3:58726d2e11f0 265 motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
Fairy_Paolina 3:58726d2e11f0 266 motors.setMotor0Speed(dir*MAX_SPEED);//right
Fairy_Paolina 2:3d0be48abcf2 267 }
Fairy_Paolina 2:3d0be48abcf2 268 }else if(pid_return < 0) {
Fairy_Paolina 2:3d0be48abcf2 269 if(side){
Fairy_Paolina 3:58726d2e11f0 270 motors.setMotor0Speed(dir*MAX_SPEED);//right
Fairy_Paolina 3:58726d2e11f0 271 motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
Fairy_Paolina 2:3d0be48abcf2 272 }
Fairy_Paolina 2:3d0be48abcf2 273 else{
Fairy_Paolina 3:58726d2e11f0 274 motors.setMotor1Speed(dir*MAX_SPEED);//left
Fairy_Paolina 3:58726d2e11f0 275 motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
Fairy_Paolina 2:3d0be48abcf2 276 }
Fairy_Paolina 2:3d0be48abcf2 277 } else {
Fairy_Paolina 3:58726d2e11f0 278 motors.setMotor0Speed(dir*MAX_SPEED);
Fairy_Paolina 3:58726d2e11f0 279 motors.setMotor1Speed(dir*MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 280 }
Fairy_Paolina 2:3d0be48abcf2 281 }
Fairy_Paolina 0:ff94cc47fef7 282 }
Fairy_Paolina 0:ff94cc47fef7 283
Fairy_Paolina 0:ff94cc47fef7 284
Fairy_Paolina 0:ff94cc47fef7 285 /* MODIFIED WALL_FOLLOW FOR NAVIGATION WITH WAVE OPENINGS PASSED IN */
Fairy_Paolina 0:ff94cc47fef7 286 /* MEANT FOR RETURNING FROM OIL RIGS */
Fairy_Paolina 0:ff94cc47fef7 287
Fairy_Paolina 0:ff94cc47fef7 288 void wall_follow3(int &currentLocation, int &WaveOpening)
Fairy_Paolina 0:ff94cc47fef7 289 {
Fairy_Paolina 2:3d0be48abcf2 290 while(1) {
Fairy_Paolina 2:3d0be48abcf2 291
Fairy_Paolina 2:3d0be48abcf2 292
Fairy_Paolina 2:3d0be48abcf2 293 pid1.setInputLimits(0, 5);
Fairy_Paolina 1:801f0b9a862a 294 pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
Fairy_Paolina 2:3d0be48abcf2 295 pid1.setSetPoint(5.0);
Fairy_Paolina 2:3d0be48abcf2 296
Fairy_Paolina 2:3d0be48abcf2 297 rangeFinderLeft.startMeas();
Fairy_Paolina 0:ff94cc47fef7 298 wait_ms(100);
Fairy_Paolina 2:3d0be48abcf2 299 if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID) && (range < 100.0) && (range > 3.0)) {
Fairy_Paolina 0:ff94cc47fef7 300 //bt.printf("Range = %f\n", range);
Fairy_Paolina 2:3d0be48abcf2 301 }
Fairy_Paolina 2:3d0be48abcf2 302
Fairy_Paolina 0:ff94cc47fef7 303 /*************CHECK FOR WAVE OPENING*****************/
Fairy_Paolina 0:ff94cc47fef7 304 /* If after 100 ms the ultrasonic still sees 20+ cm */
Fairy_Paolina 0:ff94cc47fef7 305 /* then robot is at wave opening */
Fairy_Paolina 2:3d0be48abcf2 306
Fairy_Paolina 2:3d0be48abcf2 307
Fairy_Paolina 2:3d0be48abcf2 308 if(range > 20 ) {
Fairy_Paolina 0:ff94cc47fef7 309 currentLocation--;
Fairy_Paolina 0:ff94cc47fef7 310 }
Fairy_Paolina 2:3d0be48abcf2 311
Fairy_Paolina 2:3d0be48abcf2 312 if( currentLocation == WaveOpening) {
Fairy_Paolina 0:ff94cc47fef7 313 // AT WAVE OPENING!!!!
Fairy_Paolina 2:3d0be48abcf2 314
Fairy_Paolina 0:ff94cc47fef7 315 break;
Fairy_Paolina 0:ff94cc47fef7 316 }
Fairy_Paolina 2:3d0be48abcf2 317
Fairy_Paolina 2:3d0be48abcf2 318
Fairy_Paolina 0:ff94cc47fef7 319 pid1.setProcessValue(range);
Fairy_Paolina 2:3d0be48abcf2 320 pid_return = pid1.compute();
Fairy_Paolina 0:ff94cc47fef7 321 bt.printf("Range: %f\n PID: %f", range, pid_return);
Fairy_Paolina 2:3d0be48abcf2 322
Fairy_Paolina 2:3d0be48abcf2 323 if(pid_return > 0) {
Fairy_Paolina 0:ff94cc47fef7 324 motors.setMotor0Speed(MAX_SPEED - pid_return);
Fairy_Paolina 0:ff94cc47fef7 325 motors.setMotor1Speed(MAX_SPEED);
Fairy_Paolina 2:3d0be48abcf2 326 } else if(pid_return < 0) {
Fairy_Paolina 0:ff94cc47fef7 327 motors.setMotor0Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 328 motors.setMotor1Speed(MAX_SPEED + pid_return);
Fairy_Paolina 2:3d0be48abcf2 329 } else {
Fairy_Paolina 0:ff94cc47fef7 330 motors.setMotor0Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 331 motors.setMotor1Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 332 }
Fairy_Paolina 2:3d0be48abcf2 333 }
Fairy_Paolina 0:ff94cc47fef7 334 }
Fairy_Paolina 1:801f0b9a862a 335
Fairy_Paolina 1:801f0b9a862a 336 void rightTurn(void)
Fairy_Paolina 1:801f0b9a862a 337 {
Fairy_Paolina 2:3d0be48abcf2 338 float speedL, speedR;
Fairy_Paolina 2:3d0be48abcf2 339
Fairy_Paolina 2:3d0be48abcf2 340 speedL=speedR= 0.4;
Fairy_Paolina 2:3d0be48abcf2 341
Fairy_Paolina 1:801f0b9a862a 342 leftEncoder.reset();
Fairy_Paolina 2:3d0be48abcf2 343 rightEncoder.reset();
Fairy_Paolina 2:3d0be48abcf2 344 motors.setMotor0Speed(-speedR*127);//right
Fairy_Paolina 2:3d0be48abcf2 345 motors.setMotor1Speed(speedL*127);//left
Fairy_Paolina 2:3d0be48abcf2 346 while(leftEncoder.getPulses()<1050 || rightEncoder.getPulses()>-1050);
Fairy_Paolina 1:801f0b9a862a 347 motors.stopBothMotors();
Fairy_Paolina 1:801f0b9a862a 348 }
Fairy_Paolina 1:801f0b9a862a 349
Fairy_Paolina 1:801f0b9a862a 350 void leftTurn(void)
Fairy_Paolina 1:801f0b9a862a 351 {
Fairy_Paolina 1:801f0b9a862a 352 leftEncoder.reset();
Fairy_Paolina 2:3d0be48abcf2 353 rightEncoder.reset();
Fairy_Paolina 2:3d0be48abcf2 354 motors.setMotor0Speed(0.4*127);// right
Fairy_Paolina 2:3d0be48abcf2 355 motors.setMotor1Speed(-0.4*127);// left
Fairy_Paolina 2:3d0be48abcf2 356 while(leftEncoder.getPulses()>-1200 || rightEncoder.getPulses()<1200);
Fairy_Paolina 1:801f0b9a862a 357 motors.stopBothMotors();
Fairy_Paolina 1:801f0b9a862a 358 }
Fairy_Paolina 3:58726d2e11f0 359
Fairy_Paolina 3:58726d2e11f0 360 void overBump(void){
Fairy_Paolina 3:58726d2e11f0 361
Fairy_Paolina 3:58726d2e11f0 362 leftEncoder.reset();
Fairy_Paolina 3:58726d2e11f0 363 rightEncoder.reset();
Fairy_Paolina 4:f2333e66ec2c 364 motors.setMotor0Speed(127*.15); //right
Fairy_Paolina 4:f2333e66ec2c 365 motors.setMotor1Speed(127*.15); //left
Fairy_Paolina 4:f2333e66ec2c 366 while(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR)/2 <9);
Fairy_Paolina 4:f2333e66ec2c 367
Fairy_Paolina 4:f2333e66ec2c 368 leftEncoder.reset();
Fairy_Paolina 4:f2333e66ec2c 369 rightEncoder.reset();
Fairy_Paolina 3:58726d2e11f0 370 motors.setMotor0Speed(MAX_SPEED); //right
Fairy_Paolina 3:58726d2e11f0 371 motors.setMotor1Speed(MAX_SPEED); //left
Fairy_Paolina 4:f2333e66ec2c 372 while(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR)/2 <3);
Fairy_Paolina 4:f2333e66ec2c 373
Fairy_Paolina 3:58726d2e11f0 374 motors.stopBothMotors();
Fairy_Paolina 3:58726d2e11f0 375 }
Fairy_Paolina 3:58726d2e11f0 376
Fairy_Paolina 3:58726d2e11f0 377 void tools_section(float* location, float &current){
Fairy_Paolina 3:58726d2e11f0 378
Fairy_Paolina 3:58726d2e11f0 379 location[0]=wall_follow(LEFT,FORWARD, TOOLS); //location from the left edge of the field
Fairy_Paolina 4:f2333e66ec2c 380 current=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
Fairy_Paolina 3:58726d2e11f0 381 bt.printf("wavegap %f \t current %f \r\n",location[0],current);
Fairy_Paolina 3:58726d2e11f0 382
Fairy_Paolina 3:58726d2e11f0 383 motors.stopBothMotors();
Fairy_Paolina 3:58726d2e11f0 384 ////////////////////////////////////////// determine shape and pick up tool ///////////////////////////////////////////////////////
Fairy_Paolina 4:f2333e66ec2c 385
Fairy_Paolina 3:58726d2e11f0 386 if(current >location[0]){
Fairy_Paolina 4:f2333e66ec2c 387 wall_follow2(LEFT,BACKWARD,TOOLS);
Fairy_Paolina 4:f2333e66ec2c 388 wait_ms(500);
Fairy_Paolina 3:58726d2e11f0 389 // forward
Fairy_Paolina 4:f2333e66ec2c 390 /* leftEncoder.reset();
Fairy_Paolina 3:58726d2e11f0 391 rightEncoder.reset();
Fairy_Paolina 3:58726d2e11f0 392 motors.setMotor0Speed(-MAX_SPEED); //right
Fairy_Paolina 3:58726d2e11f0 393 motors.setMotor1Speed(-MAX_SPEED); //left
Fairy_Paolina 3:58726d2e11f0 394 while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300);
Fairy_Paolina 3:58726d2e11f0 395 motors.stopBothMotors();
Fairy_Paolina 4:f2333e66ec2c 396 wait_ms(500);*/
Fairy_Paolina 4:f2333e66ec2c 397 current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
Fairy_Paolina 4:f2333e66ec2c 398 }
Fairy_Paolina 4:f2333e66ec2c 399 else{/*
Fairy_Paolina 4:f2333e66ec2c 400 wall_follow2(LEFT,FORWARD);
Fairy_Paolina 4:f2333e66ec2c 401 // backward
Fairy_Paolina 4:f2333e66ec2c 402 leftEncoder.reset();
Fairy_Paolina 4:f2333e66ec2c 403 rightEncoder.reset();
Fairy_Paolina 4:f2333e66ec2c 404 motors.setMotor0Speed(MAX_SPEED); //right
Fairy_Paolina 4:f2333e66ec2c 405 motors.setMotor1Speed(MAX_SPEED); //left
Fairy_Paolina 4:f2333e66ec2c 406 while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300);
Fairy_Paolina 4:f2333e66ec2c 407 motors.stopBothMotors();
Fairy_Paolina 4:f2333e66ec2c 408 current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;*/
Fairy_Paolina 3:58726d2e11f0 409 }
Fairy_Paolina 3:58726d2e11f0 410
Fairy_Paolina 3:58726d2e11f0 411 leftTurn();
Fairy_Paolina 3:58726d2e11f0 412
Fairy_Paolina 3:58726d2e11f0 413 //Go over
Fairy_Paolina 3:58726d2e11f0 414 overBump();
Fairy_Paolina 4:f2333e66ec2c 415
Fairy_Paolina 3:58726d2e11f0 416 }