David Spillman / Mbed 2 deprecated GPSNavigationNew

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:   GPS2 L3GD20 LSM303DLHC2 PID mbed SDFileSystem

Fork of GPSNavigation by David Spillman

main.cpp@11:1b34319671eb, 2015-04-27 (annotated)

Committer:
Spilly
Date:
Mon Apr 27 18:10:52 2015 +0000
Revision:
11:1b34319671eb
Parent:
10:b6bf86de613f
Child:
12:273479524c71
4/27/2015 2:10

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Spilly 8:c77ab7615b21 1 /************************************************************************************************************************************************************************************************/
Spilly 5:40ac894e0fa7 2 // Created by: Ryan Spillman
Spilly 5:40ac894e0fa7 3 //
Spilly 9:fb8e34e31dfb 4 // Last updated 4/27/2015
Spilly 5:40ac894e0fa7 5 //
Spilly 5:40ac894e0fa7 6 // This is the software for my teams autonomous boat that is our graduation/final project at Isothermal Community College
Spilly 5:40ac894e0fa7 7 //
Spilly 5:40ac894e0fa7 8 // The user can drive the vehicle by sending chars over the xBee's serial connection
Spilly 8:c77ab7615b21 9 // GPS waypoints are stored on an external micro-SD card
Spilly 8:c77ab7615b21 10 // The user can record new waypoints to the SD card by driving to a location and entering record mode
Spilly 8:c77ab7615b21 11 // The user can also manually adjust the waypoints with a text editor
Spilly 5:40ac894e0fa7 12 //
Spilly 5:40ac894e0fa7 13 // A PID loop is used to control the heading of the vehicle
Spilly 5:40ac894e0fa7 14 //
Spilly 5:40ac894e0fa7 15 // The project uses a FRDM-K64f (Freescale microcontroller), a LSM303DLHC (magnetometer and accelerometer) to create a tilt compensated comapass,
Spilly 10:b6bf86de613f 16 // MTK3339 GPS module, two xBee Pro S1, three TE KRPA-11 relays (relay logic H-bridge for trolling motor), and a L298n (H-Bridge for linear actuator)
Spilly 8:c77ab7615b21 17 //
Spilly 8:c77ab7615b21 18 /***************************************************How To***************************************************************************************************************************************/
Spilly 8:c77ab7615b21 19 //
Spilly 10:b6bf86de613f 20 // Requires a serial to usb adapter to connect an X-Bee to a PC
Spilly 10:b6bf86de613f 21 // Set both X-Bees up for 115200 baud
Spilly 10:b6bf86de613f 22 // Use TeraTerm (or other serial program) to read and send data over X-Bees
Spilly 10:b6bf86de613f 23 //
Spilly 8:c77ab7615b21 24 // Program starts by prompting user to press any key
Spilly 8:c77ab7615b21 25 // Once user presses a key, the program waits for a DGPS fix (can be set by changing "FIX")
Spilly 8:c77ab7615b21 26 // Once the program sees a DGPS fix, manual mode is enabled
Spilly 8:c77ab7615b21 27 // User can drive the vehicle in manual mode to any position
Spilly 8:c77ab7615b21 28 // User can record current position to a selected goal position in record mode
Spilly 8:c77ab7615b21 29 // In autonomous mode, the vehicle uses GPS data and compass data to navigate to each goal position
Spilly 8:c77ab7615b21 30 //
Spilly 8:c77ab7615b21 31 // Controls in manual mode:
Spilly 8:c77ab7615b21 32 // directional:
Spilly 8:c77ab7615b21 33 // w = forward
Spilly 8:c77ab7615b21 34 // s = backward
Spilly 8:c77ab7615b21 35 // a = left
Spilly 8:c77ab7615b21 36 // d = right
Spilly 8:c77ab7615b21 37 // mode:
Spilly 8:c77ab7615b21 38 // r = change to record mode
Spilly 8:c77ab7615b21 39 // z = change to autonomous mode
Spilly 8:c77ab7615b21 40 //
Spilly 8:c77ab7615b21 41 // Controls in autonomous mode:
Spilly 8:c77ab7615b21 42 // mode:
Spilly 8:c77ab7615b21 43 // y = change to manual mode
Spilly 8:c77ab7615b21 44 // adjustments:
Spilly 8:c77ab7615b21 45 // d = increase angle
Spilly 8:c77ab7615b21 46 // a = decrease angle
Spilly 8:c77ab7615b21 47 // r = enter new waypoint number
Spilly 8:c77ab7615b21 48 // + = increase (depends on adjust mode)
Spilly 8:c77ab7615b21 49 // - = decrease (depends on adjust mode)
Spilly 8:c77ab7615b21 50 // p = change adjust mode
Spilly 8:c77ab7615b21 51 //
Spilly 8:c77ab7615b21 52 // Controls in record mode:
Spilly 8:c77ab7615b21 53 // *follow serial prompts to record positions
Spilly 8:c77ab7615b21 54 // mode:
Spilly 8:c77ab7615b21 55 // y = change to manual mode
Spilly 8:c77ab7615b21 56 //
Spilly 8:c77ab7615b21 57 /*************************************************************************************************************************************************************************************************/
Spilly 0:e79311aae7ed 58
Spilly 5:40ac894e0fa7 59 #include "mbed.h"
Spilly 8:c77ab7615b21 60 #include "GPS.h"
Spilly 5:40ac894e0fa7 61 #include "PID.h"
Spilly 8:c77ab7615b21 62 #include "SDFileSystem.h"
Spilly 5:40ac894e0fa7 63 #include "modSensData.h"
Spilly 5:40ac894e0fa7 64 #include "navigation.h"
Spilly 8:c77ab7615b21 65 #include "Actuator.h"
Spilly 8:c77ab7615b21 66 #include "TrollingMotor.h"
Spilly 0:e79311aae7ed 67
Spilly 8:c77ab7615b21 68 #define VOLT_MULT (3.3f / (0.810f / (3.3f + 0.810f))) //voltage divider 3.3k and 810 (VREF = 3.3V) keeps 12V measurements below 3.3V
Spilly 11:1b34319671eb 69 #define RATIO_TOLERANCE 0.02f //How close the difference between the set ratio and current ratio before consider
Spilly 8:c77ab7615b21 70 #define MIN_RATIO 0.04f //Actuator hits retract limit swithc at 2.2%
Spilly 8:c77ab7615b21 71 #define MAX_RATIO 0.85f //Actuator hits extend limit switch at 87.6%
Spilly 8:c77ab7615b21 72 #define CENTER_RATIO 0.29f //Ratio where prop is centered
Spilly 8:c77ab7615b21 73
Spilly 8:c77ab7615b21 74 #define FIX 0 // 2 = DGPS (more accurate but slower to initialize) 1 = GPS only (less accurate but faster to initialize)
Spilly 8:c77ab7615b21 75 #define ARRIVED 5.0f //Tolerance, in meters, for when goal location is reached
Spilly 8:c77ab7615b21 76 #define GPS_ACCUR 3.0f //accuracy of GPS unit
Spilly 8:c77ab7615b21 77 #define GPS_PERIOD 1.0f //GPS polling period (1 Hz)
Spilly 8:c77ab7615b21 78 #define GPS_POLL 0.5f
Spilly 7:ebc76b0f21da 79 #define GPS_ALPHA 0.3f //GPS low pass alpha
Spilly 8:c77ab7615b21 80
Spilly 11:1b34319671eb 81 #define RATE 0.3f //period of heading PID loop
Spilly 8:c77ab7615b21 82 #define headKc 1.0f //directly proportional
Spilly 8:c77ab7615b21 83 #define headTi 0.0f //a larger number makes the integral have less affect on the output
Spilly 8:c77ab7615b21 84 #define headTd 0.0f //a smaller number makes the derivative have less affect on the output
Spilly 8:c77ab7615b21 85 PID headingPID(headKc, headTi, headTd, MAGN_PERIOD); //Kc, Ti, Td, interval
Spilly 8:c77ab7615b21 86
Spilly 8:c77ab7615b21 87 AnalogIn battery(A0);
Spilly 8:c77ab7615b21 88 AnalogIn pot(A1);
Spilly 0:e79311aae7ed 89
Spilly 5:40ac894e0fa7 90 Serial xBee(PTB11, PTB10); //UART 3
Spilly 5:40ac894e0fa7 91 GPS gps(PTC15, PTC14); //UART 4
Spilly 5:40ac894e0fa7 92
Spilly 5:40ac894e0fa7 93 Timer headingTime;
Spilly 5:40ac894e0fa7 94 Timer acc;
Spilly 5:40ac894e0fa7 95 Timer magn;
Spilly 5:40ac894e0fa7 96 Timer inputTimer;
Spilly 8:c77ab7615b21 97 Timer loopTimer;
Spilly 4:a397b44a0fe8 98
Spilly 8:c77ab7615b21 99 FILE *fr; //file pointer for SD card
Spilly 8:c77ab7615b21 100
Spilly 8:c77ab7615b21 101 //On board microSD
Spilly 8:c77ab7615b21 102 SDFileSystem sd(PTE3, PTE1, PTE2, PTE4, "sd", PTE6, SDFileSystem::SWITCH_POS_NO, 50000000);
Spilly 8:c77ab7615b21 103
Spilly 8:c77ab7615b21 104 /***************************************************Prototype functions****************************************************************************************************************************/
Spilly 8:c77ab7615b21 105 void getDist(double posZero, double posOne, double curPos[2]);
Spilly 8:c77ab7615b21 106 void getAngle(double posZero, double posOne, double curPos[2], int flush);
Spilly 8:c77ab7615b21 107 /***************************************************End of prototype functions*********************************************************************************************************************/
Spilly 8:c77ab7615b21 108
Spilly 8:c77ab7615b21 109 /***************************************************Global Variables*******************************************************************************************************************************/
Spilly 8:c77ab7615b21 110 double polarVector[2] = {0,0.0000001f};
Spilly 8:c77ab7615b21 111 float manualSpeed = 0.5f;
Spilly 8:c77ab7615b21 112 double curPos[2] = {0,0};
Spilly 2:503a5ac6c3b6 113
Spilly 8:c77ab7615b21 114 //waypoint data is overwritten by data from SD card
Spilly 8:c77ab7615b21 115 double goalPos[10][2] = { {35.339289, -81.913164},
Spilly 8:c77ab7615b21 116 {35.338943, -81.911024},
Spilly 8:c77ab7615b21 117 {35.339289, -81.913164},
Spilly 8:c77ab7615b21 118 {35.338943, -81.911024},
Spilly 8:c77ab7615b21 119 {35.339289, -81.913164},
Spilly 8:c77ab7615b21 120 {35.338943, -81.911024},
Spilly 8:c77ab7615b21 121 {35.339289, -81.913164},
Spilly 8:c77ab7615b21 122 {35.338943, -81.911024},
Spilly 8:c77ab7615b21 123 {35.339289, -81.913164},
Spilly 8:c77ab7615b21 124 {35.338943, -81.911024}
Spilly 8:c77ab7615b21 125 };
Spilly 0:e79311aae7ed 126
Spilly 8:c77ab7615b21 127 /*************************************************************************************************************************************************************************************************/
Spilly 7:ebc76b0f21da 128 // MAIN
Spilly 8:c77ab7615b21 129 /*************************************************************************************************************************************************************************************************/
Spilly 0:e79311aae7ed 130
Spilly 0:e79311aae7ed 131 int main()
Spilly 0:e79311aae7ed 132 {
Spilly 8:c77ab7615b21 133 int wayPtNum = 0, mode = 0, adjustMode = 0;
Spilly 5:40ac894e0fa7 134 float magDiff = 0;
Spilly 8:c77ab7615b21 135 float batVoltage = 0.0f, potVoltage = 0.0f, voltRatio = 0.0f;
Spilly 8:c77ab7615b21 136 float curSet = 0.0f, prevSet = 0.29f;
Spilly 8:c77ab7615b21 137 float filtered = 0.0000001f;
Spilly 0:e79311aae7ed 138
Spilly 5:40ac894e0fa7 139 xBee.baud(115200);
Spilly 0:e79311aae7ed 140 xBee.printf("\nI'm Alive...\n");
Spilly 5:40ac894e0fa7 141 xBee.printf("Press any key to begin\n");
Spilly 5:40ac894e0fa7 142
Spilly 8:c77ab7615b21 143 //wait for keypress to begin
Spilly 5:40ac894e0fa7 144 while(!xBee.readable());
Spilly 5:40ac894e0fa7 145
Spilly 8:c77ab7615b21 146 //start of SD card read
Spilly 8:c77ab7615b21 147 fr = fopen ("/sd/GPS_CORDS.txt", "rt");
Spilly 8:c77ab7615b21 148
Spilly 8:c77ab7615b21 149 xBee.printf("Reading SD Card Please Wait\n");
Spilly 8:c77ab7615b21 150
Spilly 8:c77ab7615b21 151 for(int x = 0; x<=9; x++)
Spilly 8:c77ab7615b21 152 {
Spilly 8:c77ab7615b21 153 fscanf(fr, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
Spilly 8:c77ab7615b21 154 xBee.printf("waypoint %d = %f,%f\n", x, goalPos[x][0], goalPos[x][1]);
Spilly 8:c77ab7615b21 155 }
Spilly 8:c77ab7615b21 156 fclose(fr);
Spilly 8:c77ab7615b21 157 //end of SD card read
Spilly 8:c77ab7615b21 158
Spilly 5:40ac894e0fa7 159 //initialize magnetometer, accelerometer
Spilly 5:40ac894e0fa7 160 compass.init();
Spilly 7:ebc76b0f21da 161 wait(1);
Spilly 0:e79311aae7ed 162 //Setup the GPS
Spilly 0:e79311aae7ed 163 gps.Init();
Spilly 5:40ac894e0fa7 164
Spilly 2:503a5ac6c3b6 165 xBee.printf("gps initialized\n");
Spilly 0:e79311aae7ed 166
Spilly 2:503a5ac6c3b6 167 xBee.printf("attempting to get a fix\n");
Spilly 8:c77ab7615b21 168
Spilly 8:c77ab7615b21 169 while(gps.fixtype != FIX)
Spilly 0:e79311aae7ed 170 {
Spilly 8:c77ab7615b21 171 while(!gps.getData());
Spilly 8:c77ab7615b21 172 if(gps.fixtype == 1)
Spilly 5:40ac894e0fa7 173 {
Spilly 8:c77ab7615b21 174 xBee.printf("Waiting for DGPS fix\tcurrent fix = GPS only\n");
Spilly 8:c77ab7615b21 175 xBee.printf("lat %f\tlon %f\thead %f\talt %f\tspd %f\tfix %d\tsat %d\n", gps.degLat, gps.degLon, gps.heading, gps.altitude, gps.speed, gps.fixtype, gps.satellites);
Spilly 5:40ac894e0fa7 176 }
Spilly 8:c77ab7615b21 177 else xBee.printf("Waiting for DGPS fix\tcurrent fix = no fix\n");
Spilly 0:e79311aae7ed 178 }
Spilly 5:40ac894e0fa7 179 //get IMU data and calculate the tilt compensated compass
Spilly 5:40ac894e0fa7 180 getAccel();
Spilly 5:40ac894e0fa7 181 getMagn();
Spilly 5:40ac894e0fa7 182 updateAngles();
Spilly 5:40ac894e0fa7 183
Spilly 7:ebc76b0f21da 184 xBee.printf("lat %f\tlon %f\thead %f\talt %f\tspd %f\tfix %d\tsat %d\n", gps.degLat, gps.degLon, gps.heading, gps.altitude, gps.speed, gps.fixtype, gps.satellites);
Spilly 5:40ac894e0fa7 185 xBee.printf("dist %f\theading %f\n", polarVector[0], polarVector[1]);
Spilly 7:ebc76b0f21da 186 xBee.printf("\n\nstarting main loop\n");
Spilly 5:40ac894e0fa7 187
Spilly 7:ebc76b0f21da 188 //PID control of left and right motors based on input from tilt compensated compass
Spilly 5:40ac894e0fa7 189 //Goal is to have the vehicle go straight
Spilly 5:40ac894e0fa7 190 headingPID.setInputLimits(-180, 180);
Spilly 8:c77ab7615b21 191
Spilly 8:c77ab7615b21 192 //set proportional output limits based on physical limits of actuator and mounting error
Spilly 8:c77ab7615b21 193 float distFromCenter = calcEnds(CENTER_RATIO, MAX_RATIO, MIN_RATIO);
Spilly 8:c77ab7615b21 194
Spilly 8:c77ab7615b21 195 headingPID.setOutputLimits((CENTER_RATIO - distFromCenter), (CENTER_RATIO + distFromCenter));
Spilly 5:40ac894e0fa7 196 //set mode to auto
Spilly 5:40ac894e0fa7 197 headingPID.setMode(0);
Spilly 5:40ac894e0fa7 198 //We want the difference to be zero
Spilly 5:40ac894e0fa7 199 headingPID.setSetPoint(0);
Spilly 0:e79311aae7ed 200
Spilly 8:c77ab7615b21 201 loopTimer.start();
Spilly 5:40ac894e0fa7 202 headingTime.start();
Spilly 5:40ac894e0fa7 203 acc.start();
Spilly 5:40ac894e0fa7 204 magn.start();
Spilly 8:c77ab7615b21 205
Spilly 0:e79311aae7ed 206 while (1)
Spilly 0:e79311aae7ed 207 {
Spilly 8:c77ab7615b21 208 /*********************************************************************************************************************************************************************************************/
Spilly 7:ebc76b0f21da 209 // manual mode
Spilly 8:c77ab7615b21 210 /*********************************************************************************************************************************************************************************************/
Spilly 7:ebc76b0f21da 211
Spilly 5:40ac894e0fa7 212 if(mode == 0)
Spilly 2:503a5ac6c3b6 213 {
Spilly 8:c77ab7615b21 214 if(gps.getData())
Spilly 8:c77ab7615b21 215 {
Spilly 8:c77ab7615b21 216 //gps.parseData();
Spilly 8:c77ab7615b21 217 //xBee.printf("lat %f\tlon %f\tHDOP %f\tfix %d\tsat %d\tspd %f\n", gps.degLat, gps.degLon, gps.HDOP, gps.fixtype, gps.satellites, gps.speed);
Spilly 8:c77ab7615b21 218 //GPSTimer.reset();
Spilly 8:c77ab7615b21 219 }
Spilly 8:c77ab7615b21 220 //This moves actuator to the requested position
Spilly 8:c77ab7615b21 221 //This is proportional feedback only
Spilly 8:c77ab7615b21 222 if(loopTimer.read() > RATE)
Spilly 5:40ac894e0fa7 223 {
Spilly 8:c77ab7615b21 224 potVoltage = pot.read();
Spilly 8:c77ab7615b21 225 batVoltage = battery.read();
Spilly 8:c77ab7615b21 226 //voltage = voltage * 3.3f;
Spilly 8:c77ab7615b21 227 voltRatio = potVoltage / batVoltage;
Spilly 8:c77ab7615b21 228
Spilly 8:c77ab7615b21 229 float absDiff = sqrt((prevSet - voltRatio) * (prevSet - voltRatio));
Spilly 8:c77ab7615b21 230 if(absDiff <= RATIO_TOLERANCE)
Spilly 8:c77ab7615b21 231 {
Spilly 8:c77ab7615b21 232 turnStop(1.0f, 1.0f);
Spilly 8:c77ab7615b21 233 //xBee.printf("done\n");
Spilly 8:c77ab7615b21 234 }
Spilly 8:c77ab7615b21 235 else if((prevSet - voltRatio) >= 0)
Spilly 8:c77ab7615b21 236 {
Spilly 8:c77ab7615b21 237 turnRight(1.0f, 1.0f);
Spilly 8:c77ab7615b21 238 //xBee.printf("turning right\n");
Spilly 8:c77ab7615b21 239 }
Spilly 8:c77ab7615b21 240 else
Spilly 8:c77ab7615b21 241 {
Spilly 8:c77ab7615b21 242 turnLeft(1.0f, 1.0f);
Spilly 8:c77ab7615b21 243 //xBee.printf("turning left\n");
Spilly 8:c77ab7615b21 244 }
Spilly 8:c77ab7615b21 245 xBee.printf("battery = %f\tpot = %f\tratio = %f\tset %f\tHDOP = %f\tfix = %f\n", (batVoltage * VOLT_MULT), (potVoltage* VOLT_MULT), voltRatio, prevSet, gps.HDOP, gps.fixtype);
Spilly 8:c77ab7615b21 246 loopTimer.reset();
Spilly 5:40ac894e0fa7 247 }
Spilly 7:ebc76b0f21da 248 //check to see if data is available on xBee
Spilly 5:40ac894e0fa7 249 if(xBee.readable())
Spilly 2:503a5ac6c3b6 250 {
Spilly 5:40ac894e0fa7 251 char recChar = xBee.getc();
Spilly 5:40ac894e0fa7 252
Spilly 5:40ac894e0fa7 253 //change to autonomous mode
Spilly 5:40ac894e0fa7 254 if(recChar == 'z')
Spilly 5:40ac894e0fa7 255 {
Spilly 8:c77ab7615b21 256 xBee.printf("Changing to autonomous mode\n");
Spilly 8:c77ab7615b21 257 goStop();
Spilly 5:40ac894e0fa7 258 mode = 1;
Spilly 5:40ac894e0fa7 259 }
Spilly 5:40ac894e0fa7 260 //change to record mode
Spilly 8:c77ab7615b21 261 else if(recChar == 'r')
Spilly 5:40ac894e0fa7 262 {
Spilly 8:c77ab7615b21 263 xBee.printf("Changing to record mode\n");
Spilly 8:c77ab7615b21 264 goStop();
Spilly 5:40ac894e0fa7 265 mode = 3;
Spilly 5:40ac894e0fa7 266 }
Spilly 8:c77ab7615b21 267 else if(recChar == '1')
Spilly 8:c77ab7615b21 268 {
Spilly 8:c77ab7615b21 269 xBee.printf("stop\n");
Spilly 8:c77ab7615b21 270 goStop();
Spilly 8:c77ab7615b21 271 }
Spilly 8:c77ab7615b21 272 else if(recChar == 'w')
Spilly 5:40ac894e0fa7 273 {
Spilly 8:c77ab7615b21 274 goForward();
Spilly 8:c77ab7615b21 275 prevSet = CENTER_RATIO;
Spilly 8:c77ab7615b21 276 xBee.printf("Forward\n");
Spilly 8:c77ab7615b21 277 }
Spilly 8:c77ab7615b21 278 else if(recChar == 's')
Spilly 8:c77ab7615b21 279 {
Spilly 8:c77ab7615b21 280 goBackward();
Spilly 8:c77ab7615b21 281 prevSet = CENTER_RATIO;
Spilly 8:c77ab7615b21 282 xBee.printf("backward\n");
Spilly 8:c77ab7615b21 283 }
Spilly 8:c77ab7615b21 284 else if(recChar == 'd')
Spilly 8:c77ab7615b21 285 {
Spilly 8:c77ab7615b21 286 xBee.printf("large step right\n");
Spilly 5:40ac894e0fa7 287
Spilly 8:c77ab7615b21 288 //find the best step size since 0.1 step will go over the limit
Spilly 8:c77ab7615b21 289 if(prevSet + 0.1f > MAX_RATIO)
Spilly 8:c77ab7615b21 290 {
Spilly 8:c77ab7615b21 291 prevSet = prevSet + (MAX_RATIO - prevSet);
Spilly 8:c77ab7615b21 292 }
Spilly 8:c77ab7615b21 293 else
Spilly 8:c77ab7615b21 294 {
Spilly 8:c77ab7615b21 295 prevSet = prevSet + 0.1f;
Spilly 8:c77ab7615b21 296 }
Spilly 8:c77ab7615b21 297 xBee.printf("set = %f\n", prevSet);
Spilly 5:40ac894e0fa7 298 }
Spilly 8:c77ab7615b21 299 //large step left
Spilly 8:c77ab7615b21 300 else if(recChar == 'a')
Spilly 5:40ac894e0fa7 301 {
Spilly 8:c77ab7615b21 302 xBee.printf("large step left\n");
Spilly 8:c77ab7615b21 303 //find the best step size since 0.1 step will go over the limit
Spilly 8:c77ab7615b21 304 if(prevSet - 0.1f < MIN_RATIO)
Spilly 8:c77ab7615b21 305 {
Spilly 8:c77ab7615b21 306 prevSet = prevSet - (prevSet - MIN_RATIO);
Spilly 8:c77ab7615b21 307 }
Spilly 8:c77ab7615b21 308 else
Spilly 8:c77ab7615b21 309 {
Spilly 8:c77ab7615b21 310 prevSet = prevSet - 0.1f;
Spilly 8:c77ab7615b21 311 }
Spilly 8:c77ab7615b21 312 xBee.printf("set = %f\n", prevSet);
Spilly 5:40ac894e0fa7 313 }
Spilly 8:c77ab7615b21 314 //small step right
Spilly 8:c77ab7615b21 315 else if(recChar == 'e')
Spilly 5:40ac894e0fa7 316 {
Spilly 8:c77ab7615b21 317 xBee.printf("small step right\n");
Spilly 8:c77ab7615b21 318 if(prevSet + 0.01f > MAX_RATIO)
Spilly 8:c77ab7615b21 319 {
Spilly 8:c77ab7615b21 320 prevSet = prevSet + (MAX_RATIO - prevSet);
Spilly 8:c77ab7615b21 321 }
Spilly 8:c77ab7615b21 322 else
Spilly 8:c77ab7615b21 323 {
Spilly 8:c77ab7615b21 324 prevSet = prevSet + 0.01f;
Spilly 8:c77ab7615b21 325 }
Spilly 8:c77ab7615b21 326 xBee.printf("set = %f\n", prevSet);
Spilly 8:c77ab7615b21 327 }
Spilly 8:c77ab7615b21 328 else if(recChar == 'q')
Spilly 8:c77ab7615b21 329 {
Spilly 8:c77ab7615b21 330 xBee.printf("Small step left\n");
Spilly 8:c77ab7615b21 331 if(prevSet - 0.01f < MIN_RATIO)
Spilly 8:c77ab7615b21 332 {
Spilly 8:c77ab7615b21 333 prevSet = prevSet - (prevSet - MIN_RATIO);
Spilly 8:c77ab7615b21 334 }
Spilly 8:c77ab7615b21 335 else
Spilly 8:c77ab7615b21 336 {
Spilly 8:c77ab7615b21 337 prevSet = prevSet - 0.01f;
Spilly 8:c77ab7615b21 338 }
Spilly 8:c77ab7615b21 339 xBee.printf("set = %f\n", prevSet);
Spilly 5:40ac894e0fa7 340 }
Spilly 2:503a5ac6c3b6 341 }
Spilly 2:503a5ac6c3b6 342 }
Spilly 7:ebc76b0f21da 343
Spilly 8:c77ab7615b21 344 /*********************************************************************************************************************************************************************************************/
Spilly 8:c77ab7615b21 345 // autonomous mode
Spilly 8:c77ab7615b21 346 /*********************************************************************************************************************************************************************************************/
Spilly 7:ebc76b0f21da 347
Spilly 5:40ac894e0fa7 348 if(mode == 1)
Spilly 2:503a5ac6c3b6 349 {
Spilly 7:ebc76b0f21da 350 //check xBee
Spilly 5:40ac894e0fa7 351 if(xBee.readable())
Spilly 2:503a5ac6c3b6 352 {
Spilly 5:40ac894e0fa7 353 char recChar = xBee.getc();
Spilly 8:c77ab7615b21 354 if(recChar == '1')
Spilly 2:503a5ac6c3b6 355 {
Spilly 8:c77ab7615b21 356 xBee.printf("stop\n");
Spilly 8:c77ab7615b21 357 goStop();
Spilly 8:c77ab7615b21 358 }
Spilly 8:c77ab7615b21 359 if(recChar == 'w')
Spilly 8:c77ab7615b21 360 {
Spilly 8:c77ab7615b21 361 xBee.printf("forward\n");
Spilly 8:c77ab7615b21 362 goForward();
Spilly 2:503a5ac6c3b6 363 }
Spilly 5:40ac894e0fa7 364 //change to manual mode
Spilly 5:40ac894e0fa7 365 if(recChar == 'y')
Spilly 2:503a5ac6c3b6 366 {
Spilly 8:c77ab7615b21 367 xBee.printf("Changing to manual mode\n");
Spilly 8:c77ab7615b21 368 goStop();
Spilly 5:40ac894e0fa7 369 mode = 0;
Spilly 7:ebc76b0f21da 370 wayPtNum = 0;
Spilly 2:503a5ac6c3b6 371 }
Spilly 8:c77ab7615b21 372 //increase calculated heading (use this to tweak/cheat calculated heading)
Spilly 8:c77ab7615b21 373 else if(recChar == 'd')
Spilly 8:c77ab7615b21 374 {
Spilly 8:c77ab7615b21 375 polarVector[1] = polarVector[1] + 1;
Spilly 8:c77ab7615b21 376 xBee.printf("increased angle %f\n", polarVector[1]);
Spilly 8:c77ab7615b21 377 }
Spilly 8:c77ab7615b21 378 //reduce calculated heading (use this to tweak/cheat calculated heading)
Spilly 8:c77ab7615b21 379 else if(recChar == 'a')
Spilly 8:c77ab7615b21 380 {
Spilly 8:c77ab7615b21 381 polarVector[1] = polarVector[1] - 1;
Spilly 8:c77ab7615b21 382 xBee.printf("reduced angle %f\n", polarVector[1]);
Spilly 8:c77ab7615b21 383 }
Spilly 8:c77ab7615b21 384 else if(recChar == '+')
Spilly 8:c77ab7615b21 385 {
Spilly 8:c77ab7615b21 386 if(adjustMode == 0)
Spilly 8:c77ab7615b21 387 {
Spilly 8:c77ab7615b21 388 if(wayPtNum != 9)
Spilly 8:c77ab7615b21 389 {
Spilly 8:c77ab7615b21 390 wayPtNum ++;
Spilly 8:c77ab7615b21 391 xBee.printf("waypoint increased to %d\n", wayPtNum);
Spilly 8:c77ab7615b21 392 }
Spilly 8:c77ab7615b21 393 else
Spilly 8:c77ab7615b21 394 {
Spilly 8:c77ab7615b21 395 xBee.printf("maximum waypoint reached\n");
Spilly 8:c77ab7615b21 396 }
Spilly 8:c77ab7615b21 397 }
Spilly 8:c77ab7615b21 398 else if(adjustMode == 1)
Spilly 8:c77ab7615b21 399 {
Spilly 8:c77ab7615b21 400 float curKc = headingPID.getPParam();
Spilly 8:c77ab7615b21 401 float curTi = headingPID.getIParam();
Spilly 8:c77ab7615b21 402 float curTd = headingPID.getDParam();
Spilly 8:c77ab7615b21 403 curKc = curKc + 0.1f;
Spilly 8:c77ab7615b21 404 headingPID.setTunings(curKc, curTi, curTd);
Spilly 8:c77ab7615b21 405 xBee.printf("Kc set to %f\n", curKc);
Spilly 8:c77ab7615b21 406 }
Spilly 8:c77ab7615b21 407 else if(adjustMode == 2)
Spilly 8:c77ab7615b21 408 {
Spilly 8:c77ab7615b21 409 float curKc = headingPID.getPParam();
Spilly 8:c77ab7615b21 410 float curTi = headingPID.getIParam();
Spilly 8:c77ab7615b21 411 float curTd = headingPID.getDParam();
Spilly 8:c77ab7615b21 412 curTi = curTi + 0.1f;
Spilly 8:c77ab7615b21 413 headingPID.setTunings(curKc, curTi, curTd);
Spilly 8:c77ab7615b21 414 xBee.printf("Ti set to %f\n", curTi);
Spilly 8:c77ab7615b21 415 }
Spilly 8:c77ab7615b21 416 else if(adjustMode == 3)
Spilly 8:c77ab7615b21 417 {
Spilly 8:c77ab7615b21 418 float curKc = headingPID.getPParam();
Spilly 8:c77ab7615b21 419 float curTi = headingPID.getIParam();
Spilly 8:c77ab7615b21 420 float curTd = headingPID.getDParam();
Spilly 8:c77ab7615b21 421 curTd = curTd + 0.1f;
Spilly 8:c77ab7615b21 422 headingPID.setTunings(curKc, curTi, curTd);
Spilly 8:c77ab7615b21 423 xBee.printf("Td set to %f\n", curTd);
Spilly 8:c77ab7615b21 424 }
Spilly 8:c77ab7615b21 425 }
Spilly 8:c77ab7615b21 426 else if(recChar == '-')
Spilly 8:c77ab7615b21 427 {
Spilly 8:c77ab7615b21 428 if(adjustMode == 0)
Spilly 8:c77ab7615b21 429 {
Spilly 8:c77ab7615b21 430 if(wayPtNum != 0)
Spilly 8:c77ab7615b21 431 {
Spilly 8:c77ab7615b21 432 wayPtNum --;
Spilly 8:c77ab7615b21 433 xBee.printf("waypoint increased to %d\n", wayPtNum);
Spilly 8:c77ab7615b21 434 }
Spilly 8:c77ab7615b21 435 else
Spilly 8:c77ab7615b21 436 {
Spilly 8:c77ab7615b21 437 xBee.printf("minimum waypoint reached\n");
Spilly 8:c77ab7615b21 438 }
Spilly 8:c77ab7615b21 439 }
Spilly 8:c77ab7615b21 440 else if(adjustMode == 1)
Spilly 8:c77ab7615b21 441 {
Spilly 8:c77ab7615b21 442 float curKc = headingPID.getPParam();
Spilly 8:c77ab7615b21 443 float curTi = headingPID.getIParam();
Spilly 8:c77ab7615b21 444 float curTd = headingPID.getDParam();
Spilly 8:c77ab7615b21 445 curKc = curKc - 0.1f;
Spilly 8:c77ab7615b21 446 headingPID.setTunings(curKc, curTi, curTd);
Spilly 8:c77ab7615b21 447 xBee.printf("Kc set to %f\n", curKc);
Spilly 8:c77ab7615b21 448 }
Spilly 8:c77ab7615b21 449 else if(adjustMode == 2)
Spilly 8:c77ab7615b21 450 {
Spilly 8:c77ab7615b21 451 float curKc = headingPID.getPParam();
Spilly 8:c77ab7615b21 452 float curTi = headingPID.getIParam();
Spilly 8:c77ab7615b21 453 float curTd = headingPID.getDParam();
Spilly 8:c77ab7615b21 454 curTi = curTi - 0.1f;
Spilly 8:c77ab7615b21 455 headingPID.setTunings(curKc, curTi, curTd);
Spilly 8:c77ab7615b21 456 xBee.printf("Ti set to %f\n", curTi);
Spilly 8:c77ab7615b21 457 }
Spilly 8:c77ab7615b21 458 else if(adjustMode == 3)
Spilly 8:c77ab7615b21 459 {
Spilly 8:c77ab7615b21 460 float curKc = headingPID.getPParam();
Spilly 8:c77ab7615b21 461 float curTi = headingPID.getIParam();
Spilly 8:c77ab7615b21 462 float curTd = headingPID.getDParam();
Spilly 8:c77ab7615b21 463 curTd = curTd - 0.1f;
Spilly 8:c77ab7615b21 464 headingPID.setTunings(curKc, curTi, curTd);
Spilly 8:c77ab7615b21 465 xBee.printf("Td set to %f\n", curTd);
Spilly 8:c77ab7615b21 466 }
Spilly 8:c77ab7615b21 467 }
Spilly 8:c77ab7615b21 468 //change current waypoint number
Spilly 8:c77ab7615b21 469 else if(recChar == 'r')
Spilly 8:c77ab7615b21 470 {
Spilly 8:c77ab7615b21 471 goStop();
Spilly 8:c77ab7615b21 472 //wayPtNum = 0;
Spilly 8:c77ab7615b21 473 //xBee.printf("waypoint count reset\n");
Spilly 8:c77ab7615b21 474 xBee.printf("Please enter desired waypoint (0-9)\t or press r to reset to zero\n");
Spilly 8:c77ab7615b21 475 while(!xBee.readable());
Spilly 8:c77ab7615b21 476 char tempWS[2];
Spilly 8:c77ab7615b21 477 tempWS[0] = xBee.getc();
Spilly 8:c77ab7615b21 478 if(tempWS[0] == 'r')
Spilly 8:c77ab7615b21 479 {
Spilly 8:c77ab7615b21 480 wayPtNum = 0;
Spilly 8:c77ab7615b21 481 }
Spilly 8:c77ab7615b21 482 else
Spilly 8:c77ab7615b21 483 {
Spilly 8:c77ab7615b21 484 sscanf(tempWS, "%d", &wayPtNum);
Spilly 8:c77ab7615b21 485 xBee.printf("waypoint is now %d\n", wayPtNum);
Spilly 8:c77ab7615b21 486 }
Spilly 8:c77ab7615b21 487 }
Spilly 8:c77ab7615b21 488 else if(recChar == 'p')
Spilly 8:c77ab7615b21 489 {
Spilly 8:c77ab7615b21 490 xBee.printf("To set adjust mode:\nEnter w to adjust waypoint number\nEnter c to adjust Kc\nEnter i to adjust Ti\nEnter d to adjust Td\nEnter z to exit\n");
Spilly 8:c77ab7615b21 491 while(!xBee.readable());
Spilly 8:c77ab7615b21 492 char recCharTemp = xBee.getc();
Spilly 8:c77ab7615b21 493 if(recCharTemp == 'w')
Spilly 8:c77ab7615b21 494 {
Spilly 8:c77ab7615b21 495 adjustMode = 0;
Spilly 8:c77ab7615b21 496 }
Spilly 8:c77ab7615b21 497 else if(recCharTemp == 'c')
Spilly 8:c77ab7615b21 498 {
Spilly 8:c77ab7615b21 499 adjustMode = 1;
Spilly 8:c77ab7615b21 500 xBee.printf("Adjust mode set to Kc\tEnter + to increment and - to decrement Kc\n");
Spilly 8:c77ab7615b21 501 }
Spilly 8:c77ab7615b21 502 else if(recCharTemp == 'i')
Spilly 8:c77ab7615b21 503 {
Spilly 8:c77ab7615b21 504 adjustMode = 2;
Spilly 8:c77ab7615b21 505 xBee.printf("Adjust mode set to Ti\tEnter + to increment and - to decrement Ti\n");
Spilly 8:c77ab7615b21 506 }
Spilly 8:c77ab7615b21 507 else if(recCharTemp == 'd')
Spilly 8:c77ab7615b21 508 {
Spilly 8:c77ab7615b21 509 adjustMode = 3;
Spilly 8:c77ab7615b21 510 xBee.printf("Adjust mode set to Td\tEnter + to increment and - to decrement Td\n");
Spilly 8:c77ab7615b21 511 }
Spilly 8:c77ab7615b21 512 else
Spilly 8:c77ab7615b21 513 {
Spilly 8:c77ab7615b21 514 xBee.printf("No changes made\n");
Spilly 8:c77ab7615b21 515 }
Spilly 8:c77ab7615b21 516 }
Spilly 2:503a5ac6c3b6 517 }
Spilly 2:503a5ac6c3b6 518 else
Spilly 0:e79311aae7ed 519 {
Spilly 8:c77ab7615b21 520 if(gps.getData())
Spilly 2:503a5ac6c3b6 521 {
Spilly 8:c77ab7615b21 522 double tempPos[2] = {0,0};
Spilly 5:40ac894e0fa7 523
Spilly 8:c77ab7615b21 524 tempPos[0] = gps.degLat;
Spilly 8:c77ab7615b21 525 tempPos[1] = gps.degLon;
Spilly 8:c77ab7615b21 526 getDist(goalPos[wayPtNum][0],goalPos[wayPtNum][1], tempPos);
Spilly 8:c77ab7615b21 527 getAngle(goalPos[wayPtNum][0],goalPos[wayPtNum][1], tempPos, 0);
Spilly 8:c77ab7615b21 528 //xBee.printf("dist %f\tMagDiff %f\tHDOP = %f\n", polarVector[0], magDiff, gps.HDOP);
Spilly 5:40ac894e0fa7 529
Spilly 5:40ac894e0fa7 530 if(polarVector[0] <= ARRIVED)
Spilly 5:40ac894e0fa7 531 {
Spilly 5:40ac894e0fa7 532 xBee.printf("Goal Position %d reached!\n", wayPtNum);
Spilly 5:40ac894e0fa7 533 wait(1);
Spilly 5:40ac894e0fa7 534 wayPtNum ++;
Spilly 5:40ac894e0fa7 535 if(wayPtNum >= 6)
Spilly 5:40ac894e0fa7 536 {
Spilly 5:40ac894e0fa7 537 xBee.printf("Final Position Reached!\nShutting down\n");
Spilly 8:c77ab7615b21 538 goStop();
Spilly 8:c77ab7615b21 539 mode = 0;
Spilly 8:c77ab7615b21 540 //while(1);
Spilly 8:c77ab7615b21 541 }
Spilly 8:c77ab7615b21 542 else
Spilly 8:c77ab7615b21 543 {
Spilly 8:c77ab7615b21 544 //flush heading PID data since we have a new heading
Spilly 8:c77ab7615b21 545 headingPID.reset();
Spilly 8:c77ab7615b21 546 getAngle(goalPos[wayPtNum ][0],goalPos[wayPtNum][1], goalPos[wayPtNum - 1], 1);
Spilly 8:c77ab7615b21 547 xBee.printf("Moving to Goal Position %d\theading = %f\n", wayPtNum, polarVector[1]);
Spilly 5:40ac894e0fa7 548 }
Spilly 8:c77ab7615b21 549 }
Spilly 8:c77ab7615b21 550 }
Spilly 8:c77ab7615b21 551
Spilly 8:c77ab7615b21 552 if(acc.read() >= ACCEL_PERIOD)
Spilly 8:c77ab7615b21 553 {
Spilly 8:c77ab7615b21 554 getAccel();
Spilly 8:c77ab7615b21 555 acc.reset();
Spilly 8:c77ab7615b21 556 }
Spilly 8:c77ab7615b21 557 //Heading PID
Spilly 8:c77ab7615b21 558 if(magn.read() >= MAGN_PERIOD)
Spilly 8:c77ab7615b21 559 {
Spilly 8:c77ab7615b21 560 getMagn();
Spilly 8:c77ab7615b21 561 updateAngles();
Spilly 8:c77ab7615b21 562 filtered = lowPass(yaw, filtered, 0);
Spilly 8:c77ab7615b21 563 magDiff = whichWay(filtered, 0);
Spilly 8:c77ab7615b21 564 headingPID.setProcessValue(-magDiff);
Spilly 8:c77ab7615b21 565 curSet = headingPID.compute();
Spilly 8:c77ab7615b21 566 xBee.printf("ratio = %f\tcurset = %f\theading = %f\tdiff = %f\n", voltRatio, curSet, filtered, magDiff);
Spilly 8:c77ab7615b21 567 magn.reset();
Spilly 8:c77ab7615b21 568 }
Spilly 8:c77ab7615b21 569
Spilly 8:c77ab7615b21 570 //This moves actuator to the requested position
Spilly 8:c77ab7615b21 571 //This is proportional feedback only
Spilly 8:c77ab7615b21 572 if(loopTimer.read() > RATE)
Spilly 8:c77ab7615b21 573 {
Spilly 8:c77ab7615b21 574 potVoltage = pot.read();
Spilly 8:c77ab7615b21 575 batVoltage = battery.read();
Spilly 8:c77ab7615b21 576 //voltage = voltage * 3.3f;
Spilly 8:c77ab7615b21 577 voltRatio = potVoltage / batVoltage;
Spilly 8:c77ab7615b21 578
Spilly 8:c77ab7615b21 579 float absDiff = sqrt((curSet - voltRatio) * (curSet - voltRatio));
Spilly 8:c77ab7615b21 580 if(absDiff <= RATIO_TOLERANCE)
Spilly 8:c77ab7615b21 581 {
Spilly 8:c77ab7615b21 582 turnStop(1.0f, 1.0f);
Spilly 8:c77ab7615b21 583 xBee.printf("done\n");
Spilly 8:c77ab7615b21 584 }
Spilly 8:c77ab7615b21 585 else if((curSet - voltRatio) >= 0)
Spilly 8:c77ab7615b21 586 {
Spilly 8:c77ab7615b21 587 if(voltRatio > MAX_RATIO)
Spilly 8:c77ab7615b21 588 {
Spilly 8:c77ab7615b21 589 xBee.printf("Max Limit Reached\n");
Spilly 8:c77ab7615b21 590 turnStop(1.0f, 1.0f);
Spilly 8:c77ab7615b21 591 }
Spilly 8:c77ab7615b21 592 else
Spilly 8:c77ab7615b21 593 {
Spilly 8:c77ab7615b21 594 turnRight(1.0f, 1.0f);
Spilly 8:c77ab7615b21 595 xBee.printf("turning Right\n");
Spilly 8:c77ab7615b21 596 }
Spilly 8:c77ab7615b21 597 }
Spilly 8:c77ab7615b21 598 else
Spilly 8:c77ab7615b21 599 {
Spilly 8:c77ab7615b21 600 if(voltRatio < MIN_RATIO)
Spilly 8:c77ab7615b21 601 {
Spilly 8:c77ab7615b21 602 xBee.printf("Min Limit Reached\n");
Spilly 8:c77ab7615b21 603 turnStop(1.0f, 1.0f);
Spilly 8:c77ab7615b21 604 }
Spilly 8:c77ab7615b21 605 else
Spilly 8:c77ab7615b21 606 {
Spilly 8:c77ab7615b21 607 turnLeft(1.0f, 1.0f);
Spilly 8:c77ab7615b21 608 xBee.printf("turning Left\n");
Spilly 8:c77ab7615b21 609 }
Spilly 8:c77ab7615b21 610 }
Spilly 8:c77ab7615b21 611 //xBee.printf("battery = %f\tpot = %f\tratio = %f\tmotorSpeed = %f\tmoveSpeed %f\n", (batVoltage * VOLT_MULT), (potVoltage* VOLT_MULT), voltRatio, motorSpeed, moveSpeed);
Spilly 8:c77ab7615b21 612 loopTimer.reset();
Spilly 2:503a5ac6c3b6 613 }
Spilly 2:503a5ac6c3b6 614 }
Spilly 0:e79311aae7ed 615 }
Spilly 7:ebc76b0f21da 616
Spilly 8:c77ab7615b21 617 /*********************************************************************************************************************************************************************************************/
Spilly 7:ebc76b0f21da 618 // record position mode
Spilly 8:c77ab7615b21 619 /*********************************************************************************************************************************************************************************************/
Spilly 7:ebc76b0f21da 620
Spilly 5:40ac894e0fa7 621 if(mode == 3)
Spilly 0:e79311aae7ed 622 {
Spilly 8:c77ab7615b21 623 goStop();
Spilly 7:ebc76b0f21da 624 xBee.printf("\nPlease enter position number (0-9), or press y to return to manual mode\n");
Spilly 5:40ac894e0fa7 625
Spilly 5:40ac894e0fa7 626 while(!xBee.readable());
Spilly 5:40ac894e0fa7 627 char recChar = xBee.getc();
Spilly 5:40ac894e0fa7 628 recChar = xBee.getc();
Spilly 7:ebc76b0f21da 629
Spilly 5:40ac894e0fa7 630 //return to manual mode
Spilly 5:40ac894e0fa7 631 if(recChar == 'y')
Spilly 2:503a5ac6c3b6 632 {
Spilly 5:40ac894e0fa7 633 mode = 0;
Spilly 2:503a5ac6c3b6 634 }
Spilly 5:40ac894e0fa7 635 else
Spilly 2:503a5ac6c3b6 636 {
Spilly 7:ebc76b0f21da 637 xBee.printf("\nFinding most accurate GPS position\nThis will take a few seconds\n\n");
Spilly 7:ebc76b0f21da 638
Spilly 7:ebc76b0f21da 639 float lowestHDOP = 100;
Spilly 7:ebc76b0f21da 640
Spilly 8:c77ab7615b21 641 //take 50 GPS readings and keep the position with the lowest horizontal dilution of precision (HDOP)
Spilly 8:c77ab7615b21 642 //lower HDOP = less error
Spilly 7:ebc76b0f21da 643 for(int i = 0; i< 50; i++)
Spilly 7:ebc76b0f21da 644 {
Spilly 8:c77ab7615b21 645 //wait for data to be available
Spilly 8:c77ab7615b21 646 //while(!gps._UltimateGps.readable())
Spilly 7:ebc76b0f21da 647 gps.parseData();
Spilly 7:ebc76b0f21da 648
Spilly 7:ebc76b0f21da 649 if(gps.HDOP <= lowestHDOP)
Spilly 7:ebc76b0f21da 650 {
Spilly 7:ebc76b0f21da 651 lowestHDOP = gps.HDOP;
Spilly 7:ebc76b0f21da 652 curPos[0] = gps.degLat;
Spilly 7:ebc76b0f21da 653 curPos[1] = gps.degLon;
Spilly 7:ebc76b0f21da 654 }
Spilly 8:c77ab7615b21 655 xBee.printf("lat %f\tlon %f\thead %f\talt %f\tspd %f\tfix %d\tsat %d\n", gps.degLat, gps.degLon, gps.heading, gps.altitude, gps.speed, gps.fixtype, gps.satellites);
Spilly 8:c77ab7615b21 656 char tempChar = 'n';
Spilly 8:c77ab7615b21 657 while(!gps.getData() && !(tempChar == '1'))
Spilly 8:c77ab7615b21 658 {
Spilly 8:c77ab7615b21 659 if(xBee.readable())
Spilly 8:c77ab7615b21 660 {
Spilly 8:c77ab7615b21 661 tempChar = xBee.getc();
Spilly 8:c77ab7615b21 662 i = 50;
Spilly 8:c77ab7615b21 663 }
Spilly 8:c77ab7615b21 664 }
Spilly 7:ebc76b0f21da 665 }
Spilly 5:40ac894e0fa7 666 if(recChar == '0')
Spilly 5:40ac894e0fa7 667 {
Spilly 5:40ac894e0fa7 668 goalPos[0][0] = curPos[0];
Spilly 5:40ac894e0fa7 669 goalPos[0][1] = curPos[1];
Spilly 8:c77ab7615b21 670 //write new coords to SD card
Spilly 8:c77ab7615b21 671 fr = fopen("/sd/GPS_CORDS.txt", "w+");
Spilly 8:c77ab7615b21 672
Spilly 8:c77ab7615b21 673 for(int x = 0; x<=9; x++)
Spilly 8:c77ab7615b21 674 {
Spilly 8:c77ab7615b21 675 fprintf(fr, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
Spilly 8:c77ab7615b21 676 }
Spilly 8:c77ab7615b21 677 fclose(fr);
Spilly 5:40ac894e0fa7 678 }
Spilly 5:40ac894e0fa7 679 else if(recChar == '1')
Spilly 5:40ac894e0fa7 680 {
Spilly 5:40ac894e0fa7 681 goalPos[1][0] = curPos[0];
Spilly 5:40ac894e0fa7 682 goalPos[1][1] = curPos[1];
Spilly 8:c77ab7615b21 683 //write new coords to SD card
Spilly 8:c77ab7615b21 684 fr = fopen("/sd/GPS_CORDS.txt", "w+");
Spilly 8:c77ab7615b21 685
Spilly 8:c77ab7615b21 686 for(int x = 0; x<=9; x++)
Spilly 8:c77ab7615b21 687 {
Spilly 8:c77ab7615b21 688 fprintf(fr, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
Spilly 8:c77ab7615b21 689 }
Spilly 8:c77ab7615b21 690 fclose(fr);
Spilly 5:40ac894e0fa7 691 }
Spilly 5:40ac894e0fa7 692 else if(recChar == '2')
Spilly 5:40ac894e0fa7 693 {
Spilly 5:40ac894e0fa7 694 goalPos[2][0] = curPos[0];
Spilly 5:40ac894e0fa7 695 goalPos[2][1] = curPos[1];
Spilly 8:c77ab7615b21 696 //write new coords to SD card
Spilly 8:c77ab7615b21 697 fr = fopen("/sd/GPS_CORDS.txt", "w+");
Spilly 8:c77ab7615b21 698
Spilly 8:c77ab7615b21 699 for(int x = 0; x<=9; x++)
Spilly 8:c77ab7615b21 700 {
Spilly 8:c77ab7615b21 701 fprintf(fr, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
Spilly 8:c77ab7615b21 702 }
Spilly 8:c77ab7615b21 703 fclose(fr);
Spilly 5:40ac894e0fa7 704 }
Spilly 5:40ac894e0fa7 705 else if(recChar == '3')
Spilly 2:503a5ac6c3b6 706 {
Spilly 5:40ac894e0fa7 707 goalPos[3][0] = curPos[0];
Spilly 5:40ac894e0fa7 708 goalPos[3][1] = curPos[1];
Spilly 8:c77ab7615b21 709 //write new coords to SD card
Spilly 8:c77ab7615b21 710 fr = fopen("/sd/GPS_CORDS.txt", "w+");
Spilly 8:c77ab7615b21 711
Spilly 8:c77ab7615b21 712 for(int x = 0; x<=9; x++)
Spilly 8:c77ab7615b21 713 {
Spilly 8:c77ab7615b21 714 fprintf(fr, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
Spilly 8:c77ab7615b21 715 }
Spilly 8:c77ab7615b21 716 fclose(fr);
Spilly 5:40ac894e0fa7 717 }
Spilly 5:40ac894e0fa7 718 else if(recChar == '4')
Spilly 5:40ac894e0fa7 719 {
Spilly 5:40ac894e0fa7 720 goalPos[4][0] = curPos[0];
Spilly 5:40ac894e0fa7 721 goalPos[4][1] = curPos[1];
Spilly 8:c77ab7615b21 722 //write new coords to SD card
Spilly 8:c77ab7615b21 723 fr = fopen("/sd/GPS_CORDS.txt", "w+");
Spilly 8:c77ab7615b21 724
Spilly 8:c77ab7615b21 725 for(int x = 0; x<=9; x++)
Spilly 8:c77ab7615b21 726 {
Spilly 8:c77ab7615b21 727 fprintf(fr, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
Spilly 8:c77ab7615b21 728 }
Spilly 8:c77ab7615b21 729 fclose(fr);
Spilly 5:40ac894e0fa7 730 }
Spilly 5:40ac894e0fa7 731 else if(recChar == '5')
Spilly 5:40ac894e0fa7 732 {
Spilly 5:40ac894e0fa7 733 goalPos[5][0] = curPos[0];
Spilly 5:40ac894e0fa7 734 goalPos[5][1] = curPos[1];
Spilly 8:c77ab7615b21 735 //write new coords to SD card
Spilly 8:c77ab7615b21 736 fr = fopen("/sd/GPS_CORDS.txt", "w+");
Spilly 8:c77ab7615b21 737
Spilly 8:c77ab7615b21 738 for(int x = 0; x<=9; x++)
Spilly 8:c77ab7615b21 739 {
Spilly 8:c77ab7615b21 740 fprintf(fr, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
Spilly 8:c77ab7615b21 741 }
Spilly 8:c77ab7615b21 742 fclose(fr);
Spilly 5:40ac894e0fa7 743 }
Spilly 5:40ac894e0fa7 744 else if(recChar == '6')
Spilly 5:40ac894e0fa7 745 {
Spilly 5:40ac894e0fa7 746 goalPos[6][0] = curPos[0];
Spilly 5:40ac894e0fa7 747 goalPos[6][1] = curPos[1];
Spilly 5:40ac894e0fa7 748 }
Spilly 5:40ac894e0fa7 749 else if(recChar == '7')
Spilly 5:40ac894e0fa7 750 {
Spilly 5:40ac894e0fa7 751 goalPos[7][0] = curPos[0];
Spilly 5:40ac894e0fa7 752 goalPos[7][1] = curPos[1];
Spilly 8:c77ab7615b21 753 //write new coords to SD card
Spilly 8:c77ab7615b21 754 fr = fopen("/sd/GPS_CORDS.txt", "w+");
Spilly 8:c77ab7615b21 755
Spilly 8:c77ab7615b21 756 for(int x = 0; x<=9; x++)
Spilly 8:c77ab7615b21 757 {
Spilly 8:c77ab7615b21 758 fprintf(fr, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
Spilly 8:c77ab7615b21 759 }
Spilly 8:c77ab7615b21 760 fclose(fr);
Spilly 2:503a5ac6c3b6 761 }
Spilly 5:40ac894e0fa7 762 else if(recChar == '8')
Spilly 5:40ac894e0fa7 763 {
Spilly 5:40ac894e0fa7 764 goalPos[8][0] = curPos[0];
Spilly 5:40ac894e0fa7 765 goalPos[8][1] = curPos[1];
Spilly 8:c77ab7615b21 766 //write new coords to SD card
Spilly 8:c77ab7615b21 767 fr = fopen("/sd/GPS_CORDS.txt", "w+");
Spilly 8:c77ab7615b21 768
Spilly 8:c77ab7615b21 769 for(int x = 0; x<=9; x++)
Spilly 8:c77ab7615b21 770 {
Spilly 8:c77ab7615b21 771 fprintf(fr, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
Spilly 8:c77ab7615b21 772 }
Spilly 8:c77ab7615b21 773 fclose(fr);
Spilly 5:40ac894e0fa7 774 }
Spilly 5:40ac894e0fa7 775 else if(recChar == '9')
Spilly 2:503a5ac6c3b6 776 {
Spilly 5:40ac894e0fa7 777 goalPos[9][0] = curPos[0];
Spilly 5:40ac894e0fa7 778 goalPos[9][1] = curPos[1];
Spilly 8:c77ab7615b21 779 //write new coords to SD card
Spilly 8:c77ab7615b21 780 fr = fopen("/sd/GPS_CORDS.txt", "w+");
Spilly 8:c77ab7615b21 781
Spilly 8:c77ab7615b21 782 for(int x = 0; x<=9; x++)
Spilly 8:c77ab7615b21 783 {
Spilly 8:c77ab7615b21 784 fprintf(fr, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
Spilly 8:c77ab7615b21 785 }
Spilly 8:c77ab7615b21 786 fclose(fr);
Spilly 2:503a5ac6c3b6 787 }
Spilly 7:ebc76b0f21da 788 xBee.printf("position %c updated\t", recChar);
Spilly 2:503a5ac6c3b6 789 }
Spilly 7:ebc76b0f21da 790 xBee.printf("returning to manual mode\n\n");
Spilly 5:40ac894e0fa7 791 mode = 0;
Spilly 5:40ac894e0fa7 792 }
Spilly 5:40ac894e0fa7 793 }
Spilly 5:40ac894e0fa7 794 }
Spilly 5:40ac894e0fa7 795
Spilly 8:c77ab7615b21 796 /*************************************************************************************************************************************************************************************************/
Spilly 8:c77ab7615b21 797 // create polar vector based on two sets of latitude and longitude
Spilly 8:c77ab7615b21 798 /*************************************************************************************************************************************************************************************************/
Spilly 8:c77ab7615b21 799 //TODO:
Spilly 8:c77ab7615b21 800 //getDist and getAngle need to be optimized
Spilly 8:c77ab7615b21 801 //they were one function but had to be hacked apart
Spilly 7:ebc76b0f21da 802
Spilly 8:c77ab7615b21 803 void getDist(double posZero, double posOne, double curPos[2])
Spilly 5:40ac894e0fa7 804 {
Spilly 7:ebc76b0f21da 805 double arcLength[2];
Spilly 7:ebc76b0f21da 806 double goalPos[2];
Spilly 5:40ac894e0fa7 807 goalPos[0] = posZero;
Spilly 5:40ac894e0fa7 808 goalPos[1] = posOne;
Spilly 5:40ac894e0fa7 809
Spilly 7:ebc76b0f21da 810 /*Note: arc length = radius * angle*/
Spilly 5:40ac894e0fa7 811 //Y
Spilly 5:40ac894e0fa7 812 arcLength[1] = EARTHRADIUS * ((goalPos[0] - curPos[0]) * DEGREETORAD);
Spilly 5:40ac894e0fa7 813 //X
Spilly 5:40ac894e0fa7 814 arcLength[0] = EARTHRADIUS * ((goalPos[1] - curPos[1]) * DEGREETORAD);
Spilly 5:40ac894e0fa7 815
Spilly 5:40ac894e0fa7 816 //calculate magnitude of vector
Spilly 5:40ac894e0fa7 817 polarVector[0] = sqrt((arcLength[0] * arcLength[0]) + (arcLength[1] * arcLength[1]));
Spilly 8:c77ab7615b21 818 }
Spilly 8:c77ab7615b21 819
Spilly 8:c77ab7615b21 820 void getAngle(double posZero, double posOne, double curPos[2], int flush)
Spilly 8:c77ab7615b21 821 {
Spilly 8:c77ab7615b21 822 double tempAngle = 0;
Spilly 8:c77ab7615b21 823 double arcLength[2];
Spilly 8:c77ab7615b21 824 double goalPos[2];
Spilly 8:c77ab7615b21 825 goalPos[0] = posZero;
Spilly 8:c77ab7615b21 826 goalPos[1] = posOne;
Spilly 5:40ac894e0fa7 827
Spilly 8:c77ab7615b21 828 /*Note: arc length = radius * angle*/
Spilly 8:c77ab7615b21 829 //Y
Spilly 8:c77ab7615b21 830 arcLength[1] = EARTHRADIUS * ((goalPos[0] - curPos[0]) * DEGREETORAD);
Spilly 8:c77ab7615b21 831 //X
Spilly 8:c77ab7615b21 832 arcLength[0] = EARTHRADIUS * ((goalPos[1] - curPos[1]) * DEGREETORAD);
Spilly 5:40ac894e0fa7 833
Spilly 8:c77ab7615b21 834 if(flush)
Spilly 8:c77ab7615b21 835 {
Spilly 8:c77ab7615b21 836 //Use arcTan(x/y) b/c we want our heading to be in respect to North (North = 0 degrees, East = 90 deg, etc.)
Spilly 8:c77ab7615b21 837 polarVector[1] = (RADTODEGREE * (atan2(arcLength[0], arcLength[1])));
Spilly 8:c77ab7615b21 838 //make negative angles positive
Spilly 8:c77ab7615b21 839 if(polarVector[1] < 0) polarVector[1] = polarVector[1] + 360;
Spilly 8:c77ab7615b21 840 }
Spilly 8:c77ab7615b21 841 else
Spilly 8:c77ab7615b21 842 {
Spilly 8:c77ab7615b21 843
Spilly 8:c77ab7615b21 844 tempAngle = (RADTODEGREE * (atan2(arcLength[0], arcLength[1])));
Spilly 8:c77ab7615b21 845
Spilly 8:c77ab7615b21 846 if(tempAngle < 0) tempAngle = tempAngle + 360;
Spilly 8:c77ab7615b21 847 polarVector[1] = lowPass(tempAngle, polarVector[1], 3);
Spilly 8:c77ab7615b21 848 }
Spilly 8:c77ab7615b21 849 }