David Spillman / Mbed 2 deprecated GPSNavigation

Crude navigation

Dependencies:   GPS L3GD20 LSM303DLHC mbed PID

main.cpp@3:ffa0e1429a72, 2015-03-09 (annotated)

Committer:
Spilly
Date:
Mon Mar 09 20:41:28 2015 +0000
Revision:
3:ffa0e1429a72
Parent:
2:503a5ac6c3b6
Child:
4:a397b44a0fe8
gps

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 3:ffa0e1429a72 10 #define HEADDIFF 0.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 3:ffa0e1429a72 16 #define RATE 0.1f
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 3:ffa0e1429a72 26 PID controller(.1, .005, .01, 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 3:ffa0e1429a72 47 //set mode to auto
Spilly 3:ffa0e1429a72 48 controller.setMode(1);
Spilly 2:503a5ac6c3b6 49 //We want the difference to be zero
Spilly 2:503a5ac6c3b6 50 controller.setSetPoint(0);
Spilly 0:e79311aae7ed 51
Spilly 2:503a5ac6c3b6 52 xBee.baud(9600);
Spilly 0:e79311aae7ed 53
Spilly 0:e79311aae7ed 54 xBee.printf("\nI'm Alive...\n");
Spilly 0:e79311aae7ed 55
Spilly 0:e79311aae7ed 56 //Setup the GPS
Spilly 0:e79311aae7ed 57 gps.Init();
Spilly 2:503a5ac6c3b6 58 xBee.printf("gps initialized\n");
Spilly 0:e79311aae7ed 59
Spilly 3:ffa0e1429a72 60 while(!xBee.readable())
Spilly 3:ffa0e1429a72 61 {
Spilly 3:ffa0e1429a72 62 xBee.printf("Press any key to begin\n");
Spilly 3:ffa0e1429a72 63 wait(1);
Spilly 3:ffa0e1429a72 64 }
Spilly 3:ffa0e1429a72 65
Spilly 3:ffa0e1429a72 66 //Test PID heading
Spilly 3:ffa0e1429a72 67 while(1)
Spilly 3:ffa0e1429a72 68 {
Spilly 3:ffa0e1429a72 69 if(updateAngles())
Spilly 3:ffa0e1429a72 70 {
Spilly 3:ffa0e1429a72 71 makeVector();
Spilly 3:ffa0e1429a72 72 //set heading to a fixed number for testing
Spilly 3:ffa0e1429a72 73 float magDiff = whichWay(yaw, 0);
Spilly 3:ffa0e1429a72 74 controller.setProcessValue(magDiff);
Spilly 3:ffa0e1429a72 75
Spilly 3:ffa0e1429a72 76 motorSpeed = controller.compute();
Spilly 3:ffa0e1429a72 77 goForward(0.5f + motorSpeed,0.5f - motorSpeed);
Spilly 3:ffa0e1429a72 78 xBee.printf("heading = %f\tdiff = %f\tspeed = %f\n", yaw, magDiff, motorSpeed);
Spilly 3:ffa0e1429a72 79 wait(RATE);
Spilly 3:ffa0e1429a72 80 }
Spilly 3:ffa0e1429a72 81 }
Spilly 3:ffa0e1429a72 82
Spilly 2:503a5ac6c3b6 83 xBee.printf("attempting to get a fix\n");
Spilly 0:e79311aae7ed 84
Spilly 0:e79311aae7ed 85 //wait until we have a gps fix
Spilly 0:e79311aae7ed 86 while(gps.fixtype == 0)
Spilly 0:e79311aae7ed 87 {
Spilly 0:e79311aae7ed 88 xBee.printf("fix %d\n", gps.fixtype);
Spilly 0:e79311aae7ed 89 gps.parseData();
Spilly 0:e79311aae7ed 90 wait(.2);
Spilly 0:e79311aae7ed 91 }
Spilly 0:e79311aae7ed 92
Spilly 0:e79311aae7ed 93 //set starting position
Spilly 0:e79311aae7ed 94 curPos[0] = gps.latitude;
Spilly 0:e79311aae7ed 95 curPos[1] = gps.longitude;
Spilly 0:e79311aae7ed 96
Spilly 0:e79311aae7ed 97 //Convert starting position and goal position to a vector
Spilly 0:e79311aae7ed 98 makeVector();
Spilly 0:e79311aae7ed 99
Spilly 0:e79311aae7ed 100 //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 101 //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 102
Spilly 2:503a5ac6c3b6 103
Spilly 2:503a5ac6c3b6 104 xBee.printf("starting main loop\n");
Spilly 0:e79311aae7ed 105 while (1)
Spilly 0:e79311aae7ed 106 {
Spilly 2:503a5ac6c3b6 107 //Emergency stop
Spilly 2:503a5ac6c3b6 108 if(xBee.readable())
Spilly 2:503a5ac6c3b6 109 {
Spilly 2:503a5ac6c3b6 110 char tempChar = xBee.getc();
Spilly 2:503a5ac6c3b6 111 if(tempChar == 'n')
Spilly 2:503a5ac6c3b6 112 {
Spilly 2:503a5ac6c3b6 113 xBee.printf("emergency stop\n");
Spilly 2:503a5ac6c3b6 114 goStop(1,1);
Spilly 2:503a5ac6c3b6 115 while(1);
Spilly 2:503a5ac6c3b6 116 }
Spilly 2:503a5ac6c3b6 117 }
Spilly 2:503a5ac6c3b6 118 //check GPS data
Spilly 2:503a5ac6c3b6 119 //xBee.printf("GPS parse data attemp = %d\n", gps.parseData());
Spilly 0:e79311aae7ed 120 gps.parseData();
Spilly 0:e79311aae7ed 121 //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 122 //update current position
Spilly 0:e79311aae7ed 123 curPos[0] = gps.latitude;
Spilly 0:e79311aae7ed 124 curPos[1] = gps.longitude;
Spilly 0:e79311aae7ed 125
Spilly 2:503a5ac6c3b6 126 //makeVector();
Spilly 2:503a5ac6c3b6 127 //xBee.printf("Main 3\n");
Spilly 2:503a5ac6c3b6 128 //get data from IMU and do calculations to determine heading
Spilly 2:503a5ac6c3b6 129 //updateAngles();
Spilly 2:503a5ac6c3b6 130 //updateAngles();
Spilly 0:e79311aae7ed 131
Spilly 2:503a5ac6c3b6 132 float latDif = sqrt((goalPos[0] - curPos[0]) * (goalPos[0] - curPos[0]));
Spilly 2:503a5ac6c3b6 133 float longDif = sqrt((goalPos[1] - curPos[1]) * (goalPos[1] - curPos[1]));
Spilly 0:e79311aae7ed 134
Spilly 2:503a5ac6c3b6 135 //Get absolute value of how far off goal latitude and longitude are from current latitude and longitude
Spilly 2:503a5ac6c3b6 136 //If it is less than tolerance for arriving, stop
Spilly 2:503a5ac6c3b6 137 if(polarVector[0] <= ARRIVED)
Spilly 2:503a5ac6c3b6 138 {
Spilly 2:503a5ac6c3b6 139 xBee.printf("We Have Arrived\n");
Spilly 2:503a5ac6c3b6 140 goStop(1,1);
Spilly 2:503a5ac6c3b6 141 sCheck = 0;
Spilly 2:503a5ac6c3b6 142 while(1);
Spilly 2:503a5ac6c3b6 143 }
Spilly 2:503a5ac6c3b6 144
Spilly 2:503a5ac6c3b6 145 if(updateAngles())
Spilly 2:503a5ac6c3b6 146 {
Spilly 2:503a5ac6c3b6 147 makeVector();
Spilly 2:503a5ac6c3b6 148 float magDiff = whichWay(yaw, polarVector[1]);
Spilly 2:503a5ac6c3b6 149 controller.setProcessValue(magDiff);
Spilly 2:503a5ac6c3b6 150
Spilly 2:503a5ac6c3b6 151 motorSpeed = controller.compute();
Spilly 2:503a5ac6c3b6 152 goForward(0.5f + motorSpeed,0.5f - motorSpeed);
Spilly 2:503a5ac6c3b6 153 wait(RATE);
Spilly 2:503a5ac6c3b6 154 }
Spilly 2:503a5ac6c3b6 155 //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 156 }
Spilly 0:e79311aae7ed 157 }
Spilly 0:e79311aae7ed 158
Spilly 0:e79311aae7ed 159 //create polar vector based on two sets of latitude and longitude
Spilly 0:e79311aae7ed 160 void makeVector(void)
Spilly 0:e79311aae7ed 161 {
Spilly 0:e79311aae7ed 162 float arcLength[2];
Spilly 0:e79311aae7ed 163
Spilly 0:e79311aae7ed 164 //arc length = radius * angle
Spilly 0:e79311aae7ed 165 //Y
Spilly 0:e79311aae7ed 166 arcLength[1] = EARTHRADIUS * (goalPos[0] - curPos[0]);
Spilly 0:e79311aae7ed 167 //X
Spilly 0:e79311aae7ed 168 arcLength[0] = EARTHRADIUS * (curPos[1] - goalPos[1]);
Spilly 0:e79311aae7ed 169
Spilly 0:e79311aae7ed 170 //calculate magnitude of vector
Spilly 0:e79311aae7ed 171 polarVector[0] = sqrt((arcLength[0] * arcLength[0]) + (arcLength[1] * arcLength[1]));
Spilly 0:e79311aae7ed 172
Spilly 0:e79311aae7ed 173 //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 174 polarVector[1] = (RADTODEGREE * (atan2(-arcLength[0], arcLength[1])));
Spilly 0:e79311aae7ed 175
Spilly 0:e79311aae7ed 176 //make negative angles positive
Spilly 0:e79311aae7ed 177 if(polarVector[1] < 0) polarVector[1] = polarVector[1] + 360;
Spilly 0:e79311aae7ed 178 }
Spilly 0:e79311aae7ed 179
Spilly 3:ffa0e1429a72 180 //Outputs difference in compass heading(magHead) and heading required(calcHead)
Spilly 3:ffa0e1429a72 181 //negative is left and positive is right
Spilly 2:503a5ac6c3b6 182 float whichWay(float magHead, float calcHead)
Spilly 0:e79311aae7ed 183 {
Spilly 2:503a5ac6c3b6 184 float magDiff;
Spilly 0:e79311aae7ed 185
Spilly 0:e79311aae7ed 186 float absOfDiff = sqrt((calcHead - magHead) * (calcHead - magHead));
Spilly 0:e79311aae7ed 187
Spilly 0:e79311aae7ed 188 //Is the heading off enough to care?
Spilly 2:503a5ac6c3b6 189 if((absOfDiff >= HEADDIFF) && (absOfDiff <= (360 - HEADDIFF)))
Spilly 3:ffa0e1429a72 190 //if(1)
Spilly 0:e79311aae7ed 191 {
Spilly 2:503a5ac6c3b6 192 //quadrant I
Spilly 2:503a5ac6c3b6 193 if(calcHead < 90)
Spilly 0:e79311aae7ed 194 {
Spilly 2:503a5ac6c3b6 195 if(calcHead < magHead)
Spilly 2:503a5ac6c3b6 196 {
Spilly 2:503a5ac6c3b6 197 if(magHead < (180 + calcHead))
Spilly 2:503a5ac6c3b6 198 {
Spilly 2:503a5ac6c3b6 199 //turn left need negative
Spilly 2:503a5ac6c3b6 200 magDiff = calcHead - magHead;
Spilly 2:503a5ac6c3b6 201 }
Spilly 2:503a5ac6c3b6 202 else
Spilly 2:503a5ac6c3b6 203 {
Spilly 2:503a5ac6c3b6 204 //turn right need positive
Spilly 2:503a5ac6c3b6 205 magDiff = calcHead - magHead + 360;
Spilly 2:503a5ac6c3b6 206 }
Spilly 2:503a5ac6c3b6 207 }
Spilly 2:503a5ac6c3b6 208 else
Spilly 0:e79311aae7ed 209 {
Spilly 2:503a5ac6c3b6 210 if(magHead < (180 + calcHead))
Spilly 2:503a5ac6c3b6 211 {
Spilly 2:503a5ac6c3b6 212 //turn left need negative
Spilly 2:503a5ac6c3b6 213 magDiff = calcHead - magHead;
Spilly 2:503a5ac6c3b6 214 }
Spilly 2:503a5ac6c3b6 215 else
Spilly 2:503a5ac6c3b6 216 {
Spilly 2:503a5ac6c3b6 217 //turn right need positive
Spilly 2:503a5ac6c3b6 218 magDiff = calcHead - magHead + 360;
Spilly 2:503a5ac6c3b6 219 }
Spilly 2:503a5ac6c3b6 220 }
Spilly 2:503a5ac6c3b6 221 }
Spilly 2:503a5ac6c3b6 222 //quadrant II
Spilly 2:503a5ac6c3b6 223 else if(calcHead < 180)
Spilly 2:503a5ac6c3b6 224 {
Spilly 2:503a5ac6c3b6 225 if(calcHead < magHead)
Spilly 2:503a5ac6c3b6 226 {
Spilly 2:503a5ac6c3b6 227 if(magHead < (180 + calcHead))
Spilly 2:503a5ac6c3b6 228 {
Spilly 2:503a5ac6c3b6 229 //turn left need negative
Spilly 2:503a5ac6c3b6 230 magDiff = calcHead - magHead;
Spilly 2:503a5ac6c3b6 231 }
Spilly 2:503a5ac6c3b6 232 else
Spilly 2:503a5ac6c3b6 233 {
Spilly 2:503a5ac6c3b6 234 //turn right need positive
Spilly 2:503a5ac6c3b6 235 magDiff = calcHead - magHead + 360;
Spilly 2:503a5ac6c3b6 236 }
Spilly 0:e79311aae7ed 237 }
Spilly 2:503a5ac6c3b6 238 else
Spilly 2:503a5ac6c3b6 239 {
Spilly 2:503a5ac6c3b6 240 if(magHead < (180 + calcHead))
Spilly 2:503a5ac6c3b6 241 {
Spilly 2:503a5ac6c3b6 242 //turn left need negative
Spilly 2:503a5ac6c3b6 243 magDiff = calcHead - magHead;
Spilly 2:503a5ac6c3b6 244 }
Spilly 2:503a5ac6c3b6 245 else
Spilly 2:503a5ac6c3b6 246 {
Spilly 2:503a5ac6c3b6 247 //turn right need positive
Spilly 2:503a5ac6c3b6 248 magDiff = calcHead - magHead + 360;
Spilly 2:503a5ac6c3b6 249 }
Spilly 2:503a5ac6c3b6 250 }
Spilly 0:e79311aae7ed 251 }
Spilly 2:503a5ac6c3b6 252 //quadrant III
Spilly 2:503a5ac6c3b6 253 else if(calcHead < 270)
Spilly 0:e79311aae7ed 254 {
Spilly 2:503a5ac6c3b6 255 if(calcHead < magHead)
Spilly 2:503a5ac6c3b6 256 {
Spilly 2:503a5ac6c3b6 257 if(magHead < (180 + calcHead))
Spilly 2:503a5ac6c3b6 258 {
Spilly 2:503a5ac6c3b6 259 //turn left need negative
Spilly 2:503a5ac6c3b6 260 magDiff = calcHead - magHead;
Spilly 2:503a5ac6c3b6 261 }
Spilly 2:503a5ac6c3b6 262 else
Spilly 2:503a5ac6c3b6 263 {
Spilly 2:503a5ac6c3b6 264 //turn right need positive
Spilly 2:503a5ac6c3b6 265 magDiff = calcHead - magHead + 360;
Spilly 2:503a5ac6c3b6 266 }
Spilly 2:503a5ac6c3b6 267 }
Spilly 2:503a5ac6c3b6 268 else
Spilly 2:503a5ac6c3b6 269 {
Spilly 2:503a5ac6c3b6 270 if(magHead < (180 + calcHead))
Spilly 2:503a5ac6c3b6 271 {
Spilly 2:503a5ac6c3b6 272 //turn left need negative
Spilly 2:503a5ac6c3b6 273 magDiff = calcHead - magHead;
Spilly 2:503a5ac6c3b6 274 }
Spilly 2:503a5ac6c3b6 275 else
Spilly 2:503a5ac6c3b6 276 {
Spilly 2:503a5ac6c3b6 277 //turn right need positive
Spilly 2:503a5ac6c3b6 278 magDiff = calcHead - magHead + 360;
Spilly 2:503a5ac6c3b6 279 }
Spilly 2:503a5ac6c3b6 280 }
Spilly 0:e79311aae7ed 281 }
Spilly 2:503a5ac6c3b6 282 //quadrant IV
Spilly 2:503a5ac6c3b6 283 else
Spilly 2:503a5ac6c3b6 284 {
Spilly 2:503a5ac6c3b6 285
Spilly 2:503a5ac6c3b6 286 if(calcHead < magHead)
Spilly 2:503a5ac6c3b6 287 {
Spilly 2:503a5ac6c3b6 288 magDiff = calcHead - magHead;
Spilly 2:503a5ac6c3b6 289 }
Spilly 2:503a5ac6c3b6 290 else
Spilly 2:503a5ac6c3b6 291 {
Spilly 2:503a5ac6c3b6 292 if(magHead < (calcHead - 180))
Spilly 2:503a5ac6c3b6 293 {
Spilly 2:503a5ac6c3b6 294
Spilly 2:503a5ac6c3b6 295 magDiff = calcHead - 360 - magHead;
Spilly 2:503a5ac6c3b6 296 }
Spilly 2:503a5ac6c3b6 297 else
Spilly 2:503a5ac6c3b6 298 {
Spilly 2:503a5ac6c3b6 299 //turn right need positive
Spilly 2:503a5ac6c3b6 300 magDiff = calcHead - magHead;
Spilly 2:503a5ac6c3b6 301 }
Spilly 2:503a5ac6c3b6 302 }
Spilly 2:503a5ac6c3b6 303 }
Spilly 2:503a5ac6c3b6 304 return magDiff;
Spilly 0:e79311aae7ed 305 }
Spilly 2:503a5ac6c3b6 306 return 0;
Spilly 0:e79311aae7ed 307 }