AutoBoat
Project Date - 09/2013
A GPS enabled boat to take part in a company competition.
The goal was to cross the small pond out front in the fastest time possible withing a pretty large landing area.
Despite testing at home, the GPS never got a lock on the day of the competition, so I had to operate the AutoBoat in "dumb" mode without any navigation.
The wind caused it to never fully get across the pond. It crashed into the shore about midway the first run. The second run, it did circles in the middle of the pond until the battery ran out and it had to be rescued from a kayaker.
Like any good project though, I procrastinated until the last moment then tried to put it all together and hoped that it would work. There was no testing on the complete system prior to the competition, just components. One lesson learned was to to test the complete system at least a couple weeks prior to it being due.
Since then I've been jaded enough not to pull it back out and troubleshoot what's wrong, but I imagine there will be a good lesson in there.
More to follow soon...
A GPS enabled boat to take part in a company competition.
The goal was to cross the small pond out front in the fastest time possible withing a pretty large landing area.
Despite testing at home, the GPS never got a lock on the day of the competition, so I had to operate the AutoBoat in "dumb" mode without any navigation.
The wind caused it to never fully get across the pond. It crashed into the shore about midway the first run. The second run, it did circles in the middle of the pond until the battery ran out and it had to be rescued from a kayaker.
Like any good project though, I procrastinated until the last moment then tried to put it all together and hoped that it would work. There was no testing on the complete system prior to the competition, just components. One lesson learned was to to test the complete system at least a couple weeks prior to it being due.
Since then I've been jaded enough not to pull it back out and troubleshoot what's wrong, but I imagine there will be a good lesson in there.
More to follow soon...
GPS working for the first time.
Final internal - which didn't properly lock the GPS.
On the day of the competition.
Here's the code that I swore was going to work.
// AutoBoat 09/2013 // Based on... // LCD // http://linksprite.com/wiki/index.php5?title=16_X_2_LCD_Keypad_Shield_for_Arduino // Digital Compass // http://playground.arduino.cc/Learning/Hmc6352 // GPS // http://www.doctormonk.com/2012/05/sparkfun-venus-gps-and-arduino.html #include <LiquidCrystal.h> #include <Wire.h> #include <SoftwareSerial.h> #include <math.h> #include <Servo.h> // Configure the LCD Shield LiquidCrystal lcd(8, 13, 9, 4, 5, 6, 7); // For the Linksprite LCD Shield // Right, Up, Down, Left, Select int LCD_KeyArray[5] = {50, 200, 400, 600, 800}; int LCD_NumKeys = 5; int LCD_KeyRead; int LCD_CurrentKey = -1; int LCD_OldKey = -1; int LCD_KeyPin = 0; const int LCD_MaxCharRow = 16; // Configure the Digital Compass int Comp_HMC6352Address = 0x42; int Comp_SlaveAddress = Comp_HMC6352Address >> 1; // This results in 0x21 as the address to pass to TWI byte Comp_HeadingData[2]; int Comp_HeadingValue; // Configure the GPS //SoftwareSerial GPS_SoftSerial(2, 3); // RX, TX (TX not used) const int GPS_SentenceSize = 80; char GPS_Sentence[GPS_SentenceSize]; char GPS_Display[LCD_MaxCharRow]; float GPS_Lat = 0.0; float GPS_Long = 0.0; float GPS_Lat_Last = 0.0; float GPS_Long_Last = 0.0; float GPS_Lat_Target = 0.0; float GPS_Long_Target = 0.0; float GPS_R = 6371; float GPS_CurrentHeading; float GPS_TargetHeading; float GPS_CurrentDistance; const float pi = 3.14159265359; const float pi2 = pi*2; int io_KillRunPin = 5; // A5 int io_KillRunRead; boolean io_RunFlag = false; int io_MotorServoPin = 11; int io_SteerServoPin = 13; Servo motorServo; // 90 = off, 180 = full speed Servo steerServo; // 11 = full left, 49 = straight, 114 = full right // ---------------- MAIN --------------------------------------- void setup() { // put your setup code here, to run once: Serial.begin(9600); LCD_Init(); //GPS_SoftSerial.begin(9600); motorServo.attach(io_MotorServoPin); steerServo.attach(io_SteerServoPin); delay(1500); lcd.clear(); lcd.print("Get Ready!"); } void loop() { // put your main code here, to run repeatedly: GPS_UpdateReading(); io_KillRunRead = analogRead(io_KillRunPin); if (io_KillRunRead < 100) io_RunFlag = true; else io_RunFlag = false; GPS_UpdateLastValues(2000); GPS_CurrentHeading = GPS_GetHeading(GPS_Lat_Last, GPS_Long_Last, GPS_Lat, GPS_Long); GPS_TargetHeading = GPS_GetHeading(GPS_Lat, GPS_Long, GPS_Lat_Target, GPS_Long_Target); GPS_CurrentDistance = GPS_GetDistance(GPS_Lat, GPS_Long, GPS_Lat_Target, GPS_Long_Target); AB_UpdateSteering(GPS_CurrentHeading, GPS_TargetHeading, io_RunFlag); AB_UpdateSpeed(GPS_CurrentDistance, io_RunFlag); LCD_KeyRead = LCD_DetectKeyPress(LCD_KeyPin); // Set Target if (LCD_KeyRead == 3){ AB_SetTarget(); lcd.clear(); lcd.print("Get Ready Set!"); } if (LCD_KeyRead == 2){ lcd.clear(); lcd.print("Update..."); delay(500); GPS_PrintInfo(GPS_Lat_Target, GPS_Long_Target, GPS_Lat, GPS_Long); LCD_OldKey = -1; } } // --------------------------------------------------------------- // ************** AutoBoat Functions ***************************** void AB_SetTarget(){ lcd.clear(); lcd.print("Set New Target"); lcd.setCursor(0,1); lcd.print("Sel=Yes, Up=View"); // Returns 5000 if no key was pressed int lcdKey = LCD_DetectKeyPress(LCD_KeyPin); while (lcdKey < 0){ // Try agian if no key was pressed lcdKey = LCD_DetectKeyPress(LCD_KeyPin); // Up - View if (lcdKey == 1){ lcd.clear(); lcd.print("Current Target"); delay(1500); GPS_PrintInfo(GPS_Lat_Target, GPS_Long_Target, GPS_Lat, GPS_Long); delay(2000); return; } // Select - Set if(lcdKey == 4){ GPS_Lat_Target = GPS_Lat; GPS_Long_Target = GPS_Long; lcd.clear(); lcd.print("Setting"); lcd.setCursor(0, 1); lcd.print("New Target"); delay(1500); GPS_PrintInfo(GPS_Lat_Target, GPS_Long_Target, GPS_Lat, GPS_Long); delay(2000); return; } } } void AB_UpdateSteering(float currentHeading, float targetHeading, boolean runFlag){ // 11 = full left, 49 = straight, 114 = full right if (!runFlag){ steerServo.write(49); return; } float headingDiff = GPS_GetHeadingDiff(currentHeading, targetHeading); float headingDiffAbs = abs(headingDiff); // Turn Left if (headingDiff < 0){ // Full Left if ((headingDiffAbs >= 180) && (headingDiffAbs < 90)){ steerServo.write(12); return; } // Slight Left if ((headingDiffAbs >= 0) && (headingDiffAbs <= 90)){ int mapValue = map(headingDiffAbs, 90, 0, 12, 49); steerServo.write(mapValue); return; } } // Trun Right else if (headingDiff > 0){ // Full Right if ((headingDiffAbs >= 180) && (headingDiffAbs < 90)){ steerServo.write(114); return; } if ((headingDiffAbs >= 0) && (headingDiffAbs <= 90)){ int mapValue = map(headingDiffAbs, 90, 0, 114, 49); steerServo.write(mapValue); return; } } steerServo.write(49); } void AB_UpdateSpeed(float currentDistance, boolean runFlag){ // 90 = off, 180 = full speed int maxValue = 135; // Half Power if (!runFlag){ motorServo.write(90); return; } if (currentDistance > 75){ motorServo.write(maxValue); return; } int mapValue = map(currentDistance, 75, 0, maxValue, 90); } // ************** LCD Shield ************************************ void LCD_Init(){ lcd.begin(16, 2); lcd.clear(); lcd.setCursor(0,0); lcd.print("Hello World!"); lcd.setCursor(0,1); lcd.print("AutoBoat v1.0"); } int LCD_DetectKeyPress(unsigned int ADC_ReadPin){ // Introduce some debouncing // Will return -1 if no key was pressed int firstKey = LCD_GetKey(ADC_ReadPin); // At least on of the buttons was pressed if ((firstKey >= 0) && (LCD_OldKey != firstKey)){ delay(50); int newKey = LCD_GetKey(ADC_ReadPin); if (firstKey == newKey){ LCD_OldKey = newKey; return newKey; } } return -1; } // Convert ADC value to key number int LCD_GetKey(unsigned int ADC_ReadPin) { int readVal = analogRead(ADC_ReadPin); int k; for (k = 0; k < LCD_NumKeys; k++) { if (readVal < LCD_KeyArray[k]) { return k; } } // No valid key pressed return -1; } // ************** Digial Compass ****************************** float Comp_GetHeading(){ // Send a "A" command to the HMC6352 // This requests the current heading data Wire.beginTransmission(Comp_SlaveAddress); Wire.write("A"); // The "Get Data" command Wire.endTransmission(); delay(10); // The HMC6352 needs at least a 70us (microsecond) delay // after this command. Using 10ms just makes it safe // Read the 2 heading bytes, MSB first // The resulting 16bit word is the compass heading in 10th's of a degree // For example: a heading of 1345 would be 134.5 degrees Wire.requestFrom(Comp_SlaveAddress, 2); // Request the 2 byte heading (MSB comes first) int i = 0; while(Wire.available() && i < 2) { Comp_HeadingData[i] = Wire.read(); i++; } Comp_HeadingValue = Comp_HeadingData[0]*256 + Comp_HeadingData[1]; // Put the MSB and LSB together return Comp_HeadingValue / 1000; } // ************** GPS ******************************************** void GPS_UpdateReading(){ // To be called each time through the loop // Will update the Lat, Long, and diplay string static int i = 0; if (Serial.available()) { char ch = Serial.read(); if (ch != '\n' && i < GPS_SentenceSize) { GPS_Sentence[i] = ch; i++; } else { GPS_Sentence[i] = '\0'; i = 0; GPS_UpdateValues(); } } } void GPS_UpdateValues() { char field[20]; GPS_GetField(field, 0); if (strcmp(field, "$GPRMC") == 0) { float GPS_LatDegMin; float GPS_LatDeg; float GPS_LatMin; float GPS_LatMinDec; float GPS_LatTemp; float GPS_LongDegMin; float GPS_LongDeg; float GPS_LongMin; float GPS_LongMinDec; float GPS_LongTemp; GPS_GetField(field, 3); // Latitude GPS_LatDegMin = atof(field); GPS_LatDeg = floor(GPS_LatDegMin / 100.0); GPS_LatMin = GPS_LatDegMin - (GPS_LatDeg * 100.0000); GPS_LatMinDec = GPS_LatMin / 60.0000; GPS_LatTemp = GPS_LatDeg + GPS_LatMinDec; // Converted into a decimal GPS_GetField(field, 4); // N/S if (field[0] == 'S'){ GPS_LatTemp = GPS_LatTemp * -1.0; } GPS_GetField(field, 5); // Longitude GPS_LongDegMin = atof(field); GPS_LongDeg = floor(GPS_LongDegMin / 100.0); GPS_LongMin = GPS_LongDegMin - (GPS_LongDeg * 100.0000); GPS_LongMinDec = GPS_LongMin / 60.0000; GPS_LongTemp = GPS_LongDeg + GPS_LongMinDec; // Converted into a decimal GPS_GetField(field, 6); // E/W if (field[0] == 'W'){ GPS_LongTemp = GPS_LongTemp * -1.0; } // Update the Lat Long at the same time GPS_Lat = GPS_LatTemp; GPS_Long = GPS_LongTemp; } } void GPS_UpdateDisplay(int delayTimeInMsec){ unsigned long timeNow = millis(); static unsigned long timeLast = 0; if ((timeNow - timeLast) > delayTimeInMsec){ lcd.clear(); lcd.setCursor(0,0); lcd.print("Update"); delay(200); GPS_PrintInfo(GPS_Lat, GPS_Long, GPS_Lat_Last, GPS_Long_Last); timeLast = timeNow; } } void GPS_UpdateLastValues(int delayTimeInMsec){ unsigned long timeNow = millis(); static unsigned long timeLast = 0; if ((timeNow - timeLast) > delayTimeInMsec){ GPS_Lat_Last = GPS_Lat; GPS_Long_Last = GPS_Long; timeLast = timeNow; } } void GPS_PrintInfo(float lat1, float long1, float lat2, float long2){ float distKM = GPS_GetDistance(lat1, long1, lat2, long2); float distFT = distKM * 1000.0 * 3.28084; // Convert KM to ft float heading = GPS_GetHeading(lat1, long1, lat2, long2); lcd.clear(); lcd.setCursor(0,0); lcd.print(GPS_Lat, 6); lcd.setCursor(0,1); lcd.print(GPS_Long, 6); lcd.setCursor(12, 0); lcd.print(distFT); lcd.setCursor(12, 1); lcd.print(heading); } void GPS_GetField(char* buffer, int index) { int GPS_SentencePos = 0; int fieldPos = 0; int commaCount = 0; while (GPS_SentencePos < GPS_SentenceSize) { if (GPS_Sentence[GPS_SentencePos] == ',') { commaCount ++; GPS_SentencePos ++; } if (commaCount == index) { buffer[fieldPos] = GPS_Sentence[GPS_SentencePos]; fieldPos ++; } GPS_SentencePos ++; } buffer[fieldPos] = '\0'; } float GPS_GetDistance(float lat1, float long1, float lat2, float long2){ float diffLat = GPS_DegToRad(lat2 - lat1); float diffLong = GPS_DegToRad(long2 - long1); float lat1Rad = GPS_DegToRad(lat1); float lat2Rad = GPS_DegToRad(lat2); float a = sin(diffLat/2) * sin(diffLat/2) + sin(diffLong/2) * sin(diffLong/2) * cos(lat1Rad) * cos(lat2Rad); float c = 2 * atan2(sqrt(a), sqrt(1-a)); float distInKM = GPS_R * c; // Distance in KM return distInKM * 3280.84; // Return in ft } float GPS_GetHeading(float lat1, float long1, float lat2, float long2){ // The heading to get from lat1, long1 to lat2, long2 float diffLong = GPS_DegToRad(long2 - long1); float lat1Rad = GPS_DegToRad(lat1); float lat2Rad = GPS_DegToRad(lat2); float y = sin(diffLong) * cos(lat2Rad); float x = cos(lat1Rad) * sin(lat2Rad) - sin(lat1Rad) * cos(lat2Rad) * cos(diffLong); return GPS_RadToDeg(atan2(y, x)); } // initial = current heading, final = target float GPS_GetHeadingDiff(float initial, float final){ // http://stackoverflow.com/questions/5024375/getting-the-difference-between-two-headings float headingDiff = final - initial; float headingDiffAbs = abs(headingDiff); if (headingDiffAbs <= 180){ if (headingDiffAbs == 180) return headingDiffAbs; else return headingDiff; } else if (final > initial) return headingDiffAbs - 360; else return 360 - headingDiffAbs; } float GPS_DegToRad(float degree){ return (degree * (pi2 / 360.0)); } float GPS_RadToDeg(float radian){ return (radian * (360.0 / pi2)); } void serDiag(){ Serial.println("yes!"); Serial.println(millis()); Serial.println(""); }