David Spillman / Mbed 2 deprecated GPSNavigation

Crude navigation

Dependencies:   GPS L3GD20 LSM303DLHC mbed PID

main.cpp@2:503a5ac6c3b6, 2015-03-04 (annotated)

Committer:
Spilly
Date:
Wed Mar 04 17:48:08 2015 +0000
Revision:
2:503a5ac6c3b6
Parent:
0:e79311aae7ed
Child:
3:ffa0e1429a72
Implemented PID

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Spilly 0:e79311aae7ed 1 #include "mbed.h"
Spilly 0:e79311aae7ed 2 #include "GPS.h"
Spilly 0:e79311aae7ed 3 #include "modSensData.h"
Spilly 2:503a5ac6c3b6 4 #include "move.h"
Spilly 2:503a5ac6c3b6 5 #include "PID.h"
Spilly 0:e79311aae7ed 6
Spilly 0:e79311aae7ed 7 //Radius of the earth in meters
Spilly 0:e79311aae7ed 8 #define EARTHRADIUS 6378100.0f
Spilly 0:e79311aae7ed 9 //Tolerance for heading actual and heading needed
Spilly 2:503a5ac6c3b6 10 #define HEADDIFF 2.0f
Spilly 2:503a5ac6c3b6 11 //Tolerance for whether or not vehicle has arrived at goal position
Spilly 2:503a5ac6c3b6 12 #define ARRIVED 100.0f
Spilly 0:e79311aae7ed 13 //Period in seconds of the the main loop
Spilly 2:503a5ac6c3b6 14 #define PERIOD 0.5f
Spilly 2:503a5ac6c3b6 15 //Period in seconds of PID loop
Spilly 2:503a5ac6c3b6 16 #define RATE .001
Spilly 2:503a5ac6c3b6 17 #define PIDCYCLES 100
Spilly 0:e79311aae7ed 18 //GPS
Spilly 0:e79311aae7ed 19 GPS gps(D9, D7);
Spilly 0:e79311aae7ed 20
Spilly 0:e79311aae7ed 21 //X-Bee
Spilly 0:e79311aae7ed 22 Serial xBee(PTC15, PTC14);
Spilly 0:e79311aae7ed 23
Spilly 2:503a5ac6c3b6 24 //PID
Spilly 2:503a5ac6c3b6 25 //Kc, Ti, Td, interval
Spilly 2:503a5ac6c3b6 26 PID controller(.01, .01, 20, RATE);
Spilly 0:e79311aae7ed 27
Spilly 2:503a5ac6c3b6 28 //Enter new position here
Spilly 2:503a5ac6c3b6 29 float goalPos[2] = {35.336020, -81.912420};
Spilly 2:503a5ac6c3b6 30
Spilly 2:503a5ac6c3b6 31 float startPos[2], curPos[2], polarVector[2];
Spilly 2:503a5ac6c3b6 32
Spilly 2:503a5ac6c3b6 33 int sCheck, pCheck = 1;
Spilly 0:e79311aae7ed 34
Spilly 0:e79311aae7ed 35 void setGoalPos(float lat, float lon);
Spilly 0:e79311aae7ed 36 void makeVector(void);
Spilly 2:503a5ac6c3b6 37 float whichWay(float magHead, float calcHead);
Spilly 0:e79311aae7ed 38 void testYaw(void);
Spilly 0:e79311aae7ed 39
Spilly 0:e79311aae7ed 40 int main()
Spilly 0:e79311aae7ed 41 {
Spilly 2:503a5ac6c3b6 42 float motorSpeed = 0.5f;
Spilly 2:503a5ac6c3b6 43 //PID control of left and right motors based on input from gyro
Spilly 2:503a5ac6c3b6 44 //Goal is to have the vehicle go straight
Spilly 2:503a5ac6c3b6 45 controller.setInputLimits(-180, 180);
Spilly 2:503a5ac6c3b6 46 controller.setOutputLimits(-.5, .5);
Spilly 2:503a5ac6c3b6 47 controller.setMode(0);
Spilly 2:503a5ac6c3b6 48 //We want the difference to be zero
Spilly 2:503a5ac6c3b6 49 controller.setSetPoint(0);
Spilly 0:e79311aae7ed 50
Spilly 2:503a5ac6c3b6 51 xBee.baud(9600);
Spilly 0:e79311aae7ed 52
Spilly 0:e79311aae7ed 53 xBee.printf("\nI'm Alive...\n");
Spilly 0:e79311aae7ed 54
Spilly 0:e79311aae7ed 55 //Setup the GPS
Spilly 0:e79311aae7ed 56 gps.Init();
Spilly 2:503a5ac6c3b6 57 xBee.printf("gps initialized\n");
Spilly 0:e79311aae7ed 58
Spilly 2:503a5ac6c3b6 59 xBee.printf("attempting to get a fix\n");
Spilly 0:e79311aae7ed 60
Spilly 0:e79311aae7ed 61 //wait until we have a gps fix
Spilly 0:e79311aae7ed 62 while(gps.fixtype == 0)
Spilly 0:e79311aae7ed 63 {
Spilly 0:e79311aae7ed 64 xBee.printf("fix %d\n", gps.fixtype);
Spilly 0:e79311aae7ed 65 gps.parseData();
Spilly 0:e79311aae7ed 66 wait(.2);
Spilly 0:e79311aae7ed 67 }
Spilly 0:e79311aae7ed 68
Spilly 0:e79311aae7ed 69 //set starting position
Spilly 0:e79311aae7ed 70 curPos[0] = gps.latitude;
Spilly 0:e79311aae7ed 71 curPos[1] = gps.longitude;
Spilly 0:e79311aae7ed 72
Spilly 0:e79311aae7ed 73 //Convert starting position and goal position to a vector
Spilly 0:e79311aae7ed 74 makeVector();
Spilly 0:e79311aae7ed 75
Spilly 0:e79311aae7ed 76 //printf("lat %f\tlon %f\thead %f\talt %f\tspd %f\tfix %d\tsat %d\n", gps.latitude, gps.longitude, gps.heading, gps.altitude, gps.speed, gps.fixtype, gps.satellites);
Spilly 0:e79311aae7ed 77 //printf("magn %f\tangle %f\tlat %f\tlon %f\tgoalLat %f\tgoalLon %f\n", polarVector[0], polarVector[1], gps.latitude, gps.longitude, goalPos[0], goalPos[1]);
Spilly 0:e79311aae7ed 78
Spilly 2:503a5ac6c3b6 79
Spilly 2:503a5ac6c3b6 80 xBee.printf("starting main loop\n");
Spilly 0:e79311aae7ed 81 while (1)
Spilly 0:e79311aae7ed 82 {
Spilly 2:503a5ac6c3b6 83 //Emergency stop
Spilly 2:503a5ac6c3b6 84 if(xBee.readable())
Spilly 2:503a5ac6c3b6 85 {
Spilly 2:503a5ac6c3b6 86 char tempChar = xBee.getc();
Spilly 2:503a5ac6c3b6 87 if(tempChar == 'n')
Spilly 2:503a5ac6c3b6 88 {
Spilly 2:503a5ac6c3b6 89 xBee.printf("emergency stop\n");
Spilly 2:503a5ac6c3b6 90 goStop(1,1);
Spilly 2:503a5ac6c3b6 91 while(1);
Spilly 2:503a5ac6c3b6 92 }
Spilly 2:503a5ac6c3b6 93 }
Spilly 2:503a5ac6c3b6 94 //check GPS data
Spilly 2:503a5ac6c3b6 95 //xBee.printf("GPS parse data attemp = %d\n", gps.parseData());
Spilly 0:e79311aae7ed 96 gps.parseData();
Spilly 0:e79311aae7ed 97 //printf("lat %f\tlon %f\thead %f\talt %f\tspd %f\tfix %d\tsat %d\n", gps.latitude, gps.longitude, gps.heading, gps.altitude, gps.speed, gps.fixtype, gps.satellites);
Spilly 0:e79311aae7ed 98 //update current position
Spilly 0:e79311aae7ed 99 curPos[0] = gps.latitude;
Spilly 0:e79311aae7ed 100 curPos[1] = gps.longitude;
Spilly 0:e79311aae7ed 101
Spilly 2:503a5ac6c3b6 102 //makeVector();
Spilly 2:503a5ac6c3b6 103 //xBee.printf("Main 3\n");
Spilly 2:503a5ac6c3b6 104 //get data from IMU and do calculations to determine heading
Spilly 2:503a5ac6c3b6 105 //updateAngles();
Spilly 2:503a5ac6c3b6 106 //updateAngles();
Spilly 0:e79311aae7ed 107
Spilly 2:503a5ac6c3b6 108 float latDif = sqrt((goalPos[0] - curPos[0]) * (goalPos[0] - curPos[0]));
Spilly 2:503a5ac6c3b6 109 float longDif = sqrt((goalPos[1] - curPos[1]) * (goalPos[1] - curPos[1]));
Spilly 0:e79311aae7ed 110
Spilly 2:503a5ac6c3b6 111 //Get absolute value of how far off goal latitude and longitude are from current latitude and longitude
Spilly 2:503a5ac6c3b6 112 //If it is less than tolerance for arriving, stop
Spilly 2:503a5ac6c3b6 113 if(polarVector[0] <= ARRIVED)
Spilly 2:503a5ac6c3b6 114 {
Spilly 2:503a5ac6c3b6 115 xBee.printf("We Have Arrived\n");
Spilly 2:503a5ac6c3b6 116 goStop(1,1);
Spilly 2:503a5ac6c3b6 117 sCheck = 0;
Spilly 2:503a5ac6c3b6 118 while(1);
Spilly 2:503a5ac6c3b6 119 }
Spilly 2:503a5ac6c3b6 120
Spilly 2:503a5ac6c3b6 121 if(updateAngles())
Spilly 2:503a5ac6c3b6 122 {
Spilly 2:503a5ac6c3b6 123 makeVector();
Spilly 2:503a5ac6c3b6 124 float magDiff = whichWay(yaw, polarVector[1]);
Spilly 2:503a5ac6c3b6 125 controller.setProcessValue(magDiff);
Spilly 2:503a5ac6c3b6 126
Spilly 2:503a5ac6c3b6 127 motorSpeed = controller.compute();
Spilly 2:503a5ac6c3b6 128 goForward(0.5f + motorSpeed,0.5f - motorSpeed);
Spilly 2:503a5ac6c3b6 129 wait(RATE);
Spilly 2:503a5ac6c3b6 130 }
Spilly 2:503a5ac6c3b6 131 //xBee.printf("dist %f\tneed %f\tcurrent %f\tdir %c\tlat %f\tlon %f\tgoalLat %f\tgoalLon %f\n", polarVector[0], polarVector[1], yaw, direction, gps.latitude, gps.longitude, goalPos[0], goalPos[1]);
Spilly 0:e79311aae7ed 132 }
Spilly 0:e79311aae7ed 133 }
Spilly 0:e79311aae7ed 134
Spilly 0:e79311aae7ed 135 //create polar vector based on two sets of latitude and longitude
Spilly 0:e79311aae7ed 136 void makeVector(void)
Spilly 0:e79311aae7ed 137 {
Spilly 0:e79311aae7ed 138 float arcLength[2];
Spilly 0:e79311aae7ed 139
Spilly 0:e79311aae7ed 140 //arc length = radius * angle
Spilly 0:e79311aae7ed 141 //Y
Spilly 0:e79311aae7ed 142 arcLength[1] = EARTHRADIUS * (goalPos[0] - curPos[0]);
Spilly 0:e79311aae7ed 143 //X
Spilly 0:e79311aae7ed 144 arcLength[0] = EARTHRADIUS * (curPos[1] - goalPos[1]);
Spilly 0:e79311aae7ed 145
Spilly 0:e79311aae7ed 146 //calculate magnitude of vector
Spilly 0:e79311aae7ed 147 polarVector[0] = sqrt((arcLength[0] * arcLength[0]) + (arcLength[1] * arcLength[1]));
Spilly 0:e79311aae7ed 148
Spilly 0:e79311aae7ed 149 //Use arcTan(-x/y) b/c we want our heading to be in respect to North (North = 0 degrees, East = 90 deg, etc.)
Spilly 0:e79311aae7ed 150 polarVector[1] = (RADTODEGREE * (atan2(-arcLength[0], arcLength[1])));
Spilly 0:e79311aae7ed 151
Spilly 0:e79311aae7ed 152 //make negative angles positive
Spilly 0:e79311aae7ed 153 if(polarVector[1] < 0) polarVector[1] = polarVector[1] + 360;
Spilly 0:e79311aae7ed 154 }
Spilly 0:e79311aae7ed 155
Spilly 2:503a5ac6c3b6 156 //Decide which direction to turn based on current compass heading(magHead) and heading required(calcHead)
Spilly 2:503a5ac6c3b6 157 float whichWay(float magHead, float calcHead)
Spilly 0:e79311aae7ed 158 {
Spilly 2:503a5ac6c3b6 159 float magDiff;
Spilly 0:e79311aae7ed 160
Spilly 0:e79311aae7ed 161 float absOfDiff = sqrt((calcHead - magHead) * (calcHead - magHead));
Spilly 0:e79311aae7ed 162
Spilly 0:e79311aae7ed 163 //Is the heading off enough to care?
Spilly 2:503a5ac6c3b6 164 if((absOfDiff >= HEADDIFF) && (absOfDiff <= (360 - HEADDIFF)))
Spilly 0:e79311aae7ed 165 {
Spilly 2:503a5ac6c3b6 166 //quadrant I
Spilly 2:503a5ac6c3b6 167 if(calcHead < 90)
Spilly 0:e79311aae7ed 168 {
Spilly 2:503a5ac6c3b6 169 if(calcHead < magHead)
Spilly 2:503a5ac6c3b6 170 {
Spilly 2:503a5ac6c3b6 171 if(magHead < (180 + calcHead))
Spilly 2:503a5ac6c3b6 172 {
Spilly 2:503a5ac6c3b6 173 //turn left need negative
Spilly 2:503a5ac6c3b6 174 magDiff = calcHead - magHead;
Spilly 2:503a5ac6c3b6 175 }
Spilly 2:503a5ac6c3b6 176 else
Spilly 2:503a5ac6c3b6 177 {
Spilly 2:503a5ac6c3b6 178 //turn right need positive
Spilly 2:503a5ac6c3b6 179 magDiff = calcHead - magHead + 360;
Spilly 2:503a5ac6c3b6 180 }
Spilly 2:503a5ac6c3b6 181 }
Spilly 2:503a5ac6c3b6 182 else
Spilly 0:e79311aae7ed 183 {
Spilly 2:503a5ac6c3b6 184 if(magHead < (180 + calcHead))
Spilly 2:503a5ac6c3b6 185 {
Spilly 2:503a5ac6c3b6 186 //turn left need negative
Spilly 2:503a5ac6c3b6 187 magDiff = calcHead - magHead;
Spilly 2:503a5ac6c3b6 188 }
Spilly 2:503a5ac6c3b6 189 else
Spilly 2:503a5ac6c3b6 190 {
Spilly 2:503a5ac6c3b6 191 //turn right need positive
Spilly 2:503a5ac6c3b6 192 magDiff = calcHead - magHead + 360;
Spilly 2:503a5ac6c3b6 193 }
Spilly 2:503a5ac6c3b6 194 }
Spilly 2:503a5ac6c3b6 195 }
Spilly 2:503a5ac6c3b6 196 //quadrant II
Spilly 2:503a5ac6c3b6 197 else if(calcHead < 180)
Spilly 2:503a5ac6c3b6 198 {
Spilly 2:503a5ac6c3b6 199 if(calcHead < magHead)
Spilly 2:503a5ac6c3b6 200 {
Spilly 2:503a5ac6c3b6 201 if(magHead < (180 + calcHead))
Spilly 2:503a5ac6c3b6 202 {
Spilly 2:503a5ac6c3b6 203 //turn left need negative
Spilly 2:503a5ac6c3b6 204 magDiff = calcHead - magHead;
Spilly 2:503a5ac6c3b6 205 }
Spilly 2:503a5ac6c3b6 206 else
Spilly 2:503a5ac6c3b6 207 {
Spilly 2:503a5ac6c3b6 208 //turn right need positive
Spilly 2:503a5ac6c3b6 209 magDiff = calcHead - magHead + 360;
Spilly 2:503a5ac6c3b6 210 }
Spilly 0:e79311aae7ed 211 }
Spilly 2:503a5ac6c3b6 212 else
Spilly 2:503a5ac6c3b6 213 {
Spilly 2:503a5ac6c3b6 214 if(magHead < (180 + calcHead))
Spilly 2:503a5ac6c3b6 215 {
Spilly 2:503a5ac6c3b6 216 //turn left need negative
Spilly 2:503a5ac6c3b6 217 magDiff = calcHead - magHead;
Spilly 2:503a5ac6c3b6 218 }
Spilly 2:503a5ac6c3b6 219 else
Spilly 2:503a5ac6c3b6 220 {
Spilly 2:503a5ac6c3b6 221 //turn right need positive
Spilly 2:503a5ac6c3b6 222 magDiff = calcHead - magHead + 360;
Spilly 2:503a5ac6c3b6 223 }
Spilly 2:503a5ac6c3b6 224 }
Spilly 0:e79311aae7ed 225 }
Spilly 2:503a5ac6c3b6 226 //quadrant III
Spilly 2:503a5ac6c3b6 227 else if(calcHead < 270)
Spilly 0:e79311aae7ed 228 {
Spilly 2:503a5ac6c3b6 229 if(calcHead < magHead)
Spilly 2:503a5ac6c3b6 230 {
Spilly 2:503a5ac6c3b6 231 if(magHead < (180 + calcHead))
Spilly 2:503a5ac6c3b6 232 {
Spilly 2:503a5ac6c3b6 233 //turn left need negative
Spilly 2:503a5ac6c3b6 234 magDiff = calcHead - magHead;
Spilly 2:503a5ac6c3b6 235 }
Spilly 2:503a5ac6c3b6 236 else
Spilly 2:503a5ac6c3b6 237 {
Spilly 2:503a5ac6c3b6 238 //turn right need positive
Spilly 2:503a5ac6c3b6 239 magDiff = calcHead - magHead + 360;
Spilly 2:503a5ac6c3b6 240 }
Spilly 2:503a5ac6c3b6 241 }
Spilly 2:503a5ac6c3b6 242 else
Spilly 2:503a5ac6c3b6 243 {
Spilly 2:503a5ac6c3b6 244 if(magHead < (180 + calcHead))
Spilly 2:503a5ac6c3b6 245 {
Spilly 2:503a5ac6c3b6 246 //turn left need negative
Spilly 2:503a5ac6c3b6 247 magDiff = calcHead - magHead;
Spilly 2:503a5ac6c3b6 248 }
Spilly 2:503a5ac6c3b6 249 else
Spilly 2:503a5ac6c3b6 250 {
Spilly 2:503a5ac6c3b6 251 //turn right need positive
Spilly 2:503a5ac6c3b6 252 magDiff = calcHead - magHead + 360;
Spilly 2:503a5ac6c3b6 253 }
Spilly 2:503a5ac6c3b6 254 }
Spilly 0:e79311aae7ed 255 }
Spilly 2:503a5ac6c3b6 256 //quadrant IV
Spilly 2:503a5ac6c3b6 257 else
Spilly 2:503a5ac6c3b6 258 {
Spilly 2:503a5ac6c3b6 259
Spilly 2:503a5ac6c3b6 260 if(calcHead < magHead)
Spilly 2:503a5ac6c3b6 261 {
Spilly 2:503a5ac6c3b6 262 magDiff = calcHead - magHead;
Spilly 2:503a5ac6c3b6 263 }
Spilly 2:503a5ac6c3b6 264 else
Spilly 2:503a5ac6c3b6 265 {
Spilly 2:503a5ac6c3b6 266 if(magHead < (calcHead - 180))
Spilly 2:503a5ac6c3b6 267 {
Spilly 2:503a5ac6c3b6 268
Spilly 2:503a5ac6c3b6 269 magDiff = calcHead - 360 - magHead;
Spilly 2:503a5ac6c3b6 270 }
Spilly 2:503a5ac6c3b6 271 else
Spilly 2:503a5ac6c3b6 272 {
Spilly 2:503a5ac6c3b6 273 //turn right need positive
Spilly 2:503a5ac6c3b6 274 magDiff = calcHead - magHead;
Spilly 2:503a5ac6c3b6 275 }
Spilly 2:503a5ac6c3b6 276 }
Spilly 2:503a5ac6c3b6 277 }
Spilly 2:503a5ac6c3b6 278 return magDiff;
Spilly 0:e79311aae7ed 279 }
Spilly 2:503a5ac6c3b6 280 return 0;
Spilly 0:e79311aae7ed 281 }
Spilly 0:e79311aae7ed 282
Spilly 2:503a5ac6c3b6 283
Spilly 2:503a5ac6c3b6 284
Spilly 0:e79311aae7ed 285 //Test for verifying heading
Spilly 0:e79311aae7ed 286 void testYaw(void)
Spilly 0:e79311aae7ed 287 {
Spilly 0:e79311aae7ed 288 //set test position to indicate a necessary heading of:
Spilly 0:e79311aae7ed 289 curPos[0] = 34.833716f;
Spilly 0:e79311aae7ed 290 curPos[1] = -82.404188f;
Spilly 0:e79311aae7ed 291
Spilly 0:e79311aae7ed 292 //due north
Spilly 0:e79311aae7ed 293 goalPos[0] = 35.833716f;
Spilly 0:e79311aae7ed 294 goalPos[1] = -82.404188f;
Spilly 0:e79311aae7ed 295
Spilly 0:e79311aae7ed 296 //Convert starting position and goal position to a vector
Spilly 0:e79311aae7ed 297 makeVector();
Spilly 0:e79311aae7ed 298
Spilly 0:e79311aae7ed 299 xBee.printf("dist %fmeters\tangle %fdegrees due north\n", polarVector[0], polarVector[1]);
Spilly 0:e79311aae7ed 300
Spilly 0:e79311aae7ed 301 //due south
Spilly 0:e79311aae7ed 302 goalPos[0] = 33.833716f;
Spilly 0:e79311aae7ed 303 goalPos[1] = -82.404188f;
Spilly 0:e79311aae7ed 304
Spilly 0:e79311aae7ed 305
Spilly 0:e79311aae7ed 306 //Convert starting position and goal position to a vector
Spilly 0:e79311aae7ed 307 makeVector();
Spilly 0:e79311aae7ed 308
Spilly 0:e79311aae7ed 309 xBee.printf("dist %fmeters\tangle %fdegrees due south\n", polarVector[0], polarVector[1]);
Spilly 0:e79311aae7ed 310
Spilly 0:e79311aae7ed 311 //due east
Spilly 0:e79311aae7ed 312 goalPos[0] = 34.833716f;
Spilly 0:e79311aae7ed 313 goalPos[1] = -81.404188f;
Spilly 0:e79311aae7ed 314
Spilly 0:e79311aae7ed 315 //Convert starting position and goal position to a vector
Spilly 0:e79311aae7ed 316 makeVector();
Spilly 0:e79311aae7ed 317
Spilly 0:e79311aae7ed 318 xBee.printf("dist %fmeters\tangle %fdegrees due east\n", polarVector[0], polarVector[1]);
Spilly 0:e79311aae7ed 319
Spilly 0:e79311aae7ed 320
Spilly 0:e79311aae7ed 321 //due west
Spilly 0:e79311aae7ed 322 goalPos[0] = 34.833716f;
Spilly 0:e79311aae7ed 323 goalPos[1] = -83.404188f;
Spilly 0:e79311aae7ed 324
Spilly 0:e79311aae7ed 325 //Convert starting position and goal position to a vector
Spilly 0:e79311aae7ed 326 makeVector();
Spilly 0:e79311aae7ed 327
Spilly 0:e79311aae7ed 328 xBee.printf("dist %fmeters\tangle %fdegrees due west\n", polarVector[0], polarVector[1]);
Spilly 0:e79311aae7ed 329
Spilly 0:e79311aae7ed 330 //north east
Spilly 0:e79311aae7ed 331 goalPos[0] = 35.833716f;
Spilly 0:e79311aae7ed 332 goalPos[1] = -81.404188f;
Spilly 0:e79311aae7ed 333
Spilly 0:e79311aae7ed 334 //Convert starting position and goal position to a vector
Spilly 0:e79311aae7ed 335 makeVector();
Spilly 0:e79311aae7ed 336
Spilly 0:e79311aae7ed 337 xBee.printf("dist %fmeters\tangle %fdegrees north east\n", polarVector[0], polarVector[1]);
Spilly 0:e79311aae7ed 338
Spilly 0:e79311aae7ed 339
Spilly 0:e79311aae7ed 340 //north west
Spilly 0:e79311aae7ed 341 goalPos[0] = 35.833716f;
Spilly 0:e79311aae7ed 342 goalPos[1] = -83.404188f;
Spilly 0:e79311aae7ed 343
Spilly 0:e79311aae7ed 344
Spilly 0:e79311aae7ed 345 //Convert starting position and goal position to a vector
Spilly 0:e79311aae7ed 346 makeVector();
Spilly 0:e79311aae7ed 347
Spilly 0:e79311aae7ed 348 xBee.printf("dist %fmeters\tangle %fdegrees north west\n", polarVector[0], polarVector[1]);
Spilly 0:e79311aae7ed 349
Spilly 0:e79311aae7ed 350 //south east
Spilly 0:e79311aae7ed 351 goalPos[0] = 33.833716f;
Spilly 0:e79311aae7ed 352 goalPos[1] = -81.404188f;
Spilly 0:e79311aae7ed 353
Spilly 0:e79311aae7ed 354 //Convert starting position and goal position to a vector
Spilly 0:e79311aae7ed 355 makeVector();
Spilly 0:e79311aae7ed 356
Spilly 0:e79311aae7ed 357 xBee.printf("dist %fmeters\tangle %fdegrees south east\n", polarVector[0], polarVector[1]);
Spilly 0:e79311aae7ed 358
Spilly 0:e79311aae7ed 359
Spilly 0:e79311aae7ed 360 //south west
Spilly 0:e79311aae7ed 361 goalPos[0] = 33.833716f;
Spilly 0:e79311aae7ed 362 goalPos[1] = -83.404188f;
Spilly 0:e79311aae7ed 363
Spilly 0:e79311aae7ed 364
Spilly 0:e79311aae7ed 365 //Convert starting position and goal position to a vector
Spilly 0:e79311aae7ed 366 makeVector();
Spilly 0:e79311aae7ed 367
Spilly 0:e79311aae7ed 368 xBee.printf("dist %fmeters\tangle %fdegrees south west\n", polarVector[0], polarVector[1]);
Spilly 0:e79311aae7ed 369
Spilly 0:e79311aae7ed 370 while(1);
Spilly 0:e79311aae7ed 371
Spilly 0:e79311aae7ed 372 /*
Spilly 0:e79311aae7ed 373 //below is useful when checking compass heading
Spilly 0:e79311aae7ed 374 while(1)
Spilly 0:e79311aae7ed 375 {
Spilly 0:e79311aae7ed 376 updateAngles();
Spilly 0:e79311aae7ed 377 char direction = whichWay(yaw, polarVector[1]);
Spilly 0:e79311aae7ed 378 //printf("accX %f\taccY %f\taccZ %f\tmagX %f\tmagY %f\tmagZ %f\troll %f\tpitch %f\tyaw %f\tturn %c\n", accel[0], accel[1], accel[2], magnetom[0], magnetom[1], magnetom[2], roll, pitch, yaw, direction);
Spilly 0:e79311aae7ed 379 wait(1);
Spilly 0:e79311aae7ed 380 }
Spilly 0:e79311aae7ed 381 */
Spilly 0:e79311aae7ed 382 }