David Spillman / Mbed 2 deprecated GPSNavigation

Crude navigation

Dependencies:   GPS L3GD20 LSM303DLHC mbed PID

main.cpp@4:a397b44a0fe8, 2015-03-17 (annotated)

Committer:
Spilly
Date:
Tue Mar 17 01:13:17 2015 +0000
Revision:
4:a397b44a0fe8
Parent:
3:ffa0e1429a72 
Accel and magn offsets

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