David Spillman / Mbed 2 deprecated GPSNavigationNew

Crude navigation

Dependencies:   GPS2 L3GD20 LSM303DLHC2 PID mbed SDFileSystem

Fork of GPSNavigation by David Spillman

main.cpp@8:c77ab7615b21, 2015-04-27 (annotated)

Committer:
Spilly
Date:
Mon Apr 27 16:49:48 2015 +0000
Revision:
8:c77ab7615b21
Parent:
7:ebc76b0f21da
Child:
9:fb8e34e31dfb
BoatProject

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