Introduction
The following is a write-up of my experiences in preparing for and competing in the 3rd Annual Sparkfun Autonomous Vehicle Competition (AVC) held in Boulder, Colorado on April 23, 2011. This paper includes descriptions of the hardware, software, navigation methodology, competition experiences, design considerations, and lessons learned for Team Roadrunner, which placed 4th in the competition with a time of 1:18.
Hardware
 |
| Figure 1 - Hardware Layout |
A description of each item used for the competition is as follows:
Vehicle: Traxxas Stampede that Team Minuteman (my brother) kindly let me borrow for the competition. This has a top speed of ~11-mph without the burden of the electronics and a ~10-mph with the electronics. The electronics cause the vehicle to be top-heavy. A white PVC pipe is attached using hose clamps to prevent it from rolling.
1 – home made circuit board that allows easy connection between the microcontroller and the LCD
2 – digital compass (HPDM01) from Satistronics, a China based electronics distributor (http://www.satistronics.com/compass-module-hdpm01_p2758.html) for $18. The sensor uses I2C communication and is actually a pressure, temperature, and digital compass all packaged as one. I selected and purchased this because it was the cheapest I could find. I only used the compass feature of this sensor. I had no idea how to calibrate it until I stumbled upon this site (http://cineon01.medien.uni-weimar.de/elektrowiki/Pavlos_Iliopoulos) that provided an excellent description of the process. Thanks Pavlos Iliopoulos! Instead of using his method exactly, I placed the sensor on a breadboard, lined the edges of the breadboard up with the edges of my desk, took data, repeated the process 4 times, and then used those points to find the mid-point. The calibration method may not have been perfect, but worked well enough to get me around the Sparkfun building.
3 – 40x4 LCD I picked up for $7 from Weird Stuff, an electronics recycler in Sunnyvale, California and uses as standard Hitachi HD44780. If you are ever in the Bay Area, it is definitely a place to stop by for a few hours.
4 – 74LS157 – Multiplexer from ebay for ~$1.50. This is the same chip used in the ardupilot to swap between autonomous and manual control. This was VERY useful during testing. Instead of chasing my car and trying to catch it before it slammed into a curb, I could simply switch my radio on or off to swap between manual and autonomous mode.
5 – chinduino from ebay for $43. Once again, I purchased this product because it was the cheapest I could find. I determined that I needed more pins than a standard Arduino had available and the Mega was the best choice. The seller is ge-th2009 and they have good feedback and the board has worked flawlessly. However, a few months after purchasing from them, Sparkfun posted the following article (http://www.sparkfun.com/news/488) about GE Tech (http://www.geeetech.com/). I’m pretty sure they are also the ebay seller listed above.
6 – Gyroscope ADXRS613 150°/s from Sparkfun (http://www.sparkfun.com/products/9059) for $25. I purchased this on recommendation from my brother. I ended up not using it because the digital compass was working well and I had difficulty with unexplained gyro drift after ~1 minute. The issue was a software bug…I think the null value was being overwritten somehow. Additionally, I couldn’t figure out how to determine and set the initial condition (i.e. correct heading to the first waypoint). The compass achieved both and that’s why I stuck with it. After further consideration, the initial conditions CAN be determined and set in the following ways: offset the gyro with a reading from the compass or walk to the starting line while holding the vehicle and offset the gyro with the GPS heading output.
7 – 7.4V 2200mAh lipo battery for $20 from HobbyKing. I used an independent battery supply from the RC vehicle supply because I found that sudden acceleration would cause a low power condition on the microcontroller and sometimes cause a reset.
8 – NMEA TTL DATA 5V Sirf II GPS Module Board With Antenna from ebay for $15 after shipping. This worked amazingly well and I had good gps reception except on the East side of the Sparkfun building. I cut off the standard connector and patched it into the microcontroller.
Miscellaneous – LM317 power regulator from ebay for ~$0.50. Breadboard jumper wires (M-F, M-M, F-F) from ebay/china for ~$15. These were COMPLETELY worth the money. Hot glue…lots and lots of hot glue on all connections, jumper wires, etc. I was hesitant to use it at first, but saw a video on youtube showing that swabbing the glued area with rubbing alcohol can remove the glue with very little effort. Cable ties.
Total Cost = ~$135 (excluding the cost of the car that I borrowed)
Vehicle: Traxxas Stampede that Team Minuteman (my brother) kindly let me borrow for the competition. This has a top speed of ~11-mph without the burden of the electronics and a ~10-mph with the electronics. The electronics cause the vehicle to be top-heavy. A white PVC pipe is attached using hose clamps to prevent it from rolling.
1 – home made circuit board that allows easy connection between the microcontroller and the LCD
2 – digital compass (HPDM01) from Satistronics, a China based electronics distributor (http://www.satistronics.com/compass-module-hdpm01_p2758.html) for $18. The sensor uses I2C communication and is actually a pressure, temperature, and digital compass all packaged as one. I selected and purchased this because it was the cheapest I could find. I only used the compass feature of this sensor. I had no idea how to calibrate it until I stumbled upon this site (http://cineon01.medien.uni-weimar.de/elektrowiki/Pavlos_Iliopoulos) that provided an excellent description of the process. Thanks Pavlos Iliopoulos! Instead of using his method exactly, I placed the sensor on a breadboard, lined the edges of the breadboard up with the edges of my desk, took data, repeated the process 4 times, and then used those points to find the mid-point. The calibration method may not have been perfect, but worked well enough to get me around the Sparkfun building.
3 – 40x4 LCD I picked up for $7 from Weird Stuff, an electronics recycler in Sunnyvale, California and uses as standard Hitachi HD44780. If you are ever in the Bay Area, it is definitely a place to stop by for a few hours.
4 – 74LS157 – Multiplexer from ebay for ~$1.50. This is the same chip used in the ardupilot to swap between autonomous and manual control. This was VERY useful during testing. Instead of chasing my car and trying to catch it before it slammed into a curb, I could simply switch my radio on or off to swap between manual and autonomous mode.
5 – chinduino from ebay for $43. Once again, I purchased this product because it was the cheapest I could find. I determined that I needed more pins than a standard Arduino had available and the Mega was the best choice. The seller is ge-th2009 and they have good feedback and the board has worked flawlessly. However, a few months after purchasing from them, Sparkfun posted the following article (http://www.sparkfun.com/news/488) about GE Tech (http://www.geeetech.com/). I’m pretty sure they are also the ebay seller listed above.
6 – Gyroscope ADXRS613 150°/s from Sparkfun (http://www.sparkfun.com/products/9059) for $25. I purchased this on recommendation from my brother. I ended up not using it because the digital compass was working well and I had difficulty with unexplained gyro drift after ~1 minute. The issue was a software bug…I think the null value was being overwritten somehow. Additionally, I couldn’t figure out how to determine and set the initial condition (i.e. correct heading to the first waypoint). The compass achieved both and that’s why I stuck with it. After further consideration, the initial conditions CAN be determined and set in the following ways: offset the gyro with a reading from the compass or walk to the starting line while holding the vehicle and offset the gyro with the GPS heading output.
7 – 7.4V 2200mAh lipo battery for $20 from HobbyKing. I used an independent battery supply from the RC vehicle supply because I found that sudden acceleration would cause a low power condition on the microcontroller and sometimes cause a reset.
8 – NMEA TTL DATA 5V Sirf II GPS Module Board With Antenna from ebay for $15 after shipping. This worked amazingly well and I had good gps reception except on the East side of the Sparkfun building. I cut off the standard connector and patched it into the microcontroller.
Miscellaneous – LM317 power regulator from ebay for ~$0.50. Breadboard jumper wires (M-F, M-M, F-F) from ebay/china for ~$15. These were COMPLETELY worth the money. Hot glue…lots and lots of hot glue on all connections, jumper wires, etc. I was hesitant to use it at first, but saw a video on youtube showing that swabbing the glued area with rubbing alcohol can remove the glue with very little effort. Cable ties.
Total Cost = ~$135 (excluding the cost of the car that I borrowed)
System Description
 |
| Figure 2 - System Diagram |
Software
All coding was done using Notepad++ and compiled and uploaded using the Arduino IDE. I NEVER could have accomplished this project without the existing Arduino libraries, system, etc. It is an amazing system that makes projects like this possible for non-software engineers. THANK YOU! to all those that have developed the libraries, IDE, hardware etc.
I used the following libraries: Wire.h (needed for I2C communication, Servo.h (self-explanatory), math.h (required for angle calculations), TinyGPS.h (GPS NMEA parsing), LiquidCrystal.h (LCD writing). The code is at the end of this write-up.
Navigation
The most confusing part and most critical part for navigation is to ensure that all heading systems are in sync with one another. See the figure below:
 |
| Figure 3 - Navigation Domains |
Waypoint Selection and Export (google maps vs bing maps)
I selected my path to navigate first using google maps and secondly using bing maps. Both systems allow for exporting latitude and longitude points, but bing accomplishes this much more conveniently. Lat/lon points may be extracted from google maps in two ways: 1 – hold down “shift” and the write down the numbers for each point; 2 – export the waypoints to a kml file using the URL change found here (http://sites.google.com/site/gmapstips/export-my-map-as-kml). Each method is time consuming and not easy. However, lat/lon points may easily be exported and extracted using bing maps by selecting “export to gpx.” Post processing of gpx files is quick and easy.
Using bing maps, I drew a line around the Sparkfun building (or a selected practice course). I would add additional waypoints as necessary to ensure that the vehicle lined up or slowed down for critical areas (turns, narrow sections, etc).
Waypoint Acceptance
During initial testing, the car was programmed to head in a straight line to a waypoint and once the vehicle was within 10-ft of that waypoint, it would proceed to the next. However, the GPS only updates at 1-Hz and if the car was going too fast (>10-ft/s or 7-mph) or if GPS reception quality was low, it would overshoot the waypoint, backtrack to find the waypoint, and finally proceed to the next. This sometimes caused anomalies.
I first addressed this issue by decreasing vehicle speed from 100% to ~20% when it was 20-ft from the waypoint. This would slow the vehicle enough so that the GPS could get an update within the 10-ft acceptance range. This reduced my missed waypoints and backtracking substantially, but not to 0.
I next addressed this issue by changing my acceptance criteria from distance to latitude/longitude. For example, a vehicle is at a lat/lon location of (45°, -109°) traveling in a Southern direction (latitude is decreasing, longitude is relatively constant) and the next waypoint is at (44°, -109°). Once the vehicle’s latitude is less than or equal to the next waypoint’s latitude (<=44°, -109), the waypoint would be accepted and the vehicle will then start navigating to the next waypoint.
The disadvantage of this method is initial setup of each waypoint takes longer and more forethought. I used a 3 column array (lat, long, modifier) and the modifier was input manually. The modifier instructed the microcontroller if the acceptance criteria are based on latitude (traveling North/South), longitude (East/West), distance (<=10-ft), or a special case. In fact, there are 2 additional cases since latitude is increasing while traveling North, where latitude is decreasing while traveling South. The same holds true for longitude as well.
The advantages of this method are the vehicle speed does not need to be decreased when nearing a waypoint and the vehicle will never miss and backtrack to a waypoint. This method reduced my missed waypoint acceptance to 0 and ensured that my car always advanced.
I first addressed this issue by decreasing vehicle speed from 100% to ~20% when it was 20-ft from the waypoint. This would slow the vehicle enough so that the GPS could get an update within the 10-ft acceptance range. This reduced my missed waypoints and backtracking substantially, but not to 0.
The disadvantage of this method is initial setup of each waypoint takes longer and more forethought. I used a 3 column array (lat, long, modifier) and the modifier was input manually. The modifier instructed the microcontroller if the acceptance criteria are based on latitude (traveling North/South), longitude (East/West), distance (<=10-ft), or a special case. In fact, there are 2 additional cases since latitude is increasing while traveling North, where latitude is decreasing while traveling South. The same holds true for longitude as well.
The advantages of this method are the vehicle speed does not need to be decreased when nearing a waypoint and the vehicle will never miss and backtrack to a waypoint. This method reduced my missed waypoint acceptance to 0 and ensured that my car always advanced.
Several examples of the acceptance lines I selected are marked in yellow in the bird’s eye view of the Sparkfun building. Luckily, the Sparkfun building sides are aligned with North, South, East, and West.
GPS Signal Quality Around Sparkfun
Navigation on the East Side
Additionally, since GPS quality was degraded on the East side, I relied on a form of dead reckoning. Multiple trials showed that my car would often hit the curbs using GPS to determine the next waypoint bearing. I solved this issue by setting a special case in my waypoint acceptance. I instructed the car to follow a predetermined heading once in the narrow section until it passed a predetermine latitude. This kept the car driving straight instead of veering into curbs. This method was effective 75% of the time rather than 25% when relying solely on GPS through the East side.
Testing Method
I approached testing the vehicle and navigation in 2 parts: steering and throttle. I first tested by have the microcontroller only control steering while I controlled throttle. After many trials, my confidence in the navigation system increased until I integrated autonomous throttle control. The multiplexer was amazingly useful during this testing since it enabled me take control / relinquish control instantly. I still crashed and rolled the vehicle many times, but damage was significantly limited by this function. I tested in various parking lots in the area as well as 4 times at the Sparkfun building. That testing was critical to finding and resolving the navigation issues encountered on the East side.
Compass vs gyroscope
I tried to incorporate a gyroscope into my design several times with little luck. Mostly, I think my code was buggy and prevented me from using it. However, I do feel they are more reliable if the bugs can be worked out of the code. Many other teams successfully used them. The one major advantage to using a compass is that if an impact causes sudden rotation of the vehicle (hitting a curb or barrel or hitting or being hit by another vehicle), a gyroscope may lose the correct heading. However, a compass will continue to give the correct heading since it determines heading using magnetic north rather than constant gyro sampling. This came to my attention when my car hit a curb and was spun 180° 2 separate times during the 2nd heat and was able to recover and continue navigating the course. A gyro probably would have missed part of the sudden rate change, causing navigation failure.
Lessons Learned
- Get navigation working early
- Test, test, test
- Test in two parts: 1st autonomous navigation, 2nd autonomous navigation AND throttle
- Get navigation working well, get the throttle incorporated, THEN start working other items like ultrasonic sensors, IR sensors, object avoidance, etc.
- Use the simplest methods for obstacle avoidance…route around the obstacles rather than using sensors to avoid them if possible
- A large LCD is very handy to display realtime data during testing rather than using a laptop cabled to the vehicle
- A multiplexer is very useful in being able to take over control if the car is about to hit something
- Incorporate steering limits so you don’t roll your car or come up with some means to make sure it doesn’t turn too hard (i.e. PVC roll bar)
- Hot glue everything, ziptie everything else
- During the competition...make sure the final waypoint is ~20 feet beyond the finish line so you are sure to finish the race
CODE
A formatted and easy to copy version of the code may be seen here:
http://pastebin.com/6r12WuUW
/* 2011 Sparkfun Autonomous Vehicle Competition Vehicle Code
Author: Richard Burnside
Verion: 1.0
Last Update, 2:00PM: 4/23/2011
*/
//Included Libraries
#include <Wire.h>
#include <Servo.h>
#include <math.h>
#include <TinyGPS.h>
#include <LiquidCrystal.h>
//Sensors & Servors
#define RECEIVER_IN 13 //pin # for receiver IN
#define STROBE 11 //pin # for Strobe
#define SELECT 10 //pin # for Select
#define SERVO 12 //pin # for steering servo
#define THROTTLE 7 // pin # for throttle
Servo steering; // steering servo setup and direction
Servo speed;
bool autonomous = false; // true is ON, false is OFF
bool race_start = true; // creates a 3 second delay to start the vehicle
unsigned long temp_time = 0; // CHANGE THIS SINCE THE SD CARD IS NOT USED ANYMORE
//Compass
#define XCLR 8 // pin # for compass XCLR
#define MCLCK 9 // pin # for compass master clock
#define C_ADDRESS B0110000 // compass address, [0110xx0] [xx] is determined by factory programming, total 4 different addresses are available
#define COMP_X 2041 // compass center X
#define COMP_Y 2040 // compass center Y
double compass_heading = 0;
double angle_diff; // for the compass
//LCD Setup
#define RS 22
#define ENABLE_1 24
#define ENABLE_2 23
#define D4 25
#define D5 26
#define D6 27
#define D7 28
LiquidCrystal lcd1(RS, ENABLE_2, D4, D5, D6, D7);
LiquidCrystal lcd2(RS, ENABLE_1, D4, D5, D6, D7);
String dataString = "";
//GPS Initialization
TinyGPS gps;
float flat, flon;
unsigned long age, date, time, chars;
unsigned short sentences, failed;
int gps_speed, max_speed = 0, course;
double waypoint_distance, waypoint_heading = 0, lat_dist[5] = {0,0,0,0,0}, lon_dist[5] = {0,0,0,0,0};
/* GPS Waypoints */
int waypoint_num = 0;
int special_waypoint_num = 4; // ALWAYS 1 LESS 'cause arrays start at 0, not 1!
const int waypoint_total = 15; // <- should always be the same number of GPS waypoints
// 0 = lat use, 1 = south lat use, 2 = north lat use
double gps_array[15][3] = {{40.0651517950864528, -105.2097273131420, 0}, // 1st corner
{40.0650036900410714, -105.2097272812302, 1}, // 1st line up point
{40.0649066081190785, -105.2097109401257, 1}, // 2nd line up point or start of straight line
{40.06485, -105.20975, 1}, // just added today
{40.064528, -105.2097763511701, 1}, // lat point for start of turn
{40.06448686920148, -105.20985212357533, 0}, // 2nd corner, just inside of south parking lot
{40.06445080617122, -105.21016393037269, 0}, // bottom middle of south parking lot
{40.06449104008855, -105.21041203470733, 0}, // inbetween sidewalks for 3rd corner
{40.06450670940305, -105.2104463610966, 0}, // 3rd corner
{40.06465758628262, -105.21046433778312, 2}, // line up for inbetween next bottleneck
{40.06475817068338, -105.21046970220115, 2}, // past the bottleneck
{40.06493060073908, -105.21048043103721, 2}, // middle of the 3rd side
{40.06514305860027, -105.21048311324623, 2}, // end of 3rd side
{40.06519745591719, -105.21041069360282, 0}, // 4th corner
{40.06518985782108, -105.21001238556421, 0}}; // way past the finish line
void setup() {
//Set the car to manual mode
autonomous = false;
pinMode(RECEIVER_IN, INPUT);
pinMode(STROBE, OUTPUT);
pinMode(SELECT, OUTPUT);
digitalWrite(STROBE, LOW);
digitalWrite(SELECT, HIGH);
steering.attach(SERVO); //Servo Initialization
speed.attach(THROTTLE); //Speed control initialization
//Serial Output Initialization
Serial.begin(115200);
Serial.println("Time,IR Distance,Ultrasonic Distance, Gyro Heading, Lat, Lon, Date, Time, Course, Speed, Failed");
Serial1.begin(4800); //GPS serial port initialization
//Compass Module Initialization
pinMode(XCLR, OUTPUT);
digitalWrite(XCLR, LOW);
pinMode(MCLCK, OUTPUT);
digitalWrite(MCLCK, LOW);
Wire.begin();
delay(20); //give some time (who's in a hurry?)
Wire.beginTransmission(C_ADDRESS); //Send target (master)address
Wire.send(0x00); //Wake up call, send SET signal to set/reset coil
Wire.send(0x02);
Wire.endTransmission(); //wait for SET action to settle
delay(10);
//LCD Setup
lcd1.begin(40, 2);
lcd2.begin(40, 2);
lcd1.clear();
lcd2.clear();
lcd1.setCursor(0, 0); // top left
lcd1.print("THE ARDUINO AUTONOMOUS VEHICLE PROJECT");
lcd2.setCursor(0, 0); // top left
lcd2.print("DESIGNED BY RICHARD BURNSIDE, HIGHLANDS, CO");
delay(500);
lcd1.clear();
lcd2.clear();
delay(500);
}
void loop() {
//GPS data update
bool newdata = false;
if(feedgps())
newdata = true;
if(newdata)
gpsdump(gps);
waypoint();
compass();
set_turn();
set_speed();
if((millis() - temp_time) > 250) {
auto_control(); // check for autonomous mode 4x / second
LCD_data_log();
// Serial_data_log();
temp_time = millis();
}
}
/*----------Functions----------*/
void set_speed(void) { // test this and delete the delays after it works
if(waypoint_distance <= 20) speed.write(112);
if(waypoint_distance > 20) speed.write(150);
return;
}
void waypoint(void) { // Distance and angle to next waypoint
double x, y;
x = 69.1*(gps_array[waypoint_num][1] - flon) * cos(flat/57.3);
y = 69.1*(gps_array[waypoint_num][0] - flat);
waypoint_distance = sqrt(pow(x,2) + pow(y,2))*5280.0; // converts distance to feet
waypoint_heading = atan2(y,x)*180.0/M_PI; // 180/pi converts from rads to degrees
if(waypoint_heading < 0) waypoint_heading += 360.0;
if(waypoint_num == special_waypoint_num) waypoint_heading = 289.0;
if((gps_array[waypoint_num][2] == 1) && (gps_array[waypoint_num][0] > flat)) waypoint_num++; // going south
else if((gps_array[waypoint_num][2] == 2) && (gps_array[waypoint_num][0] < flat)) waypoint_num++; // going north
else if(waypoint_distance < 10) waypoint_num++;
while(waypoint_num >= waypoint_total) { // shuts off the vehicle by disabling autonomous mode & MUXER & sets speed to 0
autonomous = false;
digitalWrite(STROBE, LOW);
digitalWrite(SELECT, LOW);
speed.write(90);
}
return;
}
void set_turn(void) { // Set servo to steer in the direction of the next waypoint
double servo_angle;
angle_diff = compass_heading - waypoint_heading;
if(angle_diff < -180) angle_diff += 360.0;
if(angle_diff >= 180) angle_diff -= 360.0;
servo_angle = angle_diff/2.0 + 90.0; //changes domain from -180...180 to 0...180
servo_angle = 180.0 - servo_angle;
if(servo_angle > 130.0) steering.write(130);
else if(servo_angle < 50.0) steering.write(50);
else steering.write(servo_angle);
return;
}
void compass(void) { // Determine compass heading with 0-deg matching standard graph
int xParam, yParam;
double x, y, northFi;
byte rcvByte[4];
Wire.beginTransmission(C_ADDRESS); //Send target (master)address
Wire.send(0x00); //Wake up call, request for data
Wire.send(0x01);
Wire.endTransmission();
delay(7); //wait 5ms min for compass to acquire data
Wire.requestFrom(C_ADDRESS, 4);
for(int i=0; i < 4; i++) {
rcvByte[i] = 0;
rcvByte[i] = Wire.receive();
}
xParam = rcvByte[0] << 8;
xParam = xParam | rcvByte[1];
yParam = rcvByte[2] << 8;
yParam = yParam | rcvByte[3];
x = -(xParam - COMP_X);
y = (yParam - COMP_Y);
northFi = atan2(y,x);
compass_heading = (northFi * 180.0) / M_PI + 90.0 - 10.0; // 9 is to correct for magnetic north
if(compass_heading < 0) compass_heading += 360.0;
return;
}
void auto_control(void) { // Autonomous Mode
int pulse_length;
pulse_length = pulseIn(RECEIVER_IN, HIGH, 50000);
Serial.println(pulse_length);
if(pulse_length < 500) {
if(race_start) {
delay(250);
race_start = false;
}
autonomous = true;
digitalWrite(STROBE, LOW);
digitalWrite(SELECT, LOW);
}
else {
autonomous = false;
digitalWrite(STROBE, LOW);
digitalWrite(SELECT, HIGH);
}
return;
}
bool feedgps() { // GPS Data Input
while(Serial1.available()) {
if(gps.encode(Serial1.read()))
return true;
}
return false;
}
void gpsdump(TinyGPS &gps) { // GPS Calculation
gps.f_get_position(&flat, &flon, &age);
gps.get_datetime(&date, &time, &age);
gps.stats(&chars, &sentences, &failed); //should be removed later...just a waste of time
gps_speed = gps.f_speed_mph(); //not needed also
if(gps_speed > max_speed) max_speed = gps_speed;
course = gps.course(); //could be useful once i figure out what it does
return;
}
void Serial_data_log() { // Serial Data Logging
//String() will not handle float/doubles...this will convert these to arrays
long latitude, longitude, wp_distance, wp_heading, latitude_array, longitude_array;
latitude = flat*1000000;
longitude = flon*1000000;
latitude_array = gps_array[waypoint_num][0]*1000000;
longitude_array = gps_array[waypoint_num][1]*1000000;
wp_distance = waypoint_distance;
wp_heading = waypoint_heading;
long angle_string;
//String creation
dataString += String(millis());
dataString += ","; dataString += String(time);
dataString += ","; dataString += String(gps_speed);
dataString += ","; dataString += String(angle);
dataString += ","; dataString += String(angle_string);
dataString += ","; dataString += String(latitude);
dataString += ","; dataString += String(longitude);
dataString += ","; dataString += String(latitude_array);
dataString += ","; dataString += String(longitude_array);
dataString += ","; dataString += String(wp_distance);
dataString += ","; dataString += String(wp_heading);
dataString += ","; dataString += String(failed);
if(autonomous)
dataString += ",Auto";
else
dataString += ",Manual";
Serial.println(compass_heading,1);
Serial.println(waypoint_heading,1);
// servo_angle is not a global variable Serial.println(servo_angle,1);
Serial.println(angle_diff,1);
//Serial printing of string
Serial.println(dataString);
dataString = ""; //resets the datastring to 0
return;
}
void LCD_data_log() { // LCD Data Logging
lcd1.clear();
lcd1.setCursor(0,0); lcd1.print("Lt/Ln: "); lcd1.print(flat,6); lcd1.print(" "); lcd1.print(flon,6);
lcd1.setCursor(29,0); lcd1.print("Dft: "); lcd1.print(waypoint_distance,1);
lcd1.setCursor(0,1); lcd1.print("NWP: "); lcd1.print(gps_array[waypoint_num][0],6); lcd1.print(" "); lcd1.print(gps_array[waypoint_num][1],6);
lcd1.setCursor(29,1); lcd1.print("Spd: "); lcd1.print(max_speed);
lcd1.setCursor(36,1); lcd1.print("M: "); lcd1.print(autonomous);
lcd2.clear();
lcd2.setCursor(20,0); lcd2.print("Tm: "); lcd2.print(millis()/1000);
lcd2.setCursor(0,1); lcd2.print("CM: "); lcd2.print(compass_heading,1);
lcd2.setCursor(10,1); lcd2.print("Hdng: "); lcd2.print(waypoint_heading,1);
lcd2.setCursor(22,1); lcd2.print("Diff: "); lcd2.print(angle_diff,1);
lcd2.setCursor(33,1); lcd2.print("WP: "); lcd2.print(waypoint_num);
return;
}
