Justin Carr / NavigationTest_

Yeah

Dependencies:   HCSR04 PID PololuQik2 QEI Sharp mbed-rtos

Fork of NavigationTest by Paolina Povolotskaya

main.cpp@10:2aa70a504c18, 2014-03-27 (annotated)

Committer:
jjcarr2
Date:
Thu Mar 27 18:32:27 2014 +0000
Revision:
10:2aa70a504c18
Parent:
9:f34700716f1d
Child:
11:12ce7600f2f9
asd;

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 8:11ef93eebe07 9 #include "Sharp.h"
Fairy_Paolina 9:f34700716f1d 10
Fairy_Paolina 2:3d0be48abcf2 11 #define PIN_TRIGGERL (p12)
Fairy_Paolina 2:3d0be48abcf2 12 #define PIN_ECHOL (p11)
Fairy_Paolina 2:3d0be48abcf2 13 #define PIN_TRIGGERR (p29)
Fairy_Paolina 2:3d0be48abcf2 14 #define PIN_ECHOR (p30)
Fairy_Paolina 0:ff94cc47fef7 15 #define PULSE_PER_REV (1192)
Fairy_Paolina 0:ff94cc47fef7 16 #define WHEEL_CIRCUM (12.56637)
Fairy_Paolina 0:ff94cc47fef7 17 #define DIST_PER_PULSE (0.01054225722682)
Fairy_Paolina 0:ff94cc47fef7 18 #define MTRS_TO_INCH (39.3701)
Fairy_Paolina 1:801f0b9a862a 19 #define MAX_SPEED (0.3*127)
Fairy_Paolina 4:f2333e66ec2c 20 #define PPR (4331/4)
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 9:f34700716f1d 24 #define BACKWARD (0)
Fairy_Paolina 3:58726d2e11f0 25 #define TOOLS (0)
Fairy_Paolina 3:58726d2e11f0 26 #define MID (1)
Fairy_Paolina 9:f34700716f1d 27 #define RIGS (2)
Fairy_Paolina 6:f5c26372b2d0 28 #define FIRST_WAVE (0)
Fairy_Paolina 6:f5c26372b2d0 29 #define FAR (1)
Fairy_Paolina 9:f34700716f1d 30
Fairy_Paolina 9:f34700716f1d 31
Fairy_Paolina 9:f34700716f1d 32 float range, range2, pid_return;
Fairy_Paolina 0:ff94cc47fef7 33 void errFunction(void);
Fairy_Paolina 0:ff94cc47fef7 34 bool cRc;
Fairy_Paolina 9:f34700716f1d 35
Fairy_Paolina 0:ff94cc47fef7 36 //Hardware Initialization
Fairy_Paolina 0:ff94cc47fef7 37 Serial bt(p13,p14);
Fairy_Paolina 0:ff94cc47fef7 38 Serial pc(USBTX,USBRX);
Fairy_Paolina 2:3d0be48abcf2 39 HCSR04 rangeFinderLeft( PIN_TRIGGERL, PIN_ECHOL );
Fairy_Paolina 2:3d0be48abcf2 40 HCSR04 rangeFinderRight( PIN_TRIGGERR, PIN_ECHOR );
Fairy_Paolina 2:3d0be48abcf2 41 PID pid1(15.0,0.0,4.0,0.02);
Fairy_Paolina 0:ff94cc47fef7 42 PololuQik2 motors(p9, p10, p8, p15, errFunction, cRc);
Fairy_Paolina 4:f2333e66ec2c 43 QEI rightEncoder(p17,p18,NC,PPR,QEI::X4_ENCODING);
Fairy_Paolina 4:f2333e66ec2c 44 QEI leftEncoder(p16,p15,NC,PPR,QEI::X4_ENCODING);
Fairy_Paolina 8:11ef93eebe07 45 Sharp IR(p20);
Fairy_Paolina 1:801f0b9a862a 46 //InterruptIn encoder(p29);
Fairy_Paolina 9:f34700716f1d 47
Fairy_Paolina 9:f34700716f1d 48
Fairy_Paolina 0:ff94cc47fef7 49 //Functions
Fairy_Paolina 9:f34700716f1d 50
Fairy_Paolina 3:58726d2e11f0 51 float wall_follow(int side, int direction, int section);
Fairy_Paolina 6:f5c26372b2d0 52 void wall_follow2(int side, int direction, int section, float location);
Fairy_Paolina 0:ff94cc47fef7 53 void wall_follow3(int &currentLocation, int &WaveOpening);
Fairy_Paolina 1:801f0b9a862a 54 void leftTurn(void);
Fairy_Paolina 9:f34700716f1d 55 void slightleft(void);
Fairy_Paolina 1:801f0b9a862a 56 void rightTurn(void);
Fairy_Paolina 0:ff94cc47fef7 57 void us_distance(void);
Fairy_Paolina 3:58726d2e11f0 58 void tools_section(float* location, float &current);
Fairy_Paolina 6:f5c26372b2d0 59 void mid_section(float* location, float &current, int* direction);
Fairy_Paolina 6:f5c26372b2d0 60 void mid_section2(float* location, float &current, int* direction);
Fairy_Paolina 6:f5c26372b2d0 61 void rig_section(float* location, float &current, int* direction, int rig);
Fairy_Paolina 8:11ef93eebe07 62 void overBump(int section);
Fairy_Paolina 6:f5c26372b2d0 63 void alignWithWall(int section);
Fairy_Paolina 9:f34700716f1d 64
Fairy_Paolina 1:801f0b9a862a 65 //Variables
Fairy_Paolina 9:f34700716f1d 66
Fairy_Paolina 2:3d0be48abcf2 67 int main(void)
Fairy_Paolina 2:3d0be48abcf2 68 {
Fairy_Paolina 3:58726d2e11f0 69 float location[3], current=0;
Fairy_Paolina 3:58726d2e11f0 70 int direction[3];
Fairy_Paolina 8:11ef93eebe07 71 double distance;
Fairy_Paolina 9:f34700716f1d 72
Fairy_Paolina 0:ff94cc47fef7 73 pc.baud(115200);
Fairy_Paolina 1:801f0b9a862a 74 bt.baud(115200);
Fairy_Paolina 2:3d0be48abcf2 75 motors.begin();
Fairy_Paolina 9:f34700716f1d 76
Fairy_Paolina 7:78745a518957 77 bt.printf("START\r\n");
Fairy_Paolina 3:58726d2e11f0 78 //Go to tools
Fairy_Paolina 4:f2333e66ec2c 79 tools_section(location, current);
Fairy_Paolina 6:f5c26372b2d0 80 mid_section(location, current, direction);
Fairy_Paolina 9:f34700716f1d 81 //mid_section2(location, current, direction);
Fairy_Paolina 9:f34700716f1d 82 /* while(1) {
Fairy_Paolina 9:f34700716f1d 83 //bt.printf("IR %f\r\n", US.getDistance());
Fairy_Paolina 9:f34700716f1d 84 rangeFinderLeft.startMeas();
Fairy_Paolina 9:f34700716f1d 85 wait_ms(20);
Fairy_Paolina 9:f34700716f1d 86 rangeFinderLeft.getMeas(range);
Fairy_Paolina 9:f34700716f1d 87 bt.printf("Range = %f\n\r", range);
Fairy_Paolina 9:f34700716f1d 88
Fairy_Paolina 8:11ef93eebe07 89 wait_ms(200);
Fairy_Paolina 9:f34700716f1d 90 }*/
Fairy_Paolina 9:f34700716f1d 91
Fairy_Paolina 9:f34700716f1d 92
Fairy_Paolina 9:f34700716f1d 93 //leftTurn();
Fairy_Paolina 9:f34700716f1d 94 //rightTurn();
Fairy_Paolina 9:f34700716f1d 95
Fairy_Paolina 9:f34700716f1d 96
Fairy_Paolina 0:ff94cc47fef7 97 }
Fairy_Paolina 9:f34700716f1d 98
Fairy_Paolina 2:3d0be48abcf2 99 void errFunction(void)
Fairy_Paolina 2:3d0be48abcf2 100 {
Fairy_Paolina 2:3d0be48abcf2 101 //Nothing
Fairy_Paolina 0:ff94cc47fef7 102 }
Fairy_Paolina 9:f34700716f1d 103
Fairy_Paolina 0:ff94cc47fef7 104 void us_distance(void)
Fairy_Paolina 0:ff94cc47fef7 105 {
Fairy_Paolina 2:3d0be48abcf2 106 pc.printf("Ultra Sonic\n\r");
Fairy_Paolina 2:3d0be48abcf2 107 rangeFinderLeft.startMeas();
Fairy_Paolina 2:3d0be48abcf2 108 wait_us(20);
Fairy_Paolina 2:3d0be48abcf2 109 if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID)) {
Fairy_Paolina 2:3d0be48abcf2 110 pc.printf("Range = %f\n\r", range);
Fairy_Paolina 2:3d0be48abcf2 111 }
Fairy_Paolina 0:ff94cc47fef7 112 }
Fairy_Paolina 9:f34700716f1d 113
Fairy_Paolina 3:58726d2e11f0 114 float wall_follow(int side, int direction, int section)
Fairy_Paolina 0:ff94cc47fef7 115 {
Fairy_Paolina 8:11ef93eebe07 116 float location, wavegap=0, set=5;
Fairy_Paolina 5:70ccef3734ae 117 int dir=1;
Fairy_Paolina 9:f34700716f1d 118
Fairy_Paolina 4:f2333e66ec2c 119 pid1.reset();
Fairy_Paolina 9:f34700716f1d 120
Fairy_Paolina 3:58726d2e11f0 121 if(direction == BACKWARD) dir=-1;
Fairy_Paolina 5:70ccef3734ae 122 if(section == TOOLS)set= 10;
Fairy_Paolina 9:f34700716f1d 123
Fairy_Paolina 3:58726d2e11f0 124 leftEncoder.reset();
Fairy_Paolina 3:58726d2e11f0 125 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 126
Fairy_Paolina 4:f2333e66ec2c 127 location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
Fairy_Paolina 9:f34700716f1d 128
Fairy_Paolina 9:f34700716f1d 129 while(location< 73) {
Fairy_Paolina 4:f2333e66ec2c 130 location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
Fairy_Paolina 9:f34700716f1d 131
Fairy_Paolina 3:58726d2e11f0 132 pid1.setInputLimits(0, set);
Fairy_Paolina 1:801f0b9a862a 133 pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
Fairy_Paolina 3:58726d2e11f0 134 pid1.setSetPoint(set);
Fairy_Paolina 9:f34700716f1d 135 if(side) {
Fairy_Paolina 2:3d0be48abcf2 136 rangeFinderLeft.startMeas();
Fairy_Paolina 8:11ef93eebe07 137 wait_ms(38);
Fairy_Paolina 2:3d0be48abcf2 138 rangeFinderLeft.getMeas(range);
Fairy_Paolina 9:f34700716f1d 139 } else {
Fairy_Paolina 2:3d0be48abcf2 140 rangeFinderRight.startMeas();
Fairy_Paolina 8:11ef93eebe07 141 wait_ms(38);
Fairy_Paolina 2:3d0be48abcf2 142 rangeFinderRight.getMeas(range);
Fairy_Paolina 2:3d0be48abcf2 143 pc.printf("%d\r\n",range);
Fairy_Paolina 2:3d0be48abcf2 144 }
Fairy_Paolina 9:f34700716f1d 145
Fairy_Paolina 3:58726d2e11f0 146 if(range > 20) {
Fairy_Paolina 4:f2333e66ec2c 147 wavegap=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
Fairy_Paolina 7:78745a518957 148 //bt.printf("wavegap %f\r\n",wavegap);
Fairy_Paolina 3:58726d2e11f0 149 // AT WAVE OPENING!!!!
Fairy_Paolina 9:f34700716f1d 150 motors.setMotor1Speed(dir*0.4*127);//left
Fairy_Paolina 9:f34700716f1d 151 motors.setMotor0Speed(dir*0.4*127);//right
Fairy_Paolina 9:f34700716f1d 152 } else {
Fairy_Paolina 9:f34700716f1d 153
Fairy_Paolina 6:f5c26372b2d0 154 pid1.setProcessValue(range);
Fairy_Paolina 6:f5c26372b2d0 155 pid_return = pid1.compute();
Fairy_Paolina 9:f34700716f1d 156
Fairy_Paolina 6:f5c26372b2d0 157 if(pid_return > 0) {
Fairy_Paolina 9:f34700716f1d 158 if(side) {
Fairy_Paolina 6:f5c26372b2d0 159 motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
Fairy_Paolina 6:f5c26372b2d0 160 motors.setMotor1Speed(dir*MAX_SPEED);//left
Fairy_Paolina 9:f34700716f1d 161 } else {
Fairy_Paolina 6:f5c26372b2d0 162 motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
Fairy_Paolina 6:f5c26372b2d0 163 motors.setMotor0Speed(dir*MAX_SPEED);//right
Fairy_Paolina 6:f5c26372b2d0 164 }
Fairy_Paolina 9:f34700716f1d 165 } else if(pid_return < 0) {
Fairy_Paolina 9:f34700716f1d 166 if(side) {
Fairy_Paolina 6:f5c26372b2d0 167 motors.setMotor0Speed(dir*MAX_SPEED);//right
Fairy_Paolina 6:f5c26372b2d0 168 motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
Fairy_Paolina 9:f34700716f1d 169 } else {
Fairy_Paolina 6:f5c26372b2d0 170 motors.setMotor1Speed(dir*MAX_SPEED);//left
Fairy_Paolina 6:f5c26372b2d0 171 motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
Fairy_Paolina 6:f5c26372b2d0 172 }
Fairy_Paolina 9:f34700716f1d 173 } else {
Fairy_Paolina 6:f5c26372b2d0 174 motors.setMotor0Speed(dir*MAX_SPEED);//right
Fairy_Paolina 3:58726d2e11f0 175 motors.setMotor1Speed(dir*MAX_SPEED);//left
Fairy_Paolina 2:3d0be48abcf2 176 }
Fairy_Paolina 0:ff94cc47fef7 177 }
Fairy_Paolina 2:3d0be48abcf2 178 }
Fairy_Paolina 3:58726d2e11f0 179 return wavegap;
Fairy_Paolina 0:ff94cc47fef7 180 }
Fairy_Paolina 9:f34700716f1d 181
Fairy_Paolina 0:ff94cc47fef7 182 /* MODIFIED WALL_FOLLOW FOR NAVIGATION */
Fairy_Paolina 9:f34700716f1d 183
Fairy_Paolina 6:f5c26372b2d0 184 void wall_follow2(int side, int direction, int section, float location)
Fairy_Paolina 0:ff94cc47fef7 185 {
Fairy_Paolina 5:70ccef3734ae 186 int SeeWaveGap = false, dir=1;
Fairy_Paolina 8:11ef93eebe07 187 float set=5, loc=0;
Fairy_Paolina 9:f34700716f1d 188
Fairy_Paolina 4:f2333e66ec2c 189 pid1.reset();
Fairy_Paolina 9:f34700716f1d 190
Fairy_Paolina 3:58726d2e11f0 191 if(direction == BACKWARD) dir=-1;
Fairy_Paolina 6:f5c26372b2d0 192 if(section == TOOLS)set= 5;
Fairy_Paolina 9:f34700716f1d 193
Fairy_Paolina 3:58726d2e11f0 194 leftEncoder.reset();
Fairy_Paolina 3:58726d2e11f0 195 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 196
jjcarr2 10:2aa70a504c18 197 while(dir*loc + location <= 78) {
Fairy_Paolina 7:78745a518957 198 loc=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
Fairy_Paolina 9:f34700716f1d 199
Fairy_Paolina 5:70ccef3734ae 200 pid1.setInputLimits(0.0, set);
Fairy_Paolina 1:801f0b9a862a 201 pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
Fairy_Paolina 5:70ccef3734ae 202 pid1.setSetPoint(set);
Fairy_Paolina 9:f34700716f1d 203
Fairy_Paolina 9:f34700716f1d 204 if(side) {
Fairy_Paolina 2:3d0be48abcf2 205 rangeFinderLeft.startMeas();
Fairy_Paolina 8:11ef93eebe07 206 wait_ms(38);
Fairy_Paolina 2:3d0be48abcf2 207 rangeFinderLeft.getMeas(range);
Fairy_Paolina 9:f34700716f1d 208 } else {
Fairy_Paolina 2:3d0be48abcf2 209 rangeFinderRight.startMeas();
Fairy_Paolina 8:11ef93eebe07 210 wait_ms(38);
Fairy_Paolina 2:3d0be48abcf2 211 rangeFinderRight.getMeas(range);
Fairy_Paolina 2:3d0be48abcf2 212 }
Fairy_Paolina 9:f34700716f1d 213
Fairy_Paolina 9:f34700716f1d 214
Fairy_Paolina 0:ff94cc47fef7 215 /*************CHECK FOR WAVE OPENING*****************/
Fairy_Paolina 3:58726d2e11f0 216 /* If after 20 ms the ultrasonic still sees 20+ cm */
Fairy_Paolina 0:ff94cc47fef7 217 /* then robot is at wave opening */
Fairy_Paolina 9:f34700716f1d 218
jjcarr2 10:2aa70a504c18 219 //bt.printf("wall follow 2 range %f\r\n",range);
jjcarr2 10:2aa70a504c18 220 //bt.printf("loc+location = %f\r\n", loc+location);
Fairy_Paolina 2:3d0be48abcf2 221 if(range > 20) {
Fairy_Paolina 2:3d0be48abcf2 222 motors.stopBothMotors();
Fairy_Paolina 2:3d0be48abcf2 223 bt.printf("wavegap\r\n");
Fairy_Paolina 2:3d0be48abcf2 224 // AT WAVE OPENING!!!!
Fairy_Paolina 2:3d0be48abcf2 225 break;
Fairy_Paolina 0:ff94cc47fef7 226 }
Fairy_Paolina 9:f34700716f1d 227
Fairy_Paolina 0:ff94cc47fef7 228 pid1.setProcessValue(range);
Fairy_Paolina 2:3d0be48abcf2 229 pid_return = pid1.compute();
Fairy_Paolina 2:3d0be48abcf2 230 //bt.printf("Range: %f\n PID: %f\r\n", range, pid_return);
Fairy_Paolina 9:f34700716f1d 231
Fairy_Paolina 9:f34700716f1d 232 if(pid_return > 0) {
Fairy_Paolina 9:f34700716f1d 233 if(side) {
Fairy_Paolina 3:58726d2e11f0 234 motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
Fairy_Paolina 3:58726d2e11f0 235 motors.setMotor1Speed(dir*MAX_SPEED);//left
Fairy_Paolina 9:f34700716f1d 236 } else {
Fairy_Paolina 3:58726d2e11f0 237 motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
Fairy_Paolina 3:58726d2e11f0 238 motors.setMotor0Speed(dir*MAX_SPEED);//right
Fairy_Paolina 2:3d0be48abcf2 239 }
Fairy_Paolina 9:f34700716f1d 240 } else if(pid_return < 0) {
Fairy_Paolina 9:f34700716f1d 241 if(side) {
Fairy_Paolina 3:58726d2e11f0 242 motors.setMotor0Speed(dir*MAX_SPEED);//right
Fairy_Paolina 3:58726d2e11f0 243 motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
Fairy_Paolina 9:f34700716f1d 244 } else {
Fairy_Paolina 3:58726d2e11f0 245 motors.setMotor1Speed(dir*MAX_SPEED);//left
Fairy_Paolina 3:58726d2e11f0 246 motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
Fairy_Paolina 2:3d0be48abcf2 247 }
Fairy_Paolina 2:3d0be48abcf2 248 } else {
Fairy_Paolina 3:58726d2e11f0 249 motors.setMotor0Speed(dir*MAX_SPEED);
Fairy_Paolina 3:58726d2e11f0 250 motors.setMotor1Speed(dir*MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 251 }
Fairy_Paolina 2:3d0be48abcf2 252 }
Fairy_Paolina 6:f5c26372b2d0 253 motors.stopBothMotors();
Fairy_Paolina 0:ff94cc47fef7 254 }
Fairy_Paolina 9:f34700716f1d 255
Fairy_Paolina 9:f34700716f1d 256
Fairy_Paolina 9:f34700716f1d 257 void alignWithWall(int section)
Fairy_Paolina 9:f34700716f1d 258 {
Fairy_Paolina 6:f5c26372b2d0 259 float usValue = 0;
Fairy_Paolina 9:f34700716f1d 260
Fairy_Paolina 9:f34700716f1d 261 if(section == TOOLS) {
Fairy_Paolina 6:f5c26372b2d0 262 // turn at an angle
Fairy_Paolina 6:f5c26372b2d0 263 leftEncoder.reset();
Fairy_Paolina 9:f34700716f1d 264 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 265 motors.setMotor0Speed(-1.2*MAX_SPEED); //right
Fairy_Paolina 6:f5c26372b2d0 266 motors.setMotor1Speed(0.4*MAX_SPEED); //left
Fairy_Paolina 6:f5c26372b2d0 267 while(rightEncoder.getPulses()>-1000);
Fairy_Paolina 6:f5c26372b2d0 268 motors.stopBothMotors();
Fairy_Paolina 9:f34700716f1d 269
Fairy_Paolina 6:f5c26372b2d0 270 //go backwards toward wall
Fairy_Paolina 6:f5c26372b2d0 271 leftEncoder.reset();
Fairy_Paolina 9:f34700716f1d 272 rightEncoder.reset();
Fairy_Paolina 6:f5c26372b2d0 273 motors.setMotor0Speed(-MAX_SPEED); //right
Fairy_Paolina 6:f5c26372b2d0 274 motors.setMotor1Speed(-MAX_SPEED); //left
Fairy_Paolina 7:78745a518957 275 while(abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300);
Fairy_Paolina 9:f34700716f1d 276
Fairy_Paolina 6:f5c26372b2d0 277 motors.stopBothMotors();
Fairy_Paolina 9:f34700716f1d 278
Fairy_Paolina 9:f34700716f1d 279 // turn left towards wall
Fairy_Paolina 6:f5c26372b2d0 280 leftEncoder.reset();
Fairy_Paolina 6:f5c26372b2d0 281 rightEncoder.reset();
Fairy_Paolina 6:f5c26372b2d0 282 motors.setMotor0Speed(MAX_SPEED); //right
Fairy_Paolina 6:f5c26372b2d0 283 motors.setMotor1Speed(-MAX_SPEED); //left
Fairy_Paolina 9:f34700716f1d 284 while(rightEncoder.getPulses() < 10 || abs(leftEncoder.getPulses()) < 10);
Fairy_Paolina 9:f34700716f1d 285
Fairy_Paolina 9:f34700716f1d 286 motors.stopBothMotors();
Fairy_Paolina 9:f34700716f1d 287
Fairy_Paolina 7:78745a518957 288 motors.setMotor0Speed(0.7*MAX_SPEED); //right
Fairy_Paolina 7:78745a518957 289 motors.setMotor1Speed(-0.7*MAX_SPEED); //left
Fairy_Paolina 7:78745a518957 290 } else {
Fairy_Paolina 8:11ef93eebe07 291 rightTurn();
Fairy_Paolina 7:78745a518957 292 motors.setMotor0Speed(-0.7*MAX_SPEED); //right
Fairy_Paolina 7:78745a518957 293 motors.setMotor1Speed(0.7*MAX_SPEED); //left
Fairy_Paolina 6:f5c26372b2d0 294 }
Fairy_Paolina 9:f34700716f1d 295
Fairy_Paolina 6:f5c26372b2d0 296 usValue = 0;
Fairy_Paolina 9:f34700716f1d 297 while(1) {
Fairy_Paolina 2:3d0be48abcf2 298 rangeFinderLeft.startMeas();
Fairy_Paolina 6:f5c26372b2d0 299 wait_ms(20);
Fairy_Paolina 6:f5c26372b2d0 300 rangeFinderLeft.getMeas(range);
Fairy_Paolina 7:78745a518957 301 //bt.printf("Range %f \t OldValue %f\n\r",range, usValue);
Fairy_Paolina 9:f34700716f1d 302 if(range > usValue && usValue != 0 && range < 25) {
Fairy_Paolina 0:ff94cc47fef7 303 break;
Fairy_Paolina 2:3d0be48abcf2 304 } else {
Fairy_Paolina 9:f34700716f1d 305 usValue = range;
Fairy_Paolina 0:ff94cc47fef7 306 }
Fairy_Paolina 2:3d0be48abcf2 307 }
Fairy_Paolina 6:f5c26372b2d0 308 motors.stopBothMotors();
Fairy_Paolina 5:70ccef3734ae 309 }
Fairy_Paolina 9:f34700716f1d 310
Fairy_Paolina 1:801f0b9a862a 311 void rightTurn(void)
Fairy_Paolina 1:801f0b9a862a 312 {
Fairy_Paolina 9:f34700716f1d 313 motors.begin();
Fairy_Paolina 1:801f0b9a862a 314 leftEncoder.reset();
Fairy_Paolina 2:3d0be48abcf2 315 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 316 motors.setMotor0Speed(-0.5*127);//right
Fairy_Paolina 9:f34700716f1d 317 motors.setMotor1Speed(0.5*127);//left
Fairy_Paolina 9:f34700716f1d 318 while(leftEncoder.getPulses()<900 || rightEncoder.getPulses()>-900);
Fairy_Paolina 1:801f0b9a862a 319 motors.stopBothMotors();
Fairy_Paolina 1:801f0b9a862a 320 }
Fairy_Paolina 9:f34700716f1d 321
Fairy_Paolina 1:801f0b9a862a 322 void leftTurn(void)
Fairy_Paolina 9:f34700716f1d 323 {
Fairy_Paolina 9:f34700716f1d 324 /*
Fairy_Paolina 1:801f0b9a862a 325 leftEncoder.reset();
Fairy_Paolina 2:3d0be48abcf2 326 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 327 motors.setMotor0Speed(0.4*MAX_SPEED); //right
Fairy_Paolina 9:f34700716f1d 328 motors.setMotor1Speed(-MAX_SPEED); //left
Fairy_Paolina 9:f34700716f1d 329 while(abs(leftEncoder.getPulses())<2500);
Fairy_Paolina 9:f34700716f1d 330 motors.stopBothMotors();
Fairy_Paolina 9:f34700716f1d 331 */
Fairy_Paolina 9:f34700716f1d 332 motors.begin();
Fairy_Paolina 9:f34700716f1d 333 leftEncoder.reset();
Fairy_Paolina 9:f34700716f1d 334 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 335 motors.setMotor0Speed(0.5*127);// right
Fairy_Paolina 9:f34700716f1d 336 motors.setMotor1Speed(-0.5*127);// left
Fairy_Paolina 8:11ef93eebe07 337 while(abs(leftEncoder.getPulses())<1000 || rightEncoder.getPulses()<1000);
Fairy_Paolina 5:70ccef3734ae 338 motors.stopBothMotors();
Fairy_Paolina 5:70ccef3734ae 339 }
Fairy_Paolina 9:f34700716f1d 340 void slightleft(void){
Fairy_Paolina 9:f34700716f1d 341
Fairy_Paolina 9:f34700716f1d 342 leftEncoder.reset();
Fairy_Paolina 9:f34700716f1d 343 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 344 motors.setMotor0Speed(0.5*127);// right
Fairy_Paolina 9:f34700716f1d 345 motors.setMotor1Speed(-0.5*127);// left
Fairy_Paolina 9:f34700716f1d 346 while(abs(leftEncoder.getPulses())<50 || rightEncoder.getPulses()<50);
Fairy_Paolina 9:f34700716f1d 347 motors.stopBothMotors();
Fairy_Paolina 9:f34700716f1d 348 }
Fairy_Paolina 9:f34700716f1d 349
Fairy_Paolina 9:f34700716f1d 350
Fairy_Paolina 9:f34700716f1d 351 void overBump(int section)
Fairy_Paolina 9:f34700716f1d 352 {
Fairy_Paolina 9:f34700716f1d 353 int preLeft=5000, preRight=5000, out=0;
Fairy_Paolina 9:f34700716f1d 354
Fairy_Paolina 9:f34700716f1d 355 motors.begin();
Fairy_Paolina 9:f34700716f1d 356
Fairy_Paolina 9:f34700716f1d 357 leftEncoder.reset();
Fairy_Paolina 9:f34700716f1d 358 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 359 motors.setMotor0Speed(-0.2*127); //right
Fairy_Paolina 9:f34700716f1d 360 motors.setMotor1Speed(-0.2*127); //left
Fairy_Paolina 9:f34700716f1d 361 while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses())< 50);
Fairy_Paolina 9:f34700716f1d 362 motors.stopBothMotors();
Fairy_Paolina 3:58726d2e11f0 363
Fairy_Paolina 3:58726d2e11f0 364 leftEncoder.reset();
Fairy_Paolina 3:58726d2e11f0 365 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 366 motors.setMotor0Speed(0.2*127); //right
Fairy_Paolina 9:f34700716f1d 367 motors.setMotor1Speed(0.2*127); //left
Fairy_Paolina 8:11ef93eebe07 368 while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.getDistance() >20 && preLeft!=0){
Fairy_Paolina 9:f34700716f1d 369 preLeft=leftEncoder.getPulses();
Fairy_Paolina 9:f34700716f1d 370 preRight=rightEncoder.getPulses();
Fairy_Paolina 9:f34700716f1d 371 wait_ms(100);
jjcarr2 10:2aa70a504c18 372 //bt.printf(" first while left %d right %d \r\n", preLeft, preRight);
Fairy_Paolina 9:f34700716f1d 373 if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
Fairy_Paolina 8:11ef93eebe07 374 }
Fairy_Paolina 8:11ef93eebe07 375
Fairy_Paolina 8:11ef93eebe07 376 motors.stopBothMotors();
Fairy_Paolina 9:f34700716f1d 377 motors.begin();
Fairy_Paolina 9:f34700716f1d 378 wait(2);
Fairy_Paolina 9:f34700716f1d 379 /*
Fairy_Paolina 9:f34700716f1d 380 motors.stopBothMotors();
Fairy_Paolina 9:f34700716f1d 381 motors.setMotor0Speed(0.15*127); //right
Fairy_Paolina 9:f34700716f1d 382 motors.setMotor1Speed(0.15*127); //left
Fairy_Paolina 9:f34700716f1d 383 preLeft=preRight=5000 ;
Fairy_Paolina 9:f34700716f1d 384 leftEncoder.reset();
Fairy_Paolina 9:f34700716f1d 385 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 386 */
Fairy_Paolina 9:f34700716f1d 387 // while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.getDistance() >20 && preLeft!=0){
Fairy_Paolina 9:f34700716f1d 388 /* preLeft=leftEncoder.getPulses();
Fairy_Paolina 9:f34700716f1d 389 preRight=rightEncoder.getPulses();
Fairy_Paolina 9:f34700716f1d 390 bt.printf("second while left %d right %d \r\n", preLeft, preRight);
Fairy_Paolina 9:f34700716f1d 391 wait_ms(200);
Fairy_Paolina 9:f34700716f1d 392 if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
Fairy_Paolina 9:f34700716f1d 393 }*/
Fairy_Paolina 9:f34700716f1d 394
Fairy_Paolina 8:11ef93eebe07 395 leftEncoder.reset();
Fairy_Paolina 8:11ef93eebe07 396 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 397 motors.setMotor0Speed(0.3*127); //right
Fairy_Paolina 9:f34700716f1d 398 motors.setMotor1Speed(0.3*127); //left
Fairy_Paolina 9:f34700716f1d 399
Fairy_Paolina 9:f34700716f1d 400 while(!out) {
Fairy_Paolina 6:f5c26372b2d0 401 preLeft=leftEncoder.getPulses();
Fairy_Paolina 6:f5c26372b2d0 402 preRight=rightEncoder.getPulses();
Fairy_Paolina 9:f34700716f1d 403
Fairy_Paolina 9:f34700716f1d 404 rangeFinderLeft.startMeas();
Fairy_Paolina 9:f34700716f1d 405 rangeFinderRight.startMeas();
Fairy_Paolina 9:f34700716f1d 406 wait_ms(20);
Fairy_Paolina 9:f34700716f1d 407 rangeFinderLeft.getMeas(range);
Fairy_Paolina 9:f34700716f1d 408 rangeFinderRight.getMeas(range2);
Fairy_Paolina 9:f34700716f1d 409 if(range < 10 || range2 < 10) out=1;
Fairy_Paolina 9:f34700716f1d 410
Fairy_Paolina 9:f34700716f1d 411 if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) {
Fairy_Paolina 9:f34700716f1d 412 motors.setMotor0Speed(0.4*127); //right
Fairy_Paolina 9:f34700716f1d 413 motors.setMotor1Speed(0.4*127); //left
Fairy_Paolina 9:f34700716f1d 414 }
Fairy_Paolina 9:f34700716f1d 415 if(abs(leftEncoder.getPulses()) <1000 || abs(leftEncoder.getPulses())<1000) out=1;
Fairy_Paolina 8:11ef93eebe07 416 }
Fairy_Paolina 9:f34700716f1d 417
Fairy_Paolina 8:11ef93eebe07 418 motors.stopBothMotors();
Fairy_Paolina 9:f34700716f1d 419 wait(2);
Fairy_Paolina 9:f34700716f1d 420 motors.begin();
Fairy_Paolina 4:f2333e66ec2c 421
Fairy_Paolina 8:11ef93eebe07 422 preLeft=preRight=5000 ;
Fairy_Paolina 8:11ef93eebe07 423 leftEncoder.reset();
Fairy_Paolina 8:11ef93eebe07 424 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 425 motors.setMotor0Speed(.25*127); //right
Fairy_Paolina 9:f34700716f1d 426 motors.setMotor1Speed(.25*127); //left
Fairy_Paolina 9:f34700716f1d 427
Fairy_Paolina 9:f34700716f1d 428 if(section == TOOLS || section == MID) {
Fairy_Paolina 8:11ef93eebe07 429 while(IR.getDistance() > 20 ) {
jjcarr2 10:2aa70a504c18 430 //bt.printf("IR %f\r\n", IR.getDistance());
jjcarr2 10:2aa70a504c18 431 //bt.printf("third while left %d right %d \r\n", preLeft, preRight);
Fairy_Paolina 9:f34700716f1d 432 }
Fairy_Paolina 9:f34700716f1d 433 } else while((abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses())< 200));
Fairy_Paolina 8:11ef93eebe07 434
Fairy_Paolina 9:f34700716f1d 435 motors.setMotor0Speed(-.25*127); //right
Fairy_Paolina 9:f34700716f1d 436 motors.setMotor1Speed(-.25*127); //left
Fairy_Paolina 9:f34700716f1d 437 wait_ms(10);
Fairy_Paolina 3:58726d2e11f0 438 motors.stopBothMotors();
Fairy_Paolina 8:11ef93eebe07 439 wait(2);
Fairy_Paolina 9:f34700716f1d 440 motors.begin();
Fairy_Paolina 9:f34700716f1d 441
Fairy_Paolina 3:58726d2e11f0 442 }
Fairy_Paolina 9:f34700716f1d 443
Fairy_Paolina 9:f34700716f1d 444 void tools_section(float* location, float &current)
Fairy_Paolina 9:f34700716f1d 445 {
Fairy_Paolina 9:f34700716f1d 446 wall_follow(LEFT,FORWARD, TOOLS);
Fairy_Paolina 6:f5c26372b2d0 447 // current position in reference to the starting position
Fairy_Paolina 4:f2333e66ec2c 448 current=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
Fairy_Paolina 6:f5c26372b2d0 449 bt.printf("current %f \r\n",current);
Fairy_Paolina 9:f34700716f1d 450
Fairy_Paolina 5:70ccef3734ae 451 motors.stopBothMotors();
Fairy_Paolina 9:f34700716f1d 452
Fairy_Paolina 6:f5c26372b2d0 453 //Tool aquiring
Fairy_Paolina 6:f5c26372b2d0 454 wait(2);
jjcarr2 10:2aa70a504c18 455 while(1){ }
Fairy_Paolina 6:f5c26372b2d0 456 // After tool is aquired
Fairy_Paolina 9:f34700716f1d 457
Fairy_Paolina 6:f5c26372b2d0 458 alignWithWall(TOOLS);
Fairy_Paolina 9:f34700716f1d 459
Fairy_Paolina 6:f5c26372b2d0 460 wait_ms(100);
Fairy_Paolina 6:f5c26372b2d0 461
Fairy_Paolina 9:f34700716f1d 462 wall_follow2(LEFT,FORWARD,MID, current);
Fairy_Paolina 9:f34700716f1d 463 current= 78;
Fairy_Paolina 9:f34700716f1d 464
Fairy_Paolina 6:f5c26372b2d0 465 rangeFinderLeft.startMeas();
Fairy_Paolina 6:f5c26372b2d0 466 wait_ms(20);
Fairy_Paolina 6:f5c26372b2d0 467 rangeFinderLeft.getMeas(range);
Fairy_Paolina 9:f34700716f1d 468
Fairy_Paolina 9:f34700716f1d 469 if(range < 20) {
Fairy_Paolina 6:f5c26372b2d0 470 wall_follow2(LEFT,BACKWARD,TOOLS, current);
Fairy_Paolina 9:f34700716f1d 471 leftEncoder.reset();
Fairy_Paolina 9:f34700716f1d 472 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 473 motors.setMotor0Speed(-MAX_SPEED); //right
Fairy_Paolina 9:f34700716f1d 474 motors.setMotor1Speed(-MAX_SPEED); //left
Fairy_Paolina 9:f34700716f1d 475 while(abs(leftEncoder.getPulses()) < 120 || abs(rightEncoder.getPulses())< 120);
Fairy_Paolina 9:f34700716f1d 476
Fairy_Paolina 9:f34700716f1d 477 leftEncoder.reset();
Fairy_Paolina 9:f34700716f1d 478 rightEncoder.reset();
Fairy_Paolina 9:f34700716f1d 479 motors.setMotor0Speed(MAX_SPEED); //right
Fairy_Paolina 9:f34700716f1d 480 motors.setMotor1Speed(MAX_SPEED); //left
Fairy_Paolina 9:f34700716f1d 481 while(abs(leftEncoder.getPulses()) < 40 || abs(rightEncoder.getPulses())< 40);
Fairy_Paolina 9:f34700716f1d 482 motors.stopBothMotors();
Fairy_Paolina 9:f34700716f1d 483
Fairy_Paolina 9:f34700716f1d 484 wait_ms(500);
Fairy_Paolina 6:f5c26372b2d0 485 location[0]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
Fairy_Paolina 6:f5c26372b2d0 486 current= location[0];
Fairy_Paolina 6:f5c26372b2d0 487 leftTurn();
Fairy_Paolina 9:f34700716f1d 488 slightleft();
Fairy_Paolina 8:11ef93eebe07 489 overBump(TOOLS);
Fairy_Paolina 9:f34700716f1d 490 } else {
Fairy_Paolina 9:f34700716f1d 491 location[0]= 77;
Fairy_Paolina 6:f5c26372b2d0 492 leftTurn();
Fairy_Paolina 6:f5c26372b2d0 493 wait_ms(20);
Fairy_Paolina 6:f5c26372b2d0 494 overBump(FIRST_WAVE);
Fairy_Paolina 9:f34700716f1d 495 }
Fairy_Paolina 9:f34700716f1d 496
jjcarr2 10:2aa70a504c18 497 bt.printf("First Wavegap = %f\r\n",location[0]);
Fairy_Paolina 6:f5c26372b2d0 498 }
Fairy_Paolina 9:f34700716f1d 499
Fairy_Paolina 9:f34700716f1d 500 void mid_section(float* location, float &current, int* direction)
Fairy_Paolina 9:f34700716f1d 501 {
Fairy_Paolina 9:f34700716f1d 502
Fairy_Paolina 9:f34700716f1d 503 motors.begin();
Fairy_Paolina 6:f5c26372b2d0 504
Fairy_Paolina 9:f34700716f1d 505 if(IR.getDistance() > 20) return;
Fairy_Paolina 9:f34700716f1d 506
Fairy_Paolina 6:f5c26372b2d0 507 alignWithWall(MID);
Fairy_Paolina 8:11ef93eebe07 508
Fairy_Paolina 6:f5c26372b2d0 509 bt.printf("mid section current = %f\r\n",current);
Fairy_Paolina 6:f5c26372b2d0 510 wall_follow2(LEFT,FORWARD,MID, current);
Fairy_Paolina 6:f5c26372b2d0 511 current=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
Fairy_Paolina 6:f5c26372b2d0 512 bt.printf("after wf2 current = %f\r\n",current);
Fairy_Paolina 9:f34700716f1d 513
Fairy_Paolina 9:f34700716f1d 514 if(current != 0) {
Fairy_Paolina 6:f5c26372b2d0 515 direction[0]= RIGHT;
Fairy_Paolina 6:f5c26372b2d0 516 current+= location[0];
Fairy_Paolina 6:f5c26372b2d0 517 location[1]= current;
Fairy_Paolina 9:f34700716f1d 518 } else {
Fairy_Paolina 9:f34700716f1d 519 current=location[0];
Fairy_Paolina 6:f5c26372b2d0 520 direction[0]= LEFT;
Fairy_Paolina 6:f5c26372b2d0 521 wall_follow2(LEFT,BACKWARD,MID,current);
Fairy_Paolina 6:f5c26372b2d0 522 location[1]= location[0]- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
Fairy_Paolina 6:f5c26372b2d0 523 }
Fairy_Paolina 9:f34700716f1d 524
Fairy_Paolina 6:f5c26372b2d0 525 bt.printf("wavegap2 = %f\r\n",location[1]);
Fairy_Paolina 9:f34700716f1d 526 leftTurn();
Fairy_Paolina 8:11ef93eebe07 527 overBump(TOOLS);
Fairy_Paolina 6:f5c26372b2d0 528 // go forward
Fairy_Paolina 6:f5c26372b2d0 529 leftEncoder.reset();
Fairy_Paolina 6:f5c26372b2d0 530 rightEncoder.reset();
Fairy_Paolina 6:f5c26372b2d0 531 motors.setMotor0Speed(0.2*127); //right
Fairy_Paolina 6:f5c26372b2d0 532 motors.setMotor1Speed(0.2*127); //left
Fairy_Paolina 6:f5c26372b2d0 533 while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300);
Fairy_Paolina 5:70ccef3734ae 534 motors.stopBothMotors();
Fairy_Paolina 9:f34700716f1d 535
Fairy_Paolina 6:f5c26372b2d0 536 }
Fairy_Paolina 9:f34700716f1d 537
Fairy_Paolina 9:f34700716f1d 538 void mid_section2(float* location, float &current, int* direction)
Fairy_Paolina 9:f34700716f1d 539 {
Fairy_Paolina 9:f34700716f1d 540
Fairy_Paolina 9:f34700716f1d 541 motors.begin();
Fairy_Paolina 6:f5c26372b2d0 542
Fairy_Paolina 9:f34700716f1d 543 if(IR.getDistance() > 20) return;
Fairy_Paolina 6:f5c26372b2d0 544
Fairy_Paolina 6:f5c26372b2d0 545 alignWithWall(MID);
Fairy_Paolina 6:f5c26372b2d0 546 wall_follow2(LEFT,FORWARD,MID, current);
Fairy_Paolina 6:f5c26372b2d0 547 current=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
Fairy_Paolina 9:f34700716f1d 548
Fairy_Paolina 9:f34700716f1d 549 if(current != 0) {
Fairy_Paolina 6:f5c26372b2d0 550 direction[1]= RIGHT;
Fairy_Paolina 6:f5c26372b2d0 551 current+= location[1];
Fairy_Paolina 6:f5c26372b2d0 552 location[2]= current;
Fairy_Paolina 9:f34700716f1d 553 } else {
Fairy_Paolina 9:f34700716f1d 554 current=location[1];
Fairy_Paolina 6:f5c26372b2d0 555 direction[1]= LEFT;
Fairy_Paolina 6:f5c26372b2d0 556 wall_follow2(LEFT,BACKWARD,MID,current);
Fairy_Paolina 6:f5c26372b2d0 557 location[2]= location[1]- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
Fairy_Paolina 6:f5c26372b2d0 558 }
Fairy_Paolina 9:f34700716f1d 559
Fairy_Paolina 9:f34700716f1d 560 leftTurn();
Fairy_Paolina 8:11ef93eebe07 561 overBump(RIGS);
Fairy_Paolina 9:f34700716f1d 562 }
Fairy_Paolina 9:f34700716f1d 563
Fairy_Paolina 9:f34700716f1d 564 void rig_section(float* location, float &current, int* direction, int rig)
Fairy_Paolina 9:f34700716f1d 565 {
Fairy_Paolina 9:f34700716f1d 566
Fairy_Paolina 9:f34700716f1d 567
Fairy_Paolina 8:11ef93eebe07 568 }