Paolina Povolotskaya / NavigationTest

revise

Dependencies:   HCSR04 PID PololuQik2 QEI mbed-rtos Sharp

main.cpp@3:58726d2e11f0, 2014-03-18 (annotated)

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