Paolina Povolotskaya / NavigationTest

revise

Dependencies:   HCSR04 PID PololuQik2 QEI mbed-rtos Sharp

main.cpp@5:70ccef3734ae, 2014-03-20 (annotated)

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