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@12:273479524c71, 2015-04-29 (annotated)

Committer:
Spilly
Date:
Wed Apr 29 18:07:43 2015 +0000
Revision:
12:273479524c71
Parent:
11:1b34319671eb
Child:
13:17f04a55c6e2
Updated comments

Who changed what in which revision?

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