Alpha Tango / Mbed 2 deprecated R5_Robotics

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@13:2c8f8e77b549, 2018-04-01 (annotated)

Committer:
alpha_tango
Date:
Sun Apr 01 04:19:32 2018 +0000
Revision:
13:2c8f8e77b549
Parent:
12:92ba52394c85 
Final Update

Who changed what in which revision?

UserRevisionLine numberNew contents of line
alpha_tango 0:7e54f0e2be67 1 #include "mbed.h"
alpha_tango 0:7e54f0e2be67 2
alpha_tango 4:9b1c6b9dae1c 3
alpha_tango 4:9b1c6b9dae1c 4 // PIN DECLARATIONS
alpha_tango 0:7e54f0e2be67 5 DigitalOut FLdirection(PTB18);
alpha_tango 0:7e54f0e2be67 6 DigitalOut FRdirection(PTA4);
alpha_tango 0:7e54f0e2be67 7 DigitalOut magDirection(PTB19);
alpha_tango 0:7e54f0e2be67 8 PwmOut stepFL(PTD3);
alpha_tango 0:7e54f0e2be67 9 PwmOut stepFR(PTA5);
alpha_tango 0:7e54f0e2be67 10 PwmOut magArm(PTA12);
alpha_tango 0:7e54f0e2be67 11 InterruptIn killAll(PTC3);
alpha_tango 1:4ab886b72870 12 DigitalIn Start(PTC12);
alpha_tango 0:7e54f0e2be67 13 DigitalOut enableH(PTC11);
alpha_tango 0:7e54f0e2be67 14 DigitalOut highH(PTC10);
alpha_tango 4:9b1c6b9dae1c 15 DigitalOut enableL(PTC11);
alpha_tango 4:9b1c6b9dae1c 16 DigitalOut highL(PTC7);
alpha_tango 4:9b1c6b9dae1c 17 I2C i2c(PTC9, PTC8); //pins for I2C communication (SDA, SCL)
alpha_tango 4:9b1c6b9dae1c 18 Serial pc(USBTX, USBRX);
alpha_tango 4:9b1c6b9dae1c 19 DigitalOut LED(PTC4);
alpha_tango 4:9b1c6b9dae1c 20 DigitalOut green(LED_GREEN);
alpha_tango 0:7e54f0e2be67 21
alpha_tango 4:9b1c6b9dae1c 22
alpha_tango 4:9b1c6b9dae1c 23 // PROTOTYPE FUNCTION DECLARATIONS
alpha_tango 4:9b1c6b9dae1c 24 void move(float dist, bool direction);
alpha_tango 4:9b1c6b9dae1c 25 void grabToken();//Picks up the token for reading
alpha_tango 4:9b1c6b9dae1c 26 void dropToken();//Drops the token off
alpha_tango 4:9b1c6b9dae1c 27 void kill();
alpha_tango 13:2c8f8e77b549 28 void turnRight(float, bool);
alpha_tango 13:2c8f8e77b549 29 void turnLeft(float, bool);
alpha_tango 4:9b1c6b9dae1c 30 void rot180(); //Turns the robot around
alpha_tango 13:2c8f8e77b549 31 int findColor(); //Figures out what color the disk is and makes a decision on where to take the disk
alpha_tango 13:2c8f8e77b549 32 void findPath(); //Figures out the path to take to take the disk to its drop off position
alpha_tango 13:2c8f8e77b549 33 void returnHome(); //Returns to the home white square
alpha_tango 13:2c8f8e77b549 34 void returnPrevPos(); //Does the opposite of findPath() to return to the previous position
alpha_tango 4:9b1c6b9dae1c 35
alpha_tango 4:9b1c6b9dae1c 36 // GLOBAL VARIABLES
alpha_tango 4:9b1c6b9dae1c 37 const int FORWARD = 0;
alpha_tango 4:9b1c6b9dae1c 38 const int BACKWARD = 1;
alpha_tango 4:9b1c6b9dae1c 39 const float stepSize = 0.001212; //in meters
alpha_tango 4:9b1c6b9dae1c 40 const float FREQUENCY = 500; //steps per second
alpha_tango 4:9b1c6b9dae1c 41 int sensor_addr = 41 << 1;
alpha_tango 11:93c035708249 42
alpha_tango 11:93c035708249 43
alpha_tango 4:9b1c6b9dae1c 44 // NOTES
alpha_tango 0:7e54f0e2be67 45 /*
alpha_tango 13:2c8f8e77b549 46 -42 prewriten functions for the drop off decision
alpha_tango 0:7e54f0e2be67 47 -Possibly use another RGB sensor as a line follower
alpha_tango 0:7e54f0e2be67 48 -Decides function based on color
alpha_tango 0:7e54f0e2be67 49 -findPath
alpha_tango 0:7e54f0e2be67 50 -starting at bottomLeft
alpha_tango 0:7e54f0e2be67 51 -travel up one leg
alpha_tango 0:7e54f0e2be67 52 -turn right if rgb
alpha_tango 0:7e54f0e2be67 53 -turn left if cmy
alpha_tango 0:7e54f0e2be67 54 -turn right, left, or stay based on color choice
alpha_tango 4:9b1c6b9dae1c 55
alpha_tango 0:7e54f0e2be67 56 void findPath(); //Figures out the path to take to take the disk to its drop off position
alpha_tango 0:7e54f0e2be67 57 void returnHome(); //Returns to the home white square
alpha_tango 0:7e54f0e2be67 58 void returnPrevPos(); //Does the opposite of findPath() to return to the previous position
alpha_tango 0:7e54f0e2be67 59
alpha_tango 0:7e54f0e2be67 60 //Variables
alpha_tango 4:9b1c6b9dae1c 61
alpha_tango 0:7e54f0e2be67 62 -boxSizes, 2x2, 3x3, 4x4, etc
alpha_tango 0:7e54f0e2be67 63 -legSize, 1 foot, 1.5 feet, 2 feet, etc.
alpha_tango 0:7e54f0e2be67 64 -direction choices for findPath
alpha_tango 0:7e54f0e2be67 65 */
alpha_tango 0:7e54f0e2be67 66
alpha_tango 0:7e54f0e2be67 67
alpha_tango 0:7e54f0e2be67 68 int main()
alpha_tango 0:7e54f0e2be67 69 {
alpha_tango 0:7e54f0e2be67 70 //Start a timer
alpha_tango 13:2c8f8e77b549 71 //Will need to be a variable timer based on round number
alpha_tango 0:7e54f0e2be67 72
alpha_tango 5:17a8d8395a50 73 float radDistance = 0.5;
alpha_tango 5:17a8d8395a50 74 float posDistance = 0.5;
alpha_tango 5:17a8d8395a50 75 float armDistance = 0.5;
alpha_tango 5:17a8d8395a50 76
alpha_tango 0:7e54f0e2be67 77
alpha_tango 0:7e54f0e2be67 78 enableH = 0; //Making sure the H-Bridge starts low and off
alpha_tango 4:9b1c6b9dae1c 79 highH = 0; //This starts high for the H-Bridge
alpha_tango 4:9b1c6b9dae1c 80 highL = 1; //This starts low for the H-Bridge
alpha_tango 4:9b1c6b9dae1c 81 while(true) //The start button
alpha_tango 1:4ab886b72870 82 {
alpha_tango 1:4ab886b72870 83 if (Start == 0)
alpha_tango 1:4ab886b72870 84 break;
alpha_tango 1:4ab886b72870 85 }
alpha_tango 4:9b1c6b9dae1c 86
alpha_tango 4:9b1c6b9dae1c 87 killAll.rise(&kill); //The kill interupt
alpha_tango 5:17a8d8395a50 88
alpha_tango 4:9b1c6b9dae1c 89
alpha_tango 5:17a8d8395a50 90 // RGB Sensor Settings
alpha_tango 13:2c8f8e77b549 91 pc.baud(115200);
alpha_tango 13:2c8f8e77b549 92 green = 1; // off
alpha_tango 13:2c8f8e77b549 93 i2c.frequency(200000);
alpha_tango 13:2c8f8e77b549 94 char id_regval[1] = {146};
alpha_tango 13:2c8f8e77b549 95 char data[1] = {0};
alpha_tango 13:2c8f8e77b549 96 i2c.write(sensor_addr,id_regval,1, true);
alpha_tango 13:2c8f8e77b549 97 i2c.read(sensor_addr,data,1,false);
alpha_tango 13:2c8f8e77b549 98 if (data[0]==68) {
alpha_tango 4:9b1c6b9dae1c 99 green = 0;
alpha_tango 4:9b1c6b9dae1c 100 wait (2);
alpha_tango 4:9b1c6b9dae1c 101 green = 1;
alpha_tango 4:9b1c6b9dae1c 102 } else {
alpha_tango 4:9b1c6b9dae1c 103 green = 1;
alpha_tango 13:2c8f8e77b549 104 }
alpha_tango 4:9b1c6b9dae1c 105
alpha_tango 4:9b1c6b9dae1c 106 // Initialize color sensor
alpha_tango 4:9b1c6b9dae1c 107 char timing_register[2] = {129,0};
alpha_tango 4:9b1c6b9dae1c 108 i2c.write(sensor_addr,timing_register,2,false);
alpha_tango 4:9b1c6b9dae1c 109
alpha_tango 4:9b1c6b9dae1c 110 char control_register[2] = {143,0};
alpha_tango 4:9b1c6b9dae1c 111 i2c.write(sensor_addr,control_register,2,false);
alpha_tango 4:9b1c6b9dae1c 112
alpha_tango 4:9b1c6b9dae1c 113 char enable_register[2] = {128,3};
alpha_tango 4:9b1c6b9dae1c 114 i2c.write(sensor_addr,enable_register,2,false);
alpha_tango 4:9b1c6b9dae1c 115
alpha_tango 5:17a8d8395a50 116 // Initialize the robot position
alpha_tango 5:17a8d8395a50 117 move((0.6096-radDistance+posDistance+armDistance),FORWARD);
alpha_tango 5:17a8d8395a50 118 turnLeft();
alpha_tango 5:17a8d8395a50 119 move(radDistance,BACKWARD);
alpha_tango 0:7e54f0e2be67 120
alpha_tango 0:7e54f0e2be67 121 while(true)
alpha_tango 4:9b1c6b9dae1c 122 {
alpha_tango 13:2c8f8e77b549 123 grabToken();
alpha_tango 13:2c8f8e77b549 124 move(1,FORWARD);
alpha_tango 13:2c8f8e77b549 125 turnLeft();
alpha_tango 13:2c8f8e77b549 126 wait(0.5);
alpha_tango 13:2c8f8e77b549 127 turnLeft();
alpha_tango 13:2c8f8e77b549 128 wait(2);
alpha_tango 13:2c8f8e77b549 129 dropToken();
alpha_tango 13:2c8f8e77b549 130 wait(2);
alpha_tango 0:7e54f0e2be67 131 }
alpha_tango 0:7e54f0e2be67 132 }
alpha_tango 4:9b1c6b9dae1c 133
alpha_tango 0:7e54f0e2be67 134 //Distance is in meters
alpha_tango 4:9b1c6b9dae1c 135 void move(float dist, bool direction)
alpha_tango 0:7e54f0e2be67 136 {
alpha_tango 0:7e54f0e2be67 137 FLdirection = direction;
alpha_tango 0:7e54f0e2be67 138 FRdirection = !direction;
alpha_tango 0:7e54f0e2be67 139
alpha_tango 4:9b1c6b9dae1c 140 stepFL.period(1.0/FREQUENCY);
alpha_tango 0:7e54f0e2be67 141 stepFR.period(1/FREQUENCY);
alpha_tango 4:9b1c6b9dae1c 142 stepFL.write(0.5f);
alpha_tango 4:9b1c6b9dae1c 143 stepFR.write(0.5f);
alpha_tango 13:2c8f8e77b549 144 //dist/stepSize is the number of steps
alpha_tango 13:2c8f8e77b549 145 //1/FREQUENCY is the time per step
alpha_tango 13:2c8f8e77b549 146 wait(4*(dist/stepSize)*(1/FREQUENCY));
alpha_tango 4:9b1c6b9dae1c 147 stepFL.period(0.0f);
alpha_tango 4:9b1c6b9dae1c 148 stepFR.period(0.0f);
alpha_tango 4:9b1c6b9dae1c 149 stepFL.write(0.0f);
alpha_tango 4:9b1c6b9dae1c 150 stepFR.write(0.0f);
alpha_tango 0:7e54f0e2be67 151
alpha_tango 0:7e54f0e2be67 152 }
alpha_tango 4:9b1c6b9dae1c 153
alpha_tango 0:7e54f0e2be67 154 void grabToken()
alpha_tango 0:7e54f0e2be67 155 {
alpha_tango 4:9b1c6b9dae1c 156 highL = 0;
alpha_tango 4:9b1c6b9dae1c 157 highH = 1;
alpha_tango 0:7e54f0e2be67 158 enableH = 1;
alpha_tango 0:7e54f0e2be67 159 wait(1);
alpha_tango 0:7e54f0e2be67 160 magDirection = 1;
alpha_tango 0:7e54f0e2be67 161 magArm.period(0.002);
alpha_tango 0:7e54f0e2be67 162 magArm.write(0.5);
alpha_tango 0:7e54f0e2be67 163 wait(0.65);
alpha_tango 0:7e54f0e2be67 164 magArm.period(0);
alpha_tango 0:7e54f0e2be67 165 magArm.write(0);
alpha_tango 0:7e54f0e2be67 166
alpha_tango 0:7e54f0e2be67 167 }
alpha_tango 4:9b1c6b9dae1c 168
alpha_tango 0:7e54f0e2be67 169 void dropToken()
alpha_tango 0:7e54f0e2be67 170 {
alpha_tango 0:7e54f0e2be67 171 magDirection = 0;
alpha_tango 0:7e54f0e2be67 172 magArm.period(0.002);
alpha_tango 0:7e54f0e2be67 173 magArm.write(0.5);
alpha_tango 0:7e54f0e2be67 174 wait(0.65);
alpha_tango 0:7e54f0e2be67 175 magArm.period(0);
alpha_tango 4:9b1c6b9dae1c 176 magArm.write(0);
alpha_tango 4:9b1c6b9dae1c 177 highL = 1;
alpha_tango 4:9b1c6b9dae1c 178 highH = 0;
alpha_tango 4:9b1c6b9dae1c 179 wait(2);
alpha_tango 4:9b1c6b9dae1c 180 enableH = 0;
alpha_tango 0:7e54f0e2be67 181 }
alpha_tango 4:9b1c6b9dae1c 182
alpha_tango 11:93c035708249 183 void turnRight(float dist, bool direction)
alpha_tango 2:5f29bc7daa49 184 {
alpha_tango 2:5f29bc7daa49 185 //Get rid of all FR occurences which will turn right motor off
alpha_tango 5:17a8d8395a50 186 FLdirection = 0; //to turn right we want this going FORWARD so a 0;
alpha_tango 2:5f29bc7daa49 187
alpha_tango 2:5f29bc7daa49 188
alpha_tango 2:5f29bc7daa49 189 stepFL.period(1/FREQUENCY);
alpha_tango 2:5f29bc7daa49 190 stepFL.write(0.5);
alpha_tango 2:5f29bc7daa49 191
alpha_tango 2:5f29bc7daa49 192 //dist/stepSize is the number of steps
alpha_tango 2:5f29bc7daa49 193 //1/FREQUENCY is the time per step
alpha_tango 5:17a8d8395a50 194 wait(4*(0.35343/stepSize)*(1/FREQUENCY));
alpha_tango 2:5f29bc7daa49 195 stepFL.period(0);
alpha_tango 2:5f29bc7daa49 196 stepFL.write(0);
alpha_tango 0:7e54f0e2be67 197 }
alpha_tango 11:93c035708249 198 void turnLeft(float dist, bool direction)
alpha_tango 3:d3264a6f7a62 199 {
alpha_tango 3:d3264a6f7a62 200 //Get rid of all FL occurences which will turn left motor off
alpha_tango 5:17a8d8395a50 201 FRdirection = 1; //to turn right we want this going FORWARD, since FORWARD = 0, it must be !0
alpha_tango 3:d3264a6f7a62 202
alpha_tango 3:d3264a6f7a62 203
alpha_tango 3:d3264a6f7a62 204 stepFR.period(1/FREQUENCY); // We could slow motor down by subtracting from denominator.
alpha_tango 3:d3264a6f7a62 205 stepFR.write(0.5);
alpha_tango 3:d3264a6f7a62 206
alpha_tango 3:d3264a6f7a62 207 //dist/stepSize is the number of steps
alpha_tango 3:d3264a6f7a62 208 //1/FREQUENCY is the time per step
alpha_tango 5:17a8d8395a50 209 wait(4*(0.35343/stepSize)*(1/FREQUENCY));
alpha_tango 3:d3264a6f7a62 210 stepFR.period(0);
alpha_tango 3:d3264a6f7a62 211 stepFR.write(0);
alpha_tango 3:d3264a6f7a62 212 }
alpha_tango 0:7e54f0e2be67 213 void rot180()
alpha_tango 0:7e54f0e2be67 214 {
alpha_tango 4:9b1c6b9dae1c 215 //Get rid of all FR occurences which will turn right motor off
alpha_tango 13:2c8f8e77b549 216 FLdirection = direction; //to turn right we want this going FORWARD so a 0;
alpha_tango 4:9b1c6b9dae1c 217
alpha_tango 4:9b1c6b9dae1c 218
alpha_tango 4:9b1c6b9dae1c 219 stepFL.period(1/FREQUENCY);
alpha_tango 4:9b1c6b9dae1c 220 stepFL.write(0.5);
alpha_tango 4:9b1c6b9dae1c 221
alpha_tango 4:9b1c6b9dae1c 222 //dist/stepSize is the number of steps
alpha_tango 4:9b1c6b9dae1c 223 //1/FREQUENCY is the time per step
alpha_tango 4:9b1c6b9dae1c 224 wait(2*4*(0.35343/stepSize)*(1/FREQUENCY));
alpha_tango 4:9b1c6b9dae1c 225 stepFL.period(0);
alpha_tango 4:9b1c6b9dae1c 226 stepFL.write(0);
alpha_tango 0:7e54f0e2be67 227 }
alpha_tango 0:7e54f0e2be67 228 void kill()
alpha_tango 0:7e54f0e2be67 229 {
alpha_tango 0:7e54f0e2be67 230 exit(0);
alpha_tango 0:7e54f0e2be67 231 }
alpha_tango 4:9b1c6b9dae1c 232
alpha_tango 4:9b1c6b9dae1c 233 int findColor(){ //Figures out what color the disk is and makes a decision on where to take the disk
alpha_tango 4:9b1c6b9dae1c 234
alpha_tango 4:9b1c6b9dae1c 235 while (true) {
alpha_tango 4:9b1c6b9dae1c 236 wait(1);
alpha_tango 4:9b1c6b9dae1c 237 char clear_reg[1] = {148};
alpha_tango 4:9b1c6b9dae1c 238 char clear_data[2] = {0,0};
alpha_tango 4:9b1c6b9dae1c 239 i2c.write(sensor_addr,clear_reg,1, true);
alpha_tango 4:9b1c6b9dae1c 240 i2c.read(sensor_addr,clear_data,2, false);
alpha_tango 4:9b1c6b9dae1c 241
alpha_tango 4:9b1c6b9dae1c 242 int clear_value = ((int)clear_data[1] << 8) | clear_data[0];
alpha_tango 4:9b1c6b9dae1c 243
alpha_tango 4:9b1c6b9dae1c 244 char red_reg[1] = {150};
alpha_tango 4:9b1c6b9dae1c 245 char red_data[2] = {0,0};
alpha_tango 4:9b1c6b9dae1c 246 i2c.write(sensor_addr,red_reg,1, true);
alpha_tango 4:9b1c6b9dae1c 247 i2c.read(sensor_addr,red_data,2, false);
alpha_tango 4:9b1c6b9dae1c 248
alpha_tango 4:9b1c6b9dae1c 249 int red_value = ((int)red_data[1] << 8) | red_data[0];
alpha_tango 4:9b1c6b9dae1c 250
alpha_tango 4:9b1c6b9dae1c 251 char green_reg[1] = {152};
alpha_tango 4:9b1c6b9dae1c 252 char green_data[2] = {0,0};
alpha_tango 4:9b1c6b9dae1c 253 i2c.write(sensor_addr,green_reg,1, true);
alpha_tango 4:9b1c6b9dae1c 254 i2c.read(sensor_addr,green_data,2, false);
alpha_tango 4:9b1c6b9dae1c 255
alpha_tango 4:9b1c6b9dae1c 256 int green_value = ((int)green_data[1] << 8) | green_data[0];
alpha_tango 4:9b1c6b9dae1c 257
alpha_tango 4:9b1c6b9dae1c 258 char blue_reg[1] = {154};
alpha_tango 4:9b1c6b9dae1c 259 char blue_data[2] = {0,0};
alpha_tango 4:9b1c6b9dae1c 260 i2c.write(sensor_addr,blue_reg,1, true);
alpha_tango 4:9b1c6b9dae1c 261 i2c.read(sensor_addr,blue_data,2, false);
alpha_tango 4:9b1c6b9dae1c 262
alpha_tango 4:9b1c6b9dae1c 263 int blue_value = ((int)blue_data[1] << 8) | blue_data[0];
alpha_tango 4:9b1c6b9dae1c 264
alpha_tango 4:9b1c6b9dae1c 265
alpha_tango 4:9b1c6b9dae1c 266
alpha_tango 13:2c8f8e77b549 267 //1=red,2=green,3=blue,4=cyan,5=magenta,6=yellow,7=gray,8=error
alpha_tango 4:9b1c6b9dae1c 268 if(blue_value<10000 && red_value>10000){
alpha_tango 4:9b1c6b9dae1c 269 return(1);
alpha_tango 4:9b1c6b9dae1c 270 }
alpha_tango 4:9b1c6b9dae1c 271 else if(green_value>18000 && blue_value<30000){
alpha_tango 4:9b1c6b9dae1c 272 return(2);
alpha_tango 4:9b1c6b9dae1c 273 }
alpha_tango 4:9b1c6b9dae1c 274 else if(red_value<10000 && blue_value>15000){
alpha_tango 4:9b1c6b9dae1c 275 return(3);
alpha_tango 4:9b1c6b9dae1c 276 }
alpha_tango 4:9b1c6b9dae1c 277 else if(blue_value>30000 && red_value<20000){
alpha_tango 4:9b1c6b9dae1c 278 return(4);
alpha_tango 4:9b1c6b9dae1c 279 }
alpha_tango 4:9b1c6b9dae1c 280 else if(red_value>25000 && green_value<15000){
alpha_tango 4:9b1c6b9dae1c 281 return(5);
alpha_tango 4:9b1c6b9dae1c 282 }
alpha_tango 4:9b1c6b9dae1c 283 else if(red_value>50000){
alpha_tango 4:9b1c6b9dae1c 284 return(6);
alpha_tango 4:9b1c6b9dae1c 285 }
alpha_tango 4:9b1c6b9dae1c 286 else if(red_value<10000 && blue_value<10000){
alpha_tango 4:9b1c6b9dae1c 287 return(7);
alpha_tango 4:9b1c6b9dae1c 288 }
alpha_tango 4:9b1c6b9dae1c 289 else if(red_value==0){
alpha_tango 4:9b1c6b9dae1c 290 return(8);
alpha_tango 4:9b1c6b9dae1c 291 }
alpha_tango 13:2c8f8e77b549 292
alpha_tango 4:9b1c6b9dae1c 293 // print sensor readings
alpha_tango 4:9b1c6b9dae1c 294
alpha_tango 4:9b1c6b9dae1c 295 //pc.printf("Clear (%d), Red (%d), Green (%d), Blue (%d)\n", clear_value, red_value, green_value, blue_value);
alpha_tango 4:9b1c6b9dae1c 296 //wait(0.5);
alpha_tango 4:9b1c6b9dae1c 297 }
alpha_tango 13:2c8f8e77b549 298 }
alpha_tango 10:ed6ecbb420b8 299
alpha_tango 13:2c8f8e77b549 300 void findPath(){ //Figures out the path to take to take the disk to its drop off position
alpha_tango 13:2c8f8e77b549 301
alpha_tango 13:2c8f8e77b549 302 }
alpha_tango 10:ed6ecbb420b8 303
alpha_tango 13:2c8f8e77b549 304 void returnHome(){ //Returns to the home white square
alpha_tango 13:2c8f8e77b549 305
alpha_tango 11:93c035708249 306 }
alpha_tango 13:2c8f8e77b549 307
alpha_tango 13:2c8f8e77b549 308 void returnPrevPos(){ //Does the opposite of findPath() to return to the previous position
alpha_tango 13:2c8f8e77b549 309
alpha_tango 13:2c8f8e77b549 310 }