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@16:9d1e72ab7ec7, 2015-05-07 (annotated)

Committer:
Spilly
Date:
Thu May 07 16:35:00 2015 +0000
Revision:
16:9d1e72ab7ec7
Parent:
15:1087b8887b53
Child:
17:ba45eaae96b3
Disabled indicator lights

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 13:17f04a55c6e2 4 // Last updated 4/29/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 14:fd20b7ac8de8 18 /**************************************************!!!!!!!WARNING!!!!!!**************************************************************************************************************************/
Spilly 14:fd20b7ac8de8 19 // DO NOT update mbed or SDFileSystem folder
Spilly 14:fd20b7ac8de8 20 // Doing so will/may break this program
Spilly 14:fd20b7ac8de8 21 /**************************************************!!!!!!!WARNING!!!!!!**************************************************************************************************************************/
Spilly 14:fd20b7ac8de8 22 //
Spilly 8:c77ab7615b21 23 /***************************************************How To***************************************************************************************************************************************/
Spilly 8:c77ab7615b21 24 //
Spilly 10:b6bf86de613f 25 // Requires a serial to usb adapter to connect an X-Bee to a PC
Spilly 10:b6bf86de613f 26 // Set both X-Bees up for 115200 baud
Spilly 10:b6bf86de613f 27 // Use TeraTerm (or other serial program) to read and send data over X-Bees
Spilly 12:273479524c71 28 // Set TeraTerm new line character from CR (carrage return) to LF (line feed)
Spilly 8:c77ab7615b21 29 // Program starts by prompting user to press any key
Spilly 8:c77ab7615b21 30 // Once user presses a key, the program waits for a DGPS fix (can be set by changing "FIX")
Spilly 8:c77ab7615b21 31 // Once the program sees a DGPS fix, manual mode is enabled
Spilly 8:c77ab7615b21 32 // User can drive the vehicle in manual mode to any position
Spilly 8:c77ab7615b21 33 // User can record current position to a selected goal position in record mode
Spilly 8:c77ab7615b21 34 // In autonomous mode, the vehicle uses GPS data and compass data to navigate to each goal position
Spilly 8:c77ab7615b21 35 //
Spilly 8:c77ab7615b21 36 // Controls in manual mode:
Spilly 8:c77ab7615b21 37 // directional:
Spilly 8:c77ab7615b21 38 // w = forward
Spilly 8:c77ab7615b21 39 // s = backward
Spilly 8:c77ab7615b21 40 // a = left
Spilly 8:c77ab7615b21 41 // d = right
Spilly 8:c77ab7615b21 42 // mode:
Spilly 8:c77ab7615b21 43 // r = change to record mode
Spilly 8:c77ab7615b21 44 // z = change to autonomous mode
Spilly 8:c77ab7615b21 45 //
Spilly 8:c77ab7615b21 46 // Controls in autonomous mode:
Spilly 8:c77ab7615b21 47 // mode:
Spilly 8:c77ab7615b21 48 // y = change to manual mode
Spilly 8:c77ab7615b21 49 // adjustments:
Spilly 8:c77ab7615b21 50 // d = increase angle
Spilly 8:c77ab7615b21 51 // a = decrease angle
Spilly 8:c77ab7615b21 52 // r = enter new waypoint number
Spilly 8:c77ab7615b21 53 // + = increase (depends on adjust mode)
Spilly 8:c77ab7615b21 54 // - = decrease (depends on adjust mode)
Spilly 8:c77ab7615b21 55 // p = change adjust mode
Spilly 8:c77ab7615b21 56 //
Spilly 8:c77ab7615b21 57 // Controls in record mode:
Spilly 8:c77ab7615b21 58 // *follow serial prompts to record positions
Spilly 8:c77ab7615b21 59 // mode:
Spilly 8:c77ab7615b21 60 // y = change to manual mode
Spilly 8:c77ab7615b21 61 //
Spilly 8:c77ab7615b21 62 /*************************************************************************************************************************************************************************************************/
Spilly 0:e79311aae7ed 63
Spilly 12:273479524c71 64 //unmodified
Spilly 14:fd20b7ac8de8 65 #include "mbed.h" //mbed folder
Spilly 14:fd20b7ac8de8 66 #include "SDFileSystem.h" //SDFileSystem folder
Spilly 12:273479524c71 67
Spilly 12:273479524c71 68 //modified
Spilly 14:fd20b7ac8de8 69 #include "GPS.h" //GPS folder
Spilly 14:fd20b7ac8de8 70 #include "PID.h" //PID folder
Spilly 12:273479524c71 71
Spilly 12:273479524c71 72 //from scratch
Spilly 12:273479524c71 73 #include "IMUDataAndFilters.h"
Spilly 5:40ac894e0fa7 74 #include "navigation.h"
Spilly 8:c77ab7615b21 75 #include "Actuator.h"
Spilly 8:c77ab7615b21 76 #include "TrollingMotor.h"
Spilly 0:e79311aae7ed 77
Spilly 16:9d1e72ab7ec7 78 //#define LIGHT_ON_PERIOD 1.0f
Spilly 16:9d1e72ab7ec7 79 //#define LIGHT_OFF_PERIOD 5.0f
Spilly 14:fd20b7ac8de8 80
Spilly 8:c77ab7615b21 81 #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 82 #define RATIO_TOLERANCE 0.02f //How close the difference between the set ratio and current ratio before consider
Spilly 8:c77ab7615b21 83 #define MIN_RATIO 0.04f //Actuator hits retract limit swithc at 2.2%
Spilly 8:c77ab7615b21 84 #define MAX_RATIO 0.85f //Actuator hits extend limit switch at 87.6%
Spilly 8:c77ab7615b21 85 #define CENTER_RATIO 0.29f //Ratio where prop is centered
Spilly 8:c77ab7615b21 86
Spilly 8:c77ab7615b21 87 #define FIX 0 // 2 = DGPS (more accurate but slower to initialize) 1 = GPS only (less accurate but faster to initialize)
Spilly 8:c77ab7615b21 88 #define ARRIVED 5.0f //Tolerance, in meters, for when goal location is reached
Spilly 8:c77ab7615b21 89 #define GPS_ACCUR 3.0f //accuracy of GPS unit
Spilly 8:c77ab7615b21 90 #define GPS_PERIOD 1.0f //GPS polling period (1 Hz)
Spilly 8:c77ab7615b21 91 #define GPS_POLL 0.5f
Spilly 7:ebc76b0f21da 92 #define GPS_ALPHA 0.3f //GPS low pass alpha
Spilly 8:c77ab7615b21 93
Spilly 11:1b34319671eb 94 #define RATE 0.3f //period of heading PID loop
Spilly 8:c77ab7615b21 95 #define headKc 1.0f //directly proportional
Spilly 8:c77ab7615b21 96 #define headTi 0.0f //a larger number makes the integral have less affect on the output
Spilly 8:c77ab7615b21 97 #define headTd 0.0f //a smaller number makes the derivative have less affect on the output
Spilly 12:273479524c71 98
Spilly 12:273479524c71 99 //PID/PID.cpp
Spilly 8:c77ab7615b21 100 PID headingPID(headKc, headTi, headTd, MAGN_PERIOD); //Kc, Ti, Td, interval
Spilly 8:c77ab7615b21 101
Spilly 14:fd20b7ac8de8 102 //mbed classes (mbed folder)
Spilly 5:40ac894e0fa7 103 Timer headingTime;
Spilly 5:40ac894e0fa7 104 Timer acc;
Spilly 5:40ac894e0fa7 105 Timer magn;
Spilly 5:40ac894e0fa7 106 Timer inputTimer;
Spilly 8:c77ab7615b21 107 Timer loopTimer;
Spilly 16:9d1e72ab7ec7 108 //Timer lightTimer;
Spilly 12:273479524c71 109 AnalogIn battery(A0); //analog input from +12v side of linear actuator potentiometer (uses voltage divider)
Spilly 12:273479524c71 110 AnalogIn pot(A1); //analog input from the wiper of linear actuator potentionmeter (uses voltage divider)
Spilly 12:273479524c71 111 DigitalOut ldo(PTC10); //Controls 3.3V LDO v-reg powering MTK3339 (GPS) 1 = GPS powered on and 0 = GPS powered down
Spilly 16:9d1e72ab7ec7 112 //DigitalOut green(D3); //Light output
Spilly 16:9d1e72ab7ec7 113 //DigitalOut white(PTB9); //Light output
Spilly 14:fd20b7ac8de8 114 Serial xBee(PTB11, PTB10); //UART 3 xbee serial
Spilly 14:fd20b7ac8de8 115 //end of mbed classes
Spilly 8:c77ab7615b21 116
Spilly 12:273479524c71 117 //GPS/GPS.cpp
Spilly 14:fd20b7ac8de8 118 GPS gps(PTC15, PTC14); //UART 4 GPS serial
Spilly 12:273479524c71 119
Spilly 12:273479524c71 120 //Not sure where "FILE" is defined
Spilly 12:273479524c71 121 FILE *way; //file pointer for waypoints on SD card
Spilly 12:273479524c71 122 FILE *data; //file pointer for datalog
Spilly 12:273479524c71 123
Spilly 12:273479524c71 124 //SDFileSystem/SDFileSystem.h
Spilly 12:273479524c71 125 SDFileSystem sd(PTE3, PTE1, PTE2, PTE4, "sd", PTE6, SDFileSystem::SWITCH_POS_NO, 50000000); //On board microSD
Spilly 8:c77ab7615b21 126
Spilly 8:c77ab7615b21 127 /***************************************************Prototype functions****************************************************************************************************************************/
Spilly 8:c77ab7615b21 128 void getDist(double posZero, double posOne, double curPos[2]);
Spilly 8:c77ab7615b21 129 void getAngle(double posZero, double posOne, double curPos[2], int flush);
Spilly 8:c77ab7615b21 130 /***************************************************End of prototype functions*********************************************************************************************************************/
Spilly 8:c77ab7615b21 131
Spilly 8:c77ab7615b21 132 /***************************************************Global Variables*******************************************************************************************************************************/
Spilly 12:273479524c71 133 //TODO: rewrite polar vector functions to allow elimination of global variables
Spilly 2:503a5ac6c3b6 134
Spilly 12:273479524c71 135 double polarVector[2] = {0,0.0000001f}; //poalarVector[0] = magnitude of vector, polarVector[1] = angle in degrees of vector
Spilly 0:e79311aae7ed 136
Spilly 8:c77ab7615b21 137 /*************************************************************************************************************************************************************************************************/
Spilly 7:ebc76b0f21da 138 // MAIN
Spilly 8:c77ab7615b21 139 /*************************************************************************************************************************************************************************************************/
Spilly 0:e79311aae7ed 140
Spilly 0:e79311aae7ed 141 int main()
Spilly 12:273479524c71 142 {
Spilly 16:9d1e72ab7ec7 143 //int wayPtNum = 0, mode = 0, adjustMode = 0, lightCycle = 0;
Spilly 8:c77ab7615b21 144 int wayPtNum = 0, mode = 0, adjustMode = 0;
Spilly 5:40ac894e0fa7 145 float magDiff = 0;
Spilly 8:c77ab7615b21 146 float batVoltage = 0.0f, potVoltage = 0.0f, voltRatio = 0.0f;
Spilly 8:c77ab7615b21 147 float curSet = 0.0f, prevSet = 0.29f;
Spilly 8:c77ab7615b21 148 float filtered = 0.0000001f;
Spilly 12:273479524c71 149 double curPos[2] = {0,0};
Spilly 12:273479524c71 150 double goalPos[10][2]; //positions are initially read from SD card
Spilly 12:273479524c71 151 //when a position is recorded in record mode, the previously stored position on the SD card is overwritten with the new position
Spilly 12:273479524c71 152
Spilly 12:273479524c71 153 //set the initial state of the lights
Spilly 16:9d1e72ab7ec7 154 //green = 1;
Spilly 16:9d1e72ab7ec7 155 //white = 0;
Spilly 0:e79311aae7ed 156
Spilly 12:273479524c71 157 //turn the GPS 3.3V LDO v-reg on
Spilly 12:273479524c71 158 ldo = 1;
Spilly 12:273479524c71 159 //wait for the GPS unit to boot
Spilly 12:273479524c71 160 wait(1);
Spilly 12:273479524c71 161
Spilly 12:273479524c71 162 //set xBee baud rate
Spilly 5:40ac894e0fa7 163 xBee.baud(115200);
Spilly 0:e79311aae7ed 164 xBee.printf("\nI'm Alive...\n");
Spilly 5:40ac894e0fa7 165 xBee.printf("Press any key to begin\n");
Spilly 5:40ac894e0fa7 166
Spilly 8:c77ab7615b21 167 //wait for keypress to begin
Spilly 5:40ac894e0fa7 168 while(!xBee.readable());
Spilly 5:40ac894e0fa7 169
Spilly 12:273479524c71 170 //Get goal positions from SD card
Spilly 8:c77ab7615b21 171 //start of SD card read
Spilly 12:273479524c71 172 way = fopen ("/sd/GPS_CORDS.txt", "r");
Spilly 8:c77ab7615b21 173
Spilly 8:c77ab7615b21 174 xBee.printf("Reading SD Card Please Wait\n");
Spilly 8:c77ab7615b21 175
Spilly 8:c77ab7615b21 176 for(int x = 0; x<=9; x++)
Spilly 8:c77ab7615b21 177 {
Spilly 12:273479524c71 178 fscanf(way, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
Spilly 8:c77ab7615b21 179 xBee.printf("waypoint %d = %f,%f\n", x, goalPos[x][0], goalPos[x][1]);
Spilly 8:c77ab7615b21 180 }
Spilly 12:273479524c71 181 fclose(way);
Spilly 8:c77ab7615b21 182 //end of SD card read
Spilly 8:c77ab7615b21 183
Spilly 5:40ac894e0fa7 184 //initialize magnetometer, accelerometer
Spilly 5:40ac894e0fa7 185 compass.init();
Spilly 7:ebc76b0f21da 186 wait(1);
Spilly 0:e79311aae7ed 187 //Setup the GPS
Spilly 0:e79311aae7ed 188 gps.Init();
Spilly 5:40ac894e0fa7 189
Spilly 2:503a5ac6c3b6 190 xBee.printf("gps initialized\n");
Spilly 0:e79311aae7ed 191
Spilly 2:503a5ac6c3b6 192 xBee.printf("attempting to get a fix\n");
Spilly 8:c77ab7615b21 193
Spilly 8:c77ab7615b21 194 while(gps.fixtype != FIX)
Spilly 0:e79311aae7ed 195 {
Spilly 8:c77ab7615b21 196 while(!gps.getData());
Spilly 8:c77ab7615b21 197 if(gps.fixtype == 1)
Spilly 5:40ac894e0fa7 198 {
Spilly 8:c77ab7615b21 199 xBee.printf("Waiting for DGPS fix\tcurrent fix = GPS only\n");
Spilly 8:c77ab7615b21 200 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 201 }
Spilly 8:c77ab7615b21 202 else xBee.printf("Waiting for DGPS fix\tcurrent fix = no fix\n");
Spilly 0:e79311aae7ed 203 }
Spilly 12:273479524c71 204
Spilly 5:40ac894e0fa7 205 //get IMU data and calculate the tilt compensated compass
Spilly 12:273479524c71 206 getAccel(); //IMUDataAndFilters.h
Spilly 12:273479524c71 207 getMagn(); //IMUDataAndFilters.h
Spilly 12:273479524c71 208 updateAngles(); //IMUDataAndFilters.h
Spilly 5:40ac894e0fa7 209
Spilly 7:ebc76b0f21da 210 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 211 xBee.printf("dist %f\theading %f\n", polarVector[0], polarVector[1]);
Spilly 7:ebc76b0f21da 212 xBee.printf("\n\nstarting main loop\n");
Spilly 5:40ac894e0fa7 213
Spilly 12:273479524c71 214 //set input constraints (heading difference)
Spilly 5:40ac894e0fa7 215 headingPID.setInputLimits(-180, 180);
Spilly 8:c77ab7615b21 216
Spilly 8:c77ab7615b21 217 //set proportional output limits based on physical limits of actuator and mounting error
Spilly 12:273479524c71 218 float distFromCenter = calcEnds(CENTER_RATIO, MAX_RATIO, MIN_RATIO); //navigation.h
Spilly 8:c77ab7615b21 219
Spilly 12:273479524c71 220 //set output constraints (linear actuator max and min)
Spilly 8:c77ab7615b21 221 headingPID.setOutputLimits((CENTER_RATIO - distFromCenter), (CENTER_RATIO + distFromCenter));
Spilly 5:40ac894e0fa7 222 //set mode to auto
Spilly 5:40ac894e0fa7 223 headingPID.setMode(0);
Spilly 12:273479524c71 224 //We want the difference between actual heading and the heading needed to be zero
Spilly 5:40ac894e0fa7 225 headingPID.setSetPoint(0);
Spilly 0:e79311aae7ed 226
Spilly 12:273479524c71 227 //start timers
Spilly 8:c77ab7615b21 228 loopTimer.start();
Spilly 5:40ac894e0fa7 229 headingTime.start();
Spilly 5:40ac894e0fa7 230 acc.start();
Spilly 5:40ac894e0fa7 231 magn.start();
Spilly 16:9d1e72ab7ec7 232 //lightTimer.start();
Spilly 0:e79311aae7ed 233 while (1)
Spilly 0:e79311aae7ed 234 {
Spilly 8:c77ab7615b21 235 /*********************************************************************************************************************************************************************************************/
Spilly 7:ebc76b0f21da 236 // manual mode
Spilly 8:c77ab7615b21 237 /*********************************************************************************************************************************************************************************************/
Spilly 7:ebc76b0f21da 238
Spilly 5:40ac894e0fa7 239 if(mode == 0)
Spilly 2:503a5ac6c3b6 240 {
Spilly 12:273479524c71 241 //checks to see if all three NEMA sentences from the GPS UART has been received
Spilly 12:273479524c71 242 gps.getData();
Spilly 12:273479524c71 243
Spilly 12:273479524c71 244 //check timer
Spilly 8:c77ab7615b21 245 if(loopTimer.read() > RATE)
Spilly 5:40ac894e0fa7 246 {
Spilly 12:273479524c71 247 //TODO: put this in a function
Spilly 12:273479524c71 248
Spilly 12:273479524c71 249 //This moves actuator to the requested position
Spilly 12:273479524c71 250 //This is proportional feedback ONLY
Spilly 12:273479524c71 251
Spilly 12:273479524c71 252 //read analog input from wiper on potentiometer on linear actuator
Spilly 8:c77ab7615b21 253 potVoltage = pot.read();
Spilly 12:273479524c71 254 //read analog input from battery voltage
Spilly 8:c77ab7615b21 255 batVoltage = battery.read();
Spilly 12:273479524c71 256 //calculate ratio of the two (using a ratio prevents battery voltage fluctuation from affecting actual position)
Spilly 8:c77ab7615b21 257 voltRatio = potVoltage / batVoltage;
Spilly 8:c77ab7615b21 258
Spilly 12:273479524c71 259 //calculate the absolute value of how far off from requested actuator position we are (saves a few processor cycles)
Spilly 8:c77ab7615b21 260 float absDiff = sqrt((prevSet - voltRatio) * (prevSet - voltRatio));
Spilly 12:273479524c71 261 //are we off enough to care? if so, stop moving the actuator
Spilly 8:c77ab7615b21 262 if(absDiff <= RATIO_TOLERANCE)
Spilly 8:c77ab7615b21 263 {
Spilly 8:c77ab7615b21 264 turnStop(1.0f, 1.0f);
Spilly 8:c77ab7615b21 265 //xBee.printf("done\n");
Spilly 8:c77ab7615b21 266 }
Spilly 12:273479524c71 267
Spilly 12:273479524c71 268 //do we need to go further right?
Spilly 8:c77ab7615b21 269 else if((prevSet - voltRatio) >= 0)
Spilly 8:c77ab7615b21 270 {
Spilly 8:c77ab7615b21 271 turnRight(1.0f, 1.0f);
Spilly 8:c77ab7615b21 272 //xBee.printf("turning right\n");
Spilly 8:c77ab7615b21 273 }
Spilly 12:273479524c71 274
Spilly 12:273479524c71 275 //do we need to go further left?
Spilly 8:c77ab7615b21 276 else
Spilly 8:c77ab7615b21 277 {
Spilly 8:c77ab7615b21 278 turnLeft(1.0f, 1.0f);
Spilly 8:c77ab7615b21 279 //xBee.printf("turning left\n");
Spilly 8:c77ab7615b21 280 }
Spilly 12:273479524c71 281 xBee.printf("battery = %f\tpot = %f\tratio = %f\tset %f\tHDOP = %f\tfix = %d\n", (batVoltage * VOLT_MULT), (potVoltage* VOLT_MULT), voltRatio, prevSet, gps.HDOP, gps.fixtype);
Spilly 12:273479524c71 282
Spilly 12:273479524c71 283 //reset timer to zero
Spilly 8:c77ab7615b21 284 loopTimer.reset();
Spilly 5:40ac894e0fa7 285 }
Spilly 12:273479524c71 286
Spilly 7:ebc76b0f21da 287 //check to see if data is available on xBee
Spilly 5:40ac894e0fa7 288 if(xBee.readable())
Spilly 2:503a5ac6c3b6 289 {
Spilly 12:273479524c71 290
Spilly 12:273479524c71 291 //TODO: put this in a function
Spilly 12:273479524c71 292
Spilly 5:40ac894e0fa7 293 char recChar = xBee.getc();
Spilly 5:40ac894e0fa7 294
Spilly 5:40ac894e0fa7 295 //change to autonomous mode
Spilly 5:40ac894e0fa7 296 if(recChar == 'z')
Spilly 5:40ac894e0fa7 297 {
Spilly 8:c77ab7615b21 298 xBee.printf("Changing to autonomous mode\n");
Spilly 8:c77ab7615b21 299 goStop();
Spilly 5:40ac894e0fa7 300 mode = 1;
Spilly 5:40ac894e0fa7 301 }
Spilly 5:40ac894e0fa7 302 //change to record mode
Spilly 8:c77ab7615b21 303 else if(recChar == 'r')
Spilly 5:40ac894e0fa7 304 {
Spilly 8:c77ab7615b21 305 xBee.printf("Changing to record mode\n");
Spilly 8:c77ab7615b21 306 goStop();
Spilly 5:40ac894e0fa7 307 mode = 3;
Spilly 5:40ac894e0fa7 308 }
Spilly 12:273479524c71 309 else if(recChar == '<')
Spilly 12:273479524c71 310 {
Spilly 12:273479524c71 311 xBee.printf("Power cycling GPS\nPlease wait\n");
Spilly 12:273479524c71 312 goStop();
Spilly 12:273479524c71 313 ldo = 0;
Spilly 12:273479524c71 314 wait(0.5);
Spilly 12:273479524c71 315 ldo = 1;
Spilly 12:273479524c71 316 wait(0.5);
Spilly 12:273479524c71 317 }
Spilly 8:c77ab7615b21 318 else if(recChar == '1')
Spilly 8:c77ab7615b21 319 {
Spilly 8:c77ab7615b21 320 xBee.printf("stop\n");
Spilly 8:c77ab7615b21 321 goStop();
Spilly 8:c77ab7615b21 322 }
Spilly 8:c77ab7615b21 323 else if(recChar == 'w')
Spilly 5:40ac894e0fa7 324 {
Spilly 8:c77ab7615b21 325 goForward();
Spilly 8:c77ab7615b21 326 prevSet = CENTER_RATIO;
Spilly 8:c77ab7615b21 327 xBee.printf("Forward\n");
Spilly 8:c77ab7615b21 328 }
Spilly 8:c77ab7615b21 329 else if(recChar == 's')
Spilly 8:c77ab7615b21 330 {
Spilly 8:c77ab7615b21 331 goBackward();
Spilly 8:c77ab7615b21 332 prevSet = CENTER_RATIO;
Spilly 8:c77ab7615b21 333 xBee.printf("backward\n");
Spilly 8:c77ab7615b21 334 }
Spilly 8:c77ab7615b21 335 else if(recChar == 'd')
Spilly 8:c77ab7615b21 336 {
Spilly 8:c77ab7615b21 337 xBee.printf("large step right\n");
Spilly 5:40ac894e0fa7 338
Spilly 8:c77ab7615b21 339 //find the best step size since 0.1 step will go over the limit
Spilly 8:c77ab7615b21 340 if(prevSet + 0.1f > MAX_RATIO)
Spilly 8:c77ab7615b21 341 {
Spilly 8:c77ab7615b21 342 prevSet = prevSet + (MAX_RATIO - prevSet);
Spilly 8:c77ab7615b21 343 }
Spilly 8:c77ab7615b21 344 else
Spilly 8:c77ab7615b21 345 {
Spilly 8:c77ab7615b21 346 prevSet = prevSet + 0.1f;
Spilly 8:c77ab7615b21 347 }
Spilly 8:c77ab7615b21 348 xBee.printf("set = %f\n", prevSet);
Spilly 5:40ac894e0fa7 349 }
Spilly 8:c77ab7615b21 350 //large step left
Spilly 8:c77ab7615b21 351 else if(recChar == 'a')
Spilly 5:40ac894e0fa7 352 {
Spilly 8:c77ab7615b21 353 xBee.printf("large step left\n");
Spilly 8:c77ab7615b21 354 //find the best step size since 0.1 step will go over the limit
Spilly 8:c77ab7615b21 355 if(prevSet - 0.1f < MIN_RATIO)
Spilly 8:c77ab7615b21 356 {
Spilly 8:c77ab7615b21 357 prevSet = prevSet - (prevSet - MIN_RATIO);
Spilly 8:c77ab7615b21 358 }
Spilly 8:c77ab7615b21 359 else
Spilly 8:c77ab7615b21 360 {
Spilly 8:c77ab7615b21 361 prevSet = prevSet - 0.1f;
Spilly 8:c77ab7615b21 362 }
Spilly 8:c77ab7615b21 363 xBee.printf("set = %f\n", prevSet);
Spilly 5:40ac894e0fa7 364 }
Spilly 8:c77ab7615b21 365 //small step right
Spilly 8:c77ab7615b21 366 else if(recChar == 'e')
Spilly 5:40ac894e0fa7 367 {
Spilly 8:c77ab7615b21 368 xBee.printf("small step right\n");
Spilly 8:c77ab7615b21 369 if(prevSet + 0.01f > MAX_RATIO)
Spilly 8:c77ab7615b21 370 {
Spilly 8:c77ab7615b21 371 prevSet = prevSet + (MAX_RATIO - prevSet);
Spilly 8:c77ab7615b21 372 }
Spilly 8:c77ab7615b21 373 else
Spilly 8:c77ab7615b21 374 {
Spilly 8:c77ab7615b21 375 prevSet = prevSet + 0.01f;
Spilly 8:c77ab7615b21 376 }
Spilly 8:c77ab7615b21 377 xBee.printf("set = %f\n", prevSet);
Spilly 8:c77ab7615b21 378 }
Spilly 8:c77ab7615b21 379 else if(recChar == 'q')
Spilly 8:c77ab7615b21 380 {
Spilly 8:c77ab7615b21 381 xBee.printf("Small step left\n");
Spilly 8:c77ab7615b21 382 if(prevSet - 0.01f < MIN_RATIO)
Spilly 8:c77ab7615b21 383 {
Spilly 8:c77ab7615b21 384 prevSet = prevSet - (prevSet - MIN_RATIO);
Spilly 8:c77ab7615b21 385 }
Spilly 8:c77ab7615b21 386 else
Spilly 8:c77ab7615b21 387 {
Spilly 8:c77ab7615b21 388 prevSet = prevSet - 0.01f;
Spilly 8:c77ab7615b21 389 }
Spilly 8:c77ab7615b21 390 xBee.printf("set = %f\n", prevSet);
Spilly 5:40ac894e0fa7 391 }
Spilly 2:503a5ac6c3b6 392 }
Spilly 16:9d1e72ab7ec7 393 /*
Spilly 16:9d1e72ab7ec7 394 if(lightCycle == 0)
Spilly 16:9d1e72ab7ec7 395 {
Spilly 16:9d1e72ab7ec7 396 if(lightTimer.read() >= LIGHT_OFF_PERIOD)
Spilly 16:9d1e72ab7ec7 397 {
Spilly 16:9d1e72ab7ec7 398 green = 0;
Spilly 16:9d1e72ab7ec7 399 white = 0;
Spilly 16:9d1e72ab7ec7 400 lightCycle = 1;
Spilly 16:9d1e72ab7ec7 401 lightTimer.reset();
Spilly 16:9d1e72ab7ec7 402 }
Spilly 16:9d1e72ab7ec7 403 }
Spilly 16:9d1e72ab7ec7 404 else if(lightCycle == 1)
Spilly 16:9d1e72ab7ec7 405 {
Spilly 16:9d1e72ab7ec7 406 if(lightTimer.read() >= LIGHT_ON_PERIOD)
Spilly 16:9d1e72ab7ec7 407 {
Spilly 16:9d1e72ab7ec7 408 green = 1;
Spilly 16:9d1e72ab7ec7 409 white = 1;
Spilly 16:9d1e72ab7ec7 410 lightCycle = 0;
Spilly 16:9d1e72ab7ec7 411 lightTimer.reset();
Spilly 16:9d1e72ab7ec7 412 }
Spilly 16:9d1e72ab7ec7 413 }
Spilly 16:9d1e72ab7ec7 414 */
Spilly 2:503a5ac6c3b6 415 }
Spilly 7:ebc76b0f21da 416
Spilly 8:c77ab7615b21 417 /*********************************************************************************************************************************************************************************************/
Spilly 8:c77ab7615b21 418 // autonomous mode
Spilly 8:c77ab7615b21 419 /*********************************************************************************************************************************************************************************************/
Spilly 12:273479524c71 420 //default trolling motor state when entering autonomous mode is off (user must press "w" to go forward)
Spilly 5:40ac894e0fa7 421 if(mode == 1)
Spilly 2:503a5ac6c3b6 422 {
Spilly 7:ebc76b0f21da 423 //check xBee
Spilly 5:40ac894e0fa7 424 if(xBee.readable())
Spilly 2:503a5ac6c3b6 425 {
Spilly 12:273479524c71 426 //TODO: put this in a function
Spilly 12:273479524c71 427
Spilly 5:40ac894e0fa7 428 char recChar = xBee.getc();
Spilly 12:273479524c71 429 //stop
Spilly 8:c77ab7615b21 430 if(recChar == '1')
Spilly 2:503a5ac6c3b6 431 {
Spilly 8:c77ab7615b21 432 xBee.printf("stop\n");
Spilly 8:c77ab7615b21 433 goStop();
Spilly 8:c77ab7615b21 434 }
Spilly 12:273479524c71 435 //go forward
Spilly 8:c77ab7615b21 436 if(recChar == 'w')
Spilly 8:c77ab7615b21 437 {
Spilly 8:c77ab7615b21 438 xBee.printf("forward\n");
Spilly 8:c77ab7615b21 439 goForward();
Spilly 2:503a5ac6c3b6 440 }
Spilly 5:40ac894e0fa7 441 //change to manual mode
Spilly 5:40ac894e0fa7 442 if(recChar == 'y')
Spilly 2:503a5ac6c3b6 443 {
Spilly 8:c77ab7615b21 444 xBee.printf("Changing to manual mode\n");
Spilly 8:c77ab7615b21 445 goStop();
Spilly 5:40ac894e0fa7 446 mode = 0;
Spilly 7:ebc76b0f21da 447 wayPtNum = 0;
Spilly 2:503a5ac6c3b6 448 }
Spilly 8:c77ab7615b21 449 //increase calculated heading (use this to tweak/cheat calculated heading)
Spilly 8:c77ab7615b21 450 else if(recChar == 'd')
Spilly 8:c77ab7615b21 451 {
Spilly 8:c77ab7615b21 452 polarVector[1] = polarVector[1] + 1;
Spilly 8:c77ab7615b21 453 xBee.printf("increased angle %f\n", polarVector[1]);
Spilly 8:c77ab7615b21 454 }
Spilly 8:c77ab7615b21 455 //reduce calculated heading (use this to tweak/cheat calculated heading)
Spilly 8:c77ab7615b21 456 else if(recChar == 'a')
Spilly 8:c77ab7615b21 457 {
Spilly 8:c77ab7615b21 458 polarVector[1] = polarVector[1] - 1;
Spilly 8:c77ab7615b21 459 xBee.printf("reduced angle %f\n", polarVector[1]);
Spilly 8:c77ab7615b21 460 }
Spilly 12:273479524c71 461
Spilly 12:273479524c71 462 //increments settings based on adjust mode
Spilly 8:c77ab7615b21 463 else if(recChar == '+')
Spilly 8:c77ab7615b21 464 {
Spilly 12:273479524c71 465 //adjust waypoint
Spilly 8:c77ab7615b21 466 if(adjustMode == 0)
Spilly 8:c77ab7615b21 467 {
Spilly 8:c77ab7615b21 468 if(wayPtNum != 9)
Spilly 8:c77ab7615b21 469 {
Spilly 8:c77ab7615b21 470 wayPtNum ++;
Spilly 8:c77ab7615b21 471 xBee.printf("waypoint increased to %d\n", wayPtNum);
Spilly 8:c77ab7615b21 472 }
Spilly 8:c77ab7615b21 473 else
Spilly 8:c77ab7615b21 474 {
Spilly 8:c77ab7615b21 475 xBee.printf("maximum waypoint reached\n");
Spilly 8:c77ab7615b21 476 }
Spilly 8:c77ab7615b21 477 }
Spilly 12:273479524c71 478 //increment proportional gain of heading PID
Spilly 8:c77ab7615b21 479 else if(adjustMode == 1)
Spilly 8:c77ab7615b21 480 {
Spilly 8:c77ab7615b21 481 float curKc = headingPID.getPParam();
Spilly 8:c77ab7615b21 482 float curTi = headingPID.getIParam();
Spilly 8:c77ab7615b21 483 float curTd = headingPID.getDParam();
Spilly 8:c77ab7615b21 484 curKc = curKc + 0.1f;
Spilly 8:c77ab7615b21 485 headingPID.setTunings(curKc, curTi, curTd);
Spilly 8:c77ab7615b21 486 xBee.printf("Kc set to %f\n", curKc);
Spilly 8:c77ab7615b21 487 }
Spilly 12:273479524c71 488 //increment integral gain of heading PID
Spilly 8:c77ab7615b21 489 else if(adjustMode == 2)
Spilly 8:c77ab7615b21 490 {
Spilly 8:c77ab7615b21 491 float curKc = headingPID.getPParam();
Spilly 8:c77ab7615b21 492 float curTi = headingPID.getIParam();
Spilly 8:c77ab7615b21 493 float curTd = headingPID.getDParam();
Spilly 8:c77ab7615b21 494 curTi = curTi + 0.1f;
Spilly 8:c77ab7615b21 495 headingPID.setTunings(curKc, curTi, curTd);
Spilly 8:c77ab7615b21 496 xBee.printf("Ti set to %f\n", curTi);
Spilly 8:c77ab7615b21 497 }
Spilly 12:273479524c71 498 //increment derivative gain of heading PID
Spilly 8:c77ab7615b21 499 else if(adjustMode == 3)
Spilly 8:c77ab7615b21 500 {
Spilly 8:c77ab7615b21 501 float curKc = headingPID.getPParam();
Spilly 8:c77ab7615b21 502 float curTi = headingPID.getIParam();
Spilly 8:c77ab7615b21 503 float curTd = headingPID.getDParam();
Spilly 8:c77ab7615b21 504 curTd = curTd + 0.1f;
Spilly 8:c77ab7615b21 505 headingPID.setTunings(curKc, curTi, curTd);
Spilly 8:c77ab7615b21 506 xBee.printf("Td set to %f\n", curTd);
Spilly 8:c77ab7615b21 507 }
Spilly 8:c77ab7615b21 508 }
Spilly 12:273479524c71 509 //decrements setting based on adjust mode
Spilly 8:c77ab7615b21 510 else if(recChar == '-')
Spilly 8:c77ab7615b21 511 {
Spilly 8:c77ab7615b21 512 if(adjustMode == 0)
Spilly 8:c77ab7615b21 513 {
Spilly 8:c77ab7615b21 514 if(wayPtNum != 0)
Spilly 8:c77ab7615b21 515 {
Spilly 8:c77ab7615b21 516 wayPtNum --;
Spilly 8:c77ab7615b21 517 xBee.printf("waypoint increased to %d\n", wayPtNum);
Spilly 8:c77ab7615b21 518 }
Spilly 8:c77ab7615b21 519 else
Spilly 8:c77ab7615b21 520 {
Spilly 8:c77ab7615b21 521 xBee.printf("minimum waypoint reached\n");
Spilly 8:c77ab7615b21 522 }
Spilly 8:c77ab7615b21 523 }
Spilly 12:273479524c71 524 //decrement proportional gain of heading PID
Spilly 8:c77ab7615b21 525 else if(adjustMode == 1)
Spilly 8:c77ab7615b21 526 {
Spilly 8:c77ab7615b21 527 float curKc = headingPID.getPParam();
Spilly 8:c77ab7615b21 528 float curTi = headingPID.getIParam();
Spilly 8:c77ab7615b21 529 float curTd = headingPID.getDParam();
Spilly 8:c77ab7615b21 530 curKc = curKc - 0.1f;
Spilly 8:c77ab7615b21 531 headingPID.setTunings(curKc, curTi, curTd);
Spilly 8:c77ab7615b21 532 xBee.printf("Kc set to %f\n", curKc);
Spilly 8:c77ab7615b21 533 }
Spilly 12:273479524c71 534 //decrement integral gain of heading PID
Spilly 8:c77ab7615b21 535 else if(adjustMode == 2)
Spilly 8:c77ab7615b21 536 {
Spilly 8:c77ab7615b21 537 float curKc = headingPID.getPParam();
Spilly 8:c77ab7615b21 538 float curTi = headingPID.getIParam();
Spilly 8:c77ab7615b21 539 float curTd = headingPID.getDParam();
Spilly 8:c77ab7615b21 540 curTi = curTi - 0.1f;
Spilly 8:c77ab7615b21 541 headingPID.setTunings(curKc, curTi, curTd);
Spilly 8:c77ab7615b21 542 xBee.printf("Ti set to %f\n", curTi);
Spilly 8:c77ab7615b21 543 }
Spilly 12:273479524c71 544 //decrement derivative gain of heading PID
Spilly 8:c77ab7615b21 545 else if(adjustMode == 3)
Spilly 8:c77ab7615b21 546 {
Spilly 8:c77ab7615b21 547 float curKc = headingPID.getPParam();
Spilly 8:c77ab7615b21 548 float curTi = headingPID.getIParam();
Spilly 8:c77ab7615b21 549 float curTd = headingPID.getDParam();
Spilly 8:c77ab7615b21 550 curTd = curTd - 0.1f;
Spilly 8:c77ab7615b21 551 headingPID.setTunings(curKc, curTi, curTd);
Spilly 8:c77ab7615b21 552 xBee.printf("Td set to %f\n", curTd);
Spilly 8:c77ab7615b21 553 }
Spilly 8:c77ab7615b21 554 }
Spilly 12:273479524c71 555 //change or reset current waypoint number
Spilly 8:c77ab7615b21 556 else if(recChar == 'r')
Spilly 8:c77ab7615b21 557 {
Spilly 8:c77ab7615b21 558 goStop();
Spilly 8:c77ab7615b21 559 //wayPtNum = 0;
Spilly 8:c77ab7615b21 560 //xBee.printf("waypoint count reset\n");
Spilly 8:c77ab7615b21 561 xBee.printf("Please enter desired waypoint (0-9)\t or press r to reset to zero\n");
Spilly 8:c77ab7615b21 562 while(!xBee.readable());
Spilly 8:c77ab7615b21 563 char tempWS[2];
Spilly 8:c77ab7615b21 564 tempWS[0] = xBee.getc();
Spilly 12:273479524c71 565
Spilly 12:273479524c71 566 //press "r" again to reset waypoint number to zero
Spilly 8:c77ab7615b21 567 if(tempWS[0] == 'r')
Spilly 8:c77ab7615b21 568 {
Spilly 8:c77ab7615b21 569 wayPtNum = 0;
Spilly 8:c77ab7615b21 570 }
Spilly 12:273479524c71 571
Spilly 12:273479524c71 572 //else enter the number of waypoint desired
Spilly 8:c77ab7615b21 573 else
Spilly 8:c77ab7615b21 574 {
Spilly 8:c77ab7615b21 575 sscanf(tempWS, "%d", &wayPtNum);
Spilly 8:c77ab7615b21 576 xBee.printf("waypoint is now %d\n", wayPtNum);
Spilly 8:c77ab7615b21 577 }
Spilly 8:c77ab7615b21 578 }
Spilly 12:273479524c71 579 //change adjust mode
Spilly 8:c77ab7615b21 580 else if(recChar == 'p')
Spilly 8:c77ab7615b21 581 {
Spilly 8:c77ab7615b21 582 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 583 while(!xBee.readable());
Spilly 8:c77ab7615b21 584 char recCharTemp = xBee.getc();
Spilly 12:273479524c71 585
Spilly 12:273479524c71 586 //set adjust to edit waypoints
Spilly 8:c77ab7615b21 587 if(recCharTemp == 'w')
Spilly 8:c77ab7615b21 588 {
Spilly 8:c77ab7615b21 589 adjustMode = 0;
Spilly 8:c77ab7615b21 590 }
Spilly 12:273479524c71 591
Spilly 12:273479524c71 592 //set adjust to edit proportional gain
Spilly 8:c77ab7615b21 593 else if(recCharTemp == 'c')
Spilly 8:c77ab7615b21 594 {
Spilly 8:c77ab7615b21 595 adjustMode = 1;
Spilly 8:c77ab7615b21 596 xBee.printf("Adjust mode set to Kc\tEnter + to increment and - to decrement Kc\n");
Spilly 8:c77ab7615b21 597 }
Spilly 12:273479524c71 598
Spilly 12:273479524c71 599 //set adjust to edit integral gain
Spilly 8:c77ab7615b21 600 else if(recCharTemp == 'i')
Spilly 8:c77ab7615b21 601 {
Spilly 8:c77ab7615b21 602 adjustMode = 2;
Spilly 8:c77ab7615b21 603 xBee.printf("Adjust mode set to Ti\tEnter + to increment and - to decrement Ti\n");
Spilly 8:c77ab7615b21 604 }
Spilly 12:273479524c71 605
Spilly 12:273479524c71 606 //set adjust to edit derivative gain
Spilly 8:c77ab7615b21 607 else if(recCharTemp == 'd')
Spilly 8:c77ab7615b21 608 {
Spilly 8:c77ab7615b21 609 adjustMode = 3;
Spilly 8:c77ab7615b21 610 xBee.printf("Adjust mode set to Td\tEnter + to increment and - to decrement Td\n");
Spilly 8:c77ab7615b21 611 }
Spilly 12:273479524c71 612 //if any other key is pressed no change to adjust mode is made
Spilly 8:c77ab7615b21 613 else
Spilly 8:c77ab7615b21 614 {
Spilly 8:c77ab7615b21 615 xBee.printf("No changes made\n");
Spilly 8:c77ab7615b21 616 }
Spilly 8:c77ab7615b21 617 }
Spilly 2:503a5ac6c3b6 618 }
Spilly 12:273479524c71 619 //if no xBee data is received
Spilly 15:1087b8887b53 620 //else
Spilly 15:1087b8887b53 621 //{
Spilly 12:273479524c71 622
Spilly 12:273479524c71 623 //TODO: put this in a function
Spilly 12:273479524c71 624
Spilly 12:273479524c71 625 //checks to see if all three NEMA sentences from the GPS UART has been received
Spilly 8:c77ab7615b21 626 if(gps.getData())
Spilly 2:503a5ac6c3b6 627 {
Spilly 8:c77ab7615b21 628 double tempPos[2] = {0,0};
Spilly 5:40ac894e0fa7 629
Spilly 12:273479524c71 630 //store most recent gps location in a temporary variable (using temporary variables because GPS data is accumulated in a low pass filter)
Spilly 8:c77ab7615b21 631 tempPos[0] = gps.degLat;
Spilly 8:c77ab7615b21 632 tempPos[1] = gps.degLon;
Spilly 12:273479524c71 633
Spilly 12:273479524c71 634 //calculate the magnitude of the vector
Spilly 8:c77ab7615b21 635 getDist(goalPos[wayPtNum][0],goalPos[wayPtNum][1], tempPos);
Spilly 12:273479524c71 636
Spilly 12:273479524c71 637 //calculate the angle of the vector
Spilly 8:c77ab7615b21 638 getAngle(goalPos[wayPtNum][0],goalPos[wayPtNum][1], tempPos, 0);
Spilly 5:40ac894e0fa7 639
Spilly 12:273479524c71 640 //checks if the magnitude of distance from goal position is less than threshold for "arriving"
Spilly 5:40ac894e0fa7 641 if(polarVector[0] <= ARRIVED)
Spilly 5:40ac894e0fa7 642 {
Spilly 5:40ac894e0fa7 643 xBee.printf("Goal Position %d reached!\n", wayPtNum);
Spilly 5:40ac894e0fa7 644 wait(1);
Spilly 12:273479524c71 645
Spilly 12:273479524c71 646 //increment waypoint number
Spilly 5:40ac894e0fa7 647 wayPtNum ++;
Spilly 12:273479524c71 648
Spilly 12:273479524c71 649 //we only have ten waypoints so we mus stop at ten
Spilly 12:273479524c71 650 if(wayPtNum >= 10)
Spilly 5:40ac894e0fa7 651 {
Spilly 5:40ac894e0fa7 652 xBee.printf("Final Position Reached!\nShutting down\n");
Spilly 8:c77ab7615b21 653 goStop();
Spilly 8:c77ab7615b21 654 mode = 0;
Spilly 8:c77ab7615b21 655 //while(1);
Spilly 8:c77ab7615b21 656 }
Spilly 8:c77ab7615b21 657 else
Spilly 8:c77ab7615b21 658 {
Spilly 8:c77ab7615b21 659 //flush heading PID data since we have a new heading
Spilly 8:c77ab7615b21 660 headingPID.reset();
Spilly 12:273479524c71 661
Spilly 12:273479524c71 662 //calculate the angle of the vector
Spilly 8:c77ab7615b21 663 getAngle(goalPos[wayPtNum ][0],goalPos[wayPtNum][1], goalPos[wayPtNum - 1], 1);
Spilly 12:273479524c71 664
Spilly 8:c77ab7615b21 665 xBee.printf("Moving to Goal Position %d\theading = %f\n", wayPtNum, polarVector[1]);
Spilly 5:40ac894e0fa7 666 }
Spilly 8:c77ab7615b21 667 }
Spilly 8:c77ab7615b21 668 }
Spilly 8:c77ab7615b21 669
Spilly 12:273479524c71 670 //time to read accelerometer?
Spilly 8:c77ab7615b21 671 if(acc.read() >= ACCEL_PERIOD)
Spilly 8:c77ab7615b21 672 {
Spilly 12:273479524c71 673 //get accelerometer data
Spilly 8:c77ab7615b21 674 getAccel();
Spilly 12:273479524c71 675
Spilly 12:273479524c71 676 //reset timer to zero
Spilly 8:c77ab7615b21 677 acc.reset();
Spilly 8:c77ab7615b21 678 }
Spilly 12:273479524c71 679
Spilly 12:273479524c71 680 //time to read magnatometer and calculate heading PID?
Spilly 8:c77ab7615b21 681 if(magn.read() >= MAGN_PERIOD)
Spilly 8:c77ab7615b21 682 {
Spilly 12:273479524c71 683 //get magnatometer data
Spilly 12:273479524c71 684 getMagn(); //IMUDataAndFilters.h
Spilly 12:273479524c71 685 updateAngles(); //IMUDataAndFilters.h
Spilly 12:273479524c71 686 filtered = lowPass(yaw, filtered, 0); //IMUDataAndFilters.h
Spilly 12:273479524c71 687 magDiff = whichWay(filtered, 0); //navigation.h
Spilly 12:273479524c71 688
Spilly 12:273479524c71 689 //give PID input
Spilly 8:c77ab7615b21 690 headingPID.setProcessValue(-magDiff);
Spilly 12:273479524c71 691
Spilly 12:273479524c71 692 //get output from PID
Spilly 8:c77ab7615b21 693 curSet = headingPID.compute();
Spilly 12:273479524c71 694
Spilly 12:273479524c71 695 //reset timer to zero
Spilly 8:c77ab7615b21 696 magn.reset();
Spilly 8:c77ab7615b21 697 }
Spilly 8:c77ab7615b21 698
Spilly 8:c77ab7615b21 699 //This moves actuator to the requested position
Spilly 8:c77ab7615b21 700 //This is proportional feedback only
Spilly 8:c77ab7615b21 701 if(loopTimer.read() > RATE)
Spilly 8:c77ab7615b21 702 {
Spilly 12:273479524c71 703 //TODO: put this in a function
Spilly 12:273479524c71 704
Spilly 12:273479524c71 705 //This moves actuator to the requested position
Spilly 12:273479524c71 706 //This is proportional feedback ONLY
Spilly 12:273479524c71 707
Spilly 12:273479524c71 708 //read analog input from wiper on potentiometer on linear actuator
Spilly 8:c77ab7615b21 709 potVoltage = pot.read();
Spilly 12:273479524c71 710 //read analog input from battery voltage
Spilly 8:c77ab7615b21 711 batVoltage = battery.read();
Spilly 12:273479524c71 712 //calculate ratio of the two (using a ratio prevents battery voltage fluctuation from affecting actual position)
Spilly 8:c77ab7615b21 713 voltRatio = potVoltage / batVoltage;
Spilly 8:c77ab7615b21 714
Spilly 12:273479524c71 715 //calculate the absolute value of how far off from requested actuator position we are (saves a few processor cycles)
Spilly 8:c77ab7615b21 716 float absDiff = sqrt((curSet - voltRatio) * (curSet - voltRatio));
Spilly 12:273479524c71 717
Spilly 12:273479524c71 718 //are we off enough to care? if so, stop moving the actuator
Spilly 8:c77ab7615b21 719 if(absDiff <= RATIO_TOLERANCE)
Spilly 8:c77ab7615b21 720 {
Spilly 8:c77ab7615b21 721 turnStop(1.0f, 1.0f);
Spilly 12:273479524c71 722 //xBee.printf("done\n");
Spilly 8:c77ab7615b21 723 }
Spilly 12:273479524c71 724 //do we need to turn right?
Spilly 8:c77ab7615b21 725 else if((curSet - voltRatio) >= 0)
Spilly 8:c77ab7615b21 726 {
Spilly 8:c77ab7615b21 727 if(voltRatio > MAX_RATIO)
Spilly 8:c77ab7615b21 728 {
Spilly 12:273479524c71 729 //xBee.printf("Max Limit Reached\n");
Spilly 8:c77ab7615b21 730 turnStop(1.0f, 1.0f);
Spilly 8:c77ab7615b21 731 }
Spilly 8:c77ab7615b21 732 else
Spilly 8:c77ab7615b21 733 {
Spilly 8:c77ab7615b21 734 turnRight(1.0f, 1.0f);
Spilly 12:273479524c71 735 //xBee.printf("turning Right\n");
Spilly 8:c77ab7615b21 736 }
Spilly 8:c77ab7615b21 737 }
Spilly 12:273479524c71 738 //do we need to turn left?
Spilly 8:c77ab7615b21 739 else
Spilly 8:c77ab7615b21 740 {
Spilly 8:c77ab7615b21 741 if(voltRatio < MIN_RATIO)
Spilly 8:c77ab7615b21 742 {
Spilly 12:273479524c71 743 //xBee.printf("Min Limit Reached\n");
Spilly 8:c77ab7615b21 744 turnStop(1.0f, 1.0f);
Spilly 8:c77ab7615b21 745 }
Spilly 8:c77ab7615b21 746 else
Spilly 8:c77ab7615b21 747 {
Spilly 8:c77ab7615b21 748 turnLeft(1.0f, 1.0f);
Spilly 12:273479524c71 749 //xBee.printf("turning Left\n");
Spilly 8:c77ab7615b21 750 }
Spilly 8:c77ab7615b21 751 }
Spilly 12:273479524c71 752
Spilly 12:273479524c71 753 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 12:273479524c71 754
Spilly 12:273479524c71 755 //record data to SD card
Spilly 12:273479524c71 756 data = fopen("/sd/data.txt", "a");
Spilly 12:273479524c71 757 fprintf(data, "%d,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", wayPtNum, (batVoltage * VOLT_MULT), voltRatio, curSet, filtered, magDiff, polarVector[0], gps.degLat, gps.degLon, gps.heading, gps.altitude, gps.speed, gps.fixtype, gps.satellites);
Spilly 12:273479524c71 758 fclose(data);
Spilly 12:273479524c71 759
Spilly 12:273479524c71 760 //reset timer to zero
Spilly 8:c77ab7615b21 761 loopTimer.reset();
Spilly 2:503a5ac6c3b6 762 }
Spilly 15:1087b8887b53 763 //}
Spilly 0:e79311aae7ed 764 }
Spilly 7:ebc76b0f21da 765
Spilly 8:c77ab7615b21 766 /*********************************************************************************************************************************************************************************************/
Spilly 7:ebc76b0f21da 767 // record position mode
Spilly 8:c77ab7615b21 768 /*********************************************************************************************************************************************************************************************/
Spilly 7:ebc76b0f21da 769
Spilly 5:40ac894e0fa7 770 if(mode == 3)
Spilly 0:e79311aae7ed 771 {
Spilly 12:273479524c71 772 //stop the trolling motor
Spilly 8:c77ab7615b21 773 goStop();
Spilly 12:273479524c71 774
Spilly 7:ebc76b0f21da 775 xBee.printf("\nPlease enter position number (0-9), or press y to return to manual mode\n");
Spilly 5:40ac894e0fa7 776
Spilly 5:40ac894e0fa7 777 while(!xBee.readable());
Spilly 5:40ac894e0fa7 778 char recChar = xBee.getc();
Spilly 5:40ac894e0fa7 779 recChar = xBee.getc();
Spilly 7:ebc76b0f21da 780
Spilly 5:40ac894e0fa7 781 //return to manual mode
Spilly 5:40ac894e0fa7 782 if(recChar == 'y')
Spilly 2:503a5ac6c3b6 783 {
Spilly 5:40ac894e0fa7 784 mode = 0;
Spilly 2:503a5ac6c3b6 785 }
Spilly 5:40ac894e0fa7 786 else
Spilly 2:503a5ac6c3b6 787 {
Spilly 12:273479524c71 788 //TODO: put this in a function
Spilly 12:273479524c71 789
Spilly 7:ebc76b0f21da 790 xBee.printf("\nFinding most accurate GPS position\nThis will take a few seconds\n\n");
Spilly 7:ebc76b0f21da 791
Spilly 7:ebc76b0f21da 792 float lowestHDOP = 100;
Spilly 7:ebc76b0f21da 793
Spilly 8:c77ab7615b21 794 //take 50 GPS readings and keep the position with the lowest horizontal dilution of precision (HDOP)
Spilly 8:c77ab7615b21 795 //lower HDOP = less error
Spilly 7:ebc76b0f21da 796 for(int i = 0; i< 50; i++)
Spilly 7:ebc76b0f21da 797 {
Spilly 8:c77ab7615b21 798 //wait for data to be available
Spilly 8:c77ab7615b21 799 //while(!gps._UltimateGps.readable())
Spilly 7:ebc76b0f21da 800 gps.parseData();
Spilly 7:ebc76b0f21da 801
Spilly 7:ebc76b0f21da 802 if(gps.HDOP <= lowestHDOP)
Spilly 7:ebc76b0f21da 803 {
Spilly 7:ebc76b0f21da 804 lowestHDOP = gps.HDOP;
Spilly 7:ebc76b0f21da 805 curPos[0] = gps.degLat;
Spilly 7:ebc76b0f21da 806 curPos[1] = gps.degLon;
Spilly 7:ebc76b0f21da 807 }
Spilly 8:c77ab7615b21 808 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 809 char tempChar = 'n';
Spilly 12:273479524c71 810
Spilly 12:273479524c71 811 //first lockup was here
Spilly 12:273479524c71 812 //now pressing "1" will break out of while loop
Spilly 8:c77ab7615b21 813 while(!gps.getData() && !(tempChar == '1'))
Spilly 8:c77ab7615b21 814 {
Spilly 8:c77ab7615b21 815 if(xBee.readable())
Spilly 8:c77ab7615b21 816 {
Spilly 8:c77ab7615b21 817 tempChar = xBee.getc();
Spilly 8:c77ab7615b21 818 i = 50;
Spilly 8:c77ab7615b21 819 }
Spilly 8:c77ab7615b21 820 }
Spilly 7:ebc76b0f21da 821 }
Spilly 5:40ac894e0fa7 822 if(recChar == '0')
Spilly 5:40ac894e0fa7 823 {
Spilly 5:40ac894e0fa7 824 goalPos[0][0] = curPos[0];
Spilly 5:40ac894e0fa7 825 goalPos[0][1] = curPos[1];
Spilly 12:273479524c71 826
Spilly 8:c77ab7615b21 827 //write new coords to SD card
Spilly 12:273479524c71 828 way = fopen("/sd/GPS_CORDS.txt", "w+");
Spilly 8:c77ab7615b21 829 for(int x = 0; x<=9; x++)
Spilly 8:c77ab7615b21 830 {
Spilly 12:273479524c71 831 fprintf(way, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
Spilly 8:c77ab7615b21 832 }
Spilly 12:273479524c71 833 fclose(way);
Spilly 5:40ac894e0fa7 834 }
Spilly 5:40ac894e0fa7 835 else if(recChar == '1')
Spilly 5:40ac894e0fa7 836 {
Spilly 5:40ac894e0fa7 837 goalPos[1][0] = curPos[0];
Spilly 5:40ac894e0fa7 838 goalPos[1][1] = curPos[1];
Spilly 12:273479524c71 839
Spilly 8:c77ab7615b21 840 //write new coords to SD card
Spilly 12:273479524c71 841 way = fopen("/sd/GPS_CORDS.txt", "w+");
Spilly 8:c77ab7615b21 842 for(int x = 0; x<=9; x++)
Spilly 8:c77ab7615b21 843 {
Spilly 12:273479524c71 844 fprintf(way, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
Spilly 8:c77ab7615b21 845 }
Spilly 12:273479524c71 846 fclose(way);
Spilly 5:40ac894e0fa7 847 }
Spilly 5:40ac894e0fa7 848 else if(recChar == '2')
Spilly 5:40ac894e0fa7 849 {
Spilly 5:40ac894e0fa7 850 goalPos[2][0] = curPos[0];
Spilly 5:40ac894e0fa7 851 goalPos[2][1] = curPos[1];
Spilly 12:273479524c71 852
Spilly 8:c77ab7615b21 853 //write new coords to SD card
Spilly 12:273479524c71 854 way = fopen("/sd/GPS_CORDS.txt", "w+");
Spilly 8:c77ab7615b21 855 for(int x = 0; x<=9; x++)
Spilly 8:c77ab7615b21 856 {
Spilly 12:273479524c71 857 fprintf(way, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
Spilly 8:c77ab7615b21 858 }
Spilly 12:273479524c71 859 fclose(way);
Spilly 5:40ac894e0fa7 860 }
Spilly 5:40ac894e0fa7 861 else if(recChar == '3')
Spilly 2:503a5ac6c3b6 862 {
Spilly 5:40ac894e0fa7 863 goalPos[3][0] = curPos[0];
Spilly 5:40ac894e0fa7 864 goalPos[3][1] = curPos[1];
Spilly 12:273479524c71 865
Spilly 8:c77ab7615b21 866 //write new coords to SD card
Spilly 12:273479524c71 867 way = fopen("/sd/GPS_CORDS.txt", "w+");
Spilly 8:c77ab7615b21 868 for(int x = 0; x<=9; x++)
Spilly 8:c77ab7615b21 869 {
Spilly 12:273479524c71 870 fprintf(way, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
Spilly 8:c77ab7615b21 871 }
Spilly 12:273479524c71 872 fclose(way);
Spilly 5:40ac894e0fa7 873 }
Spilly 5:40ac894e0fa7 874 else if(recChar == '4')
Spilly 5:40ac894e0fa7 875 {
Spilly 5:40ac894e0fa7 876 goalPos[4][0] = curPos[0];
Spilly 5:40ac894e0fa7 877 goalPos[4][1] = curPos[1];
Spilly 12:273479524c71 878
Spilly 8:c77ab7615b21 879 //write new coords to SD card
Spilly 12:273479524c71 880 way = fopen("/sd/GPS_CORDS.txt", "w+");
Spilly 8:c77ab7615b21 881 for(int x = 0; x<=9; x++)
Spilly 8:c77ab7615b21 882 {
Spilly 12:273479524c71 883 fprintf(way, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
Spilly 8:c77ab7615b21 884 }
Spilly 12:273479524c71 885 fclose(way);
Spilly 5:40ac894e0fa7 886 }
Spilly 5:40ac894e0fa7 887 else if(recChar == '5')
Spilly 5:40ac894e0fa7 888 {
Spilly 5:40ac894e0fa7 889 goalPos[5][0] = curPos[0];
Spilly 5:40ac894e0fa7 890 goalPos[5][1] = curPos[1];
Spilly 12:273479524c71 891
Spilly 8:c77ab7615b21 892 //write new coords to SD card
Spilly 12:273479524c71 893 way = fopen("/sd/GPS_CORDS.txt", "w+");
Spilly 8:c77ab7615b21 894 for(int x = 0; x<=9; x++)
Spilly 8:c77ab7615b21 895 {
Spilly 12:273479524c71 896 fprintf(way, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
Spilly 8:c77ab7615b21 897 }
Spilly 12:273479524c71 898 fclose(way);
Spilly 5:40ac894e0fa7 899 }
Spilly 5:40ac894e0fa7 900 else if(recChar == '6')
Spilly 5:40ac894e0fa7 901 {
Spilly 5:40ac894e0fa7 902 goalPos[6][0] = curPos[0];
Spilly 5:40ac894e0fa7 903 goalPos[6][1] = curPos[1];
Spilly 5:40ac894e0fa7 904 }
Spilly 5:40ac894e0fa7 905 else if(recChar == '7')
Spilly 5:40ac894e0fa7 906 {
Spilly 5:40ac894e0fa7 907 goalPos[7][0] = curPos[0];
Spilly 5:40ac894e0fa7 908 goalPos[7][1] = curPos[1];
Spilly 12:273479524c71 909
Spilly 8:c77ab7615b21 910 //write new coords to SD card
Spilly 12:273479524c71 911 way = fopen("/sd/GPS_CORDS.txt", "w+");
Spilly 8:c77ab7615b21 912 for(int x = 0; x<=9; x++)
Spilly 8:c77ab7615b21 913 {
Spilly 12:273479524c71 914 fprintf(way, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
Spilly 8:c77ab7615b21 915 }
Spilly 12:273479524c71 916 fclose(way);
Spilly 2:503a5ac6c3b6 917 }
Spilly 5:40ac894e0fa7 918 else if(recChar == '8')
Spilly 5:40ac894e0fa7 919 {
Spilly 5:40ac894e0fa7 920 goalPos[8][0] = curPos[0];
Spilly 5:40ac894e0fa7 921 goalPos[8][1] = curPos[1];
Spilly 12:273479524c71 922
Spilly 8:c77ab7615b21 923 //write new coords to SD card
Spilly 12:273479524c71 924 way = fopen("/sd/GPS_CORDS.txt", "w+");
Spilly 8:c77ab7615b21 925 for(int x = 0; x<=9; x++)
Spilly 8:c77ab7615b21 926 {
Spilly 12:273479524c71 927 fprintf(way, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
Spilly 8:c77ab7615b21 928 }
Spilly 12:273479524c71 929 fclose(way);
Spilly 5:40ac894e0fa7 930 }
Spilly 5:40ac894e0fa7 931 else if(recChar == '9')
Spilly 2:503a5ac6c3b6 932 {
Spilly 5:40ac894e0fa7 933 goalPos[9][0] = curPos[0];
Spilly 5:40ac894e0fa7 934 goalPos[9][1] = curPos[1];
Spilly 12:273479524c71 935
Spilly 8:c77ab7615b21 936 //write new coords to SD card
Spilly 12:273479524c71 937 way = fopen("/sd/GPS_CORDS.txt", "w+");
Spilly 8:c77ab7615b21 938 for(int x = 0; x<=9; x++)
Spilly 8:c77ab7615b21 939 {
Spilly 12:273479524c71 940 fprintf(way, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
Spilly 8:c77ab7615b21 941 }
Spilly 12:273479524c71 942 fclose(way);
Spilly 2:503a5ac6c3b6 943 }
Spilly 7:ebc76b0f21da 944 xBee.printf("position %c updated\t", recChar);
Spilly 2:503a5ac6c3b6 945 }
Spilly 7:ebc76b0f21da 946 xBee.printf("returning to manual mode\n\n");
Spilly 5:40ac894e0fa7 947 mode = 0;
Spilly 5:40ac894e0fa7 948 }
Spilly 5:40ac894e0fa7 949 }
Spilly 5:40ac894e0fa7 950 }
Spilly 5:40ac894e0fa7 951
Spilly 8:c77ab7615b21 952 /*************************************************************************************************************************************************************************************************/
Spilly 8:c77ab7615b21 953 // create polar vector based on two sets of latitude and longitude
Spilly 8:c77ab7615b21 954 /*************************************************************************************************************************************************************************************************/
Spilly 8:c77ab7615b21 955 //TODO:
Spilly 8:c77ab7615b21 956 //getDist and getAngle need to be optimized
Spilly 8:c77ab7615b21 957 //they were one function but had to be hacked apart
Spilly 7:ebc76b0f21da 958
Spilly 8:c77ab7615b21 959 void getDist(double posZero, double posOne, double curPos[2])
Spilly 5:40ac894e0fa7 960 {
Spilly 7:ebc76b0f21da 961 double arcLength[2];
Spilly 7:ebc76b0f21da 962 double goalPos[2];
Spilly 5:40ac894e0fa7 963 goalPos[0] = posZero;
Spilly 5:40ac894e0fa7 964 goalPos[1] = posOne;
Spilly 5:40ac894e0fa7 965
Spilly 7:ebc76b0f21da 966 /*Note: arc length = radius * angle*/
Spilly 5:40ac894e0fa7 967 //Y
Spilly 5:40ac894e0fa7 968 arcLength[1] = EARTHRADIUS * ((goalPos[0] - curPos[0]) * DEGREETORAD);
Spilly 5:40ac894e0fa7 969 //X
Spilly 5:40ac894e0fa7 970 arcLength[0] = EARTHRADIUS * ((goalPos[1] - curPos[1]) * DEGREETORAD);
Spilly 5:40ac894e0fa7 971
Spilly 5:40ac894e0fa7 972 //calculate magnitude of vector
Spilly 5:40ac894e0fa7 973 polarVector[0] = sqrt((arcLength[0] * arcLength[0]) + (arcLength[1] * arcLength[1]));
Spilly 8:c77ab7615b21 974 }
Spilly 8:c77ab7615b21 975
Spilly 8:c77ab7615b21 976 void getAngle(double posZero, double posOne, double curPos[2], int flush)
Spilly 8:c77ab7615b21 977 {
Spilly 8:c77ab7615b21 978 double tempAngle = 0;
Spilly 8:c77ab7615b21 979 double arcLength[2];
Spilly 8:c77ab7615b21 980 double goalPos[2];
Spilly 8:c77ab7615b21 981 goalPos[0] = posZero;
Spilly 8:c77ab7615b21 982 goalPos[1] = posOne;
Spilly 5:40ac894e0fa7 983
Spilly 8:c77ab7615b21 984 /*Note: arc length = radius * angle*/
Spilly 8:c77ab7615b21 985 //Y
Spilly 8:c77ab7615b21 986 arcLength[1] = EARTHRADIUS * ((goalPos[0] - curPos[0]) * DEGREETORAD);
Spilly 8:c77ab7615b21 987 //X
Spilly 8:c77ab7615b21 988 arcLength[0] = EARTHRADIUS * ((goalPos[1] - curPos[1]) * DEGREETORAD);
Spilly 12:273479524c71 989
Spilly 12:273479524c71 990 //get rid of previous low passed angle data
Spilly 8:c77ab7615b21 991 if(flush)
Spilly 8:c77ab7615b21 992 {
Spilly 8:c77ab7615b21 993 //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 994 polarVector[1] = (RADTODEGREE * (atan2(arcLength[0], arcLength[1])));
Spilly 8:c77ab7615b21 995 //make negative angles positive
Spilly 8:c77ab7615b21 996 if(polarVector[1] < 0) polarVector[1] = polarVector[1] + 360;
Spilly 8:c77ab7615b21 997 }
Spilly 12:273479524c71 998
Spilly 12:273479524c71 999 //lowpass filter the angle
Spilly 8:c77ab7615b21 1000 else
Spilly 8:c77ab7615b21 1001 {
Spilly 8:c77ab7615b21 1002
Spilly 8:c77ab7615b21 1003 tempAngle = (RADTODEGREE * (atan2(arcLength[0], arcLength[1])));
Spilly 8:c77ab7615b21 1004
Spilly 8:c77ab7615b21 1005 if(tempAngle < 0) tempAngle = tempAngle + 360;
Spilly 8:c77ab7615b21 1006 polarVector[1] = lowPass(tempAngle, polarVector[1], 3);
Spilly 8:c77ab7615b21 1007 }
Spilly 8:c77ab7615b21 1008 }