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@15:1087b8887b53, 2015-05-06 (annotated)

Committer:
Spilly
Date:
Wed May 06 18:06:43 2015 +0000
Revision:
15:1087b8887b53
Parent:
14:fd20b7ac8de8
Child:
16:9d1e72ab7ec7
Changed autonomous mode to check GPS UART regardless of xBee UART.

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