AGV_test_turn2.pde (10570Bytes)
stripped_down_gps.pde (2476Bytes)
gps_with_distance.pde (3648Bytes)
better_setup_code.pde (12104Bytes)
Old_servo_library.zip (8451Bytes)
arduino_agvfinal2345_0.brd (20298Bytes)
So you are interested in building a GPS guided vehicle?
Well this is not for the beginner. This walk through does not hold your hand. I am expecting that you know things about motor drivers, micro controllers, and general electronics. Its a lot i have to cover and so some things i must cover more than others that you should already know.
Also read:
- Complete GPS guide (by gareth)
- My AGV post
UPDATE 07.03.10
You will notice there are now 2 setups. A "basic" setup and a "better" setup. They both will accomplish the same thing.
What would be needed: (short list basic setup)
- GPS
- Compass
- Vehicle ( you might want one that can drive on grass and cement)
- Micro Controller (in this case a Arduino. Get one with a 328)
- Motor controller (like a l298N for small vehicles)
What would be needed: (short list better setup)
Below are directions for the BASIC. If you want the better setup then scroll down further. I may refer back and forth to one or the other directions so i do not repeat information twice. I suggest if you really plan on building a AGV then read through both directions.
Here is a arduino reference picture:
GPS:
In this walkthrough we will be using the EM406A. The GPS runs on 5v. I suggest the 1 foot long GPS cable and some male headers to plug the GPS into your Arduino. We will be using the datasheet to wire the GPS up to the Arduino. Attach pin 1 to gnd, pin 2 to 5v, pin 3 to arduino TX (pin 3 of arduino), pin 4 to RX (pin 2 of arduino). NOTE Pin 1 IS NOT THE GREY WIRE ON THE GPS CABLE. Pin 1 is the first black wire on the right of the GPS cable.
Now with the GPS wired up, we are ready to test it. First download the tinygps library from http://arduiniana.org/libraries/tinygps/ and newsoftserial from http://arduiniana.org/libraries/newsoftserial/and unzip it into the libraries folder of the arduino file. The newsoftserial library allows you to use other pins for TX and RX instead of using pin 0 and 1 on the arduino. Now open up the example program. Go to file>examples>tinygps>test_with_gps_device. Note, that the code has the GPS set to RX and TX on pin 2 and 3. Also the baud rate is set for 115200. Make sure your serial monitor is set to this baud when watching the data stream.
NewSoftSerial nss(2, 3);
Upload the example to the arduino now. When the GPS red LED starts to blink, then the GPS has a lock and will start to serial print data to your computer. Watch the data on the serial monitor and you should easily be able to interpret the data.
Compass:
Ok so now we have GPS data right? hopefully you do. That is one part of a GPS guided vehicle. After location is heading. You need to know which way you are facing so when you calculate the needed heading you can easily turn to face your waypoint and drive relatively straight there. This is where the compass comes into play. "But wait, doesnt the GPS give a heading value?" Yes it does but only when moving forward and it isnt very accurate. "Well why does that matter?" it matters because when your vehicle is turning, it is not moving forward very much at all, so you can not get a good heading reading and you will not know how much to turn. So we are going to use the compass to give us a heading from 0-360 degrees. We will also use a formula that gives us the needed heading between the vehicle's location and the next waypoint location. The formula will give us a heading from 0-360 degrees. So all we have to do is match our heading with the needed heading and drive until we reach the GPS point. More on the formulas later in the post.
Lets hook up the compass now.
The gnd on the compass should be connected to ground. VCC should be connect to the 3.3v pin on the arduino (pin labeled 3V3 on arduino). It can be supplied with 2.7-5.2v but its easy to just use the 3.3v pin on the arduino.
The communication with this is done via I2C. SDA gets connected to analog pin 4 of the arduino and SCL gets connected to analog pin 5 of the arduino. These are the pins that will be used for I2C communication in the code. Mount the compass away from other electronics.
Look below. Compass is mounted in the air away from the other electronics. Mount it flat because it does not have tilt compensation and will give readings that are a little off if tilted.
Now lets upload some practice code. http://lusorobotica.com/ficheiros/bussola_codigo_1.pde. Yes it is in portuguese. Change this line in the code
//reading/=10;
to
reading/=10;
so we get the value in the degrees we want. Now open up serial monitor and choose 115200 baud and watch the data. Does it work?
Vehicle:
This part is up to you. I bought a cheep hummer from a store that ran on 4 AA batteries. I chose it because of the 6v it ran on which i am familiar with. But the one problem with this vehicle is it is terrible on grass or up hills. In a parking lot though it does fine. So if you are looking for a all terrain GPS guided vehicle, then buy or make a all terrain vehicle. If you buy one then buy something that you can easily hack. Mine was very easy to hack, it had the motor wires exposed when the shell was taken off. The hummer steers with a motor that turns a little bit left or right depending on your “high” “low” commands. The link to the hummer in the parts list is just one i found online that looks like mine. I am not sure how easy it is to hack or how good it runs. It is familiar to mine because it is made from the same company and runs on the same voltage. The motors in your vehicle determines the next part which is the motor driver.
Motor driver:
For my vehicle i used a L298N. Of course you need to follow a schematic to get it working. There are dozens online or just follow what the datasheet says. Or if your lazy, you can just buy this shield. Of course this is only relevant if your vehicle’s stall current is less than 2 amps. If it is higher than 2 amps then you will need to get a motor driver that can supply that. But if you are getting a small toy rc truck or car then the L298N should be good.
Ok so where are we at? GPS is set up and gives us our location. Compass is set up and gives us our current heading. The vehicle should move now with a motor driver controlled by the arduino. So now we need to program the thing to drive to waypoints. Now here comes the fun :D.
Programming:
I have attach my code to this page. You are free to look at it and try it out. But i can not say for certain that it will work for you. That is why i am going to explain how to write the program your self. That means distance calculations, bearing calculations, steering calculations, and integrating this all together in a program that reads data, interprets and calculates what is needed, and executes the needed movements.
First lets start by copying and pasting the example code for the GPS into a new arduino sketch. We are going to clean up the code and get rid of the stuff we dont want. I attached a file to this page called stripped_down_gps. This code gets rid of everything not needed for longitude and latitude. Thats all we need. The code is also sped up to update every 1/4 of a second instead of every 5 seconds. This sketch will be the basis of our code, we will start to build on that now.
After this is done. I went and added a new function called “distance”. You can call it what ever you want but to make it easier, just call your function “distance” too. So after
bool feedgps()
{
while (nss.available())
{
if (gps.encode(nss.read()))
return true;
}
return false;
}
add in
void distance(){ //this function will do all the calculations
}
“what is the function “distance” going to be used for?” It is going to be used to calculate distance (thats how it got its name), bearing, reading the compass, and switching in between waypoints.
Lets start with adding the some of the compass code to our stripped down gps code:
#include <NewSoftSerial.h>
#include <TinyGPS.h>
#include <Wire.h>
int HMC6352Address = 0x42;
int slaveAddress;
byte headingData[2];
int i, headingValue
int headingcompass;
TinyGPS gps;
NewSoftSerial nss(2, 3);
void gpsdump(TinyGPS &gps);
bool feedgps();
void printFloat(double f, int digits = 2);
void setup()
{
Serial.begin(115200);
nss.begin(4800);
slaveAddress = HMC6352Address >> 1;
Wire.begin();
}
So does your distance calculations work?
Next is calculating the bearing in between the 2 points:
Add this to your “distance” function. Add it right below the the other code in the function
You will need to add float heading; to the top of your code.
flon1 = radians(flon1); //also must be done in radians
x2lon = radians(x2lon); //radians duh.
heading = atan2(sin(x2lon-flon1)*cos(x2lat),cos(flat1)*sin(x2lat)-sin(flat1)cos(x2lat)cos(x2lon-flon1)),23.1415926535;
heading = heading180/3.1415926535; // convert from radians to degrees
int head =heading; //make it a integer now
if(head<0){
heading+=360; //if the heading is negative then add 360 to make it positive
}
Serial.println(“heading:”);
Serial.println(heading); // print the heading.
Do you have a heading now? Yes? Good. No? Uh oh.
Ok so we have distance and bearing. Lets read our compass next so we can drive to our waypoint. This now goes into the bottom of “distance” function.
Wire.beginTransmission(slaveAddress); //the wire stuff is for the compass module
Wire.send(“A”);
Wire.endTransmission();
delay(10);
Wire.requestFrom(slaveAddress, 2);
i = 0;
while(Wire.available() && i < 2)
{
headingData[i] = Wire.receive();
i++;
}
headingValue = headingData[0]*256 + headingData[1];
int pracheading = headingValue / 10; // this is the heading of the compass
if(pracheading>0){
headingcompass=pracheading;
}
Serial.println(“current heading:”);
Serial.println(headingcompass);
So that read our data from the compass.
Next we have to compare our heading from the compass with the bearing from the calculations. “why do we compare the values?” We compare them so we know if we need to turn left or right. We do not want to turn left 270 degrees when we could have turned right 90 degrees. So lets compare the values. This is what i came up with. This goes next in the “distance” function.
x4=headingcompass-heading; //getting the difference of our current heading to our needed bearing
int turn;
//-------------------------------------- below tells us which way we need to turn
if(x4>=-180){
if(x4<=0){
turn=8; //set turn =8 which means “right”
}
}
if(x4<-180){
turn=5; //set turn = 5 which means “left”
}
if(x4>=0){
if(x4<180){
turn=5; //set turn = 5 which means “left”
}
}
if(x4>=180){ //set turn =8 which means “right”
turn=8;
}
int hd = headingcompass;
if(hd==heading){
turn=3; //then set turn = 3 meaning go “straight”
}
Now you know which way to turn
Now execute it
if(turn==3){
Serial.println(“straight”);
digitalWrite(mo1, LOW); //go straight
digitalWrite(mo2, HIGH);
digitalWrite(mo3, LOW);
digitalWrite(mo4, LOW);
}
//-------------------------------------------------------------------------------------turn right
if(turn==8){
rightturn();
}
//------------------------------------------------------------------------------------------turn left
if(turn==5){
leftturn();
}
Here is a example of how to control how much to turn. This is my function for turning right. It reads the data from the compass and stays in this loop until it has turned enough to come within 2 degrees of the needed bearing.
void rightturn(){
if(headingcompass+2>heading){
if(headingcompass-2<heading){
digitalWrite(mo3, LOW);
digitalWrite(mo4, LOW);
return;
}
}
x4=headingcompass-heading;
if(x4<-180){
return; //check to see if we have turned too much and now need to go left
}
if(x4>=0){
if(x4<180){
return; //check to see if we have turned too much and now need to go left
}
}
digitalWrite(mo1, LOW); //motor turning code
digitalWrite(mo2, HIGH);
digitalWrite(mo3, HIGH);
digitalWrite(mo4, LOW);
Wire.beginTransmission(slaveAddress); //the wire stuff is for the compass module
Wire.send(“A”);
Wire.endTransmission();
delay(10);
Wire.requestFrom(slaveAddress, 2);
i = 0;
while(Wire.available() && i < 2)
{
headingData[i] = Wire.receive();
i++;
}
headingValue = headingData[0]*256 + headingData[1];
headingcompass = headingValue / 10; // this is the heading of the compass
rightturn();
}
Back to “distance” function. Lets set our radius so we stop or switch to our next gps point when we are close to our waypoint.
Serial.println(“distance”);
Serial.println(dist_calc);
if(dist_calc<4){ //this goes after the serial print of the distance in the function “distance”. refer to my code. 4=radius
if(waycont==waypoints){ //this has to do with switching waypoints
done(); //go to the function to stop motors
}
waycont+=1;
}
For switching waypoints make multiple waypoints in your code
int waycont=1;
float flat2=28.34140014; //waypoint 1
float flon2=-82.27369689;
float flat3=28.3415508; //waypoint 2
float flon3=-82.2736816;
float flat4=28.3415794; //waypoint 3
float flon4=-82.2741317;
float flat5=28.3414192; // waypoint 4
float flon5=-82.2741470;
float flat6 = 28.3413600;
float flon6 =-82.2739028;
and then add the below to the top of “distance” function.
if(waycont==1){
x2lat = flat2; // setting x2lat and x2lon equal to our first waypoint
x2lon = flon2;
}
if(waycont==2){
x2lat = flat3;
x2lon = flon3;
}
if(waycont==3){
x2lat = flat4;
x2lon = flon4;
}
if(waycont==4){
x2lat = flat5;
x2lon = flon5;
}
if(waycont==5){
x2lat = flat6;
x2lon = flon6;
}
Done? You should be. I am. Enjoy.
Below are directions for the Better setup. A lot of these directions are going to be referring to the directions above. Download the “better_setup_code” above.
The setup:
This setup is a little bit different. Instead of using a motor driver, we are using a vehicle and its built int ESC and servo. This setup is a little more costly, you will have to buy a vehicle that has a ESC and servo, not a cheap toy from the local toy store. This setup will allow you to control your speed and should be a more sturdy vehicle all together. A ESC stands for Electronic Speed control. Your micro controller can control this by sending it servo signals.
Here is what your ESC might look like:
You will notice it has a plug for your battery, motor, and a 3 pin plug that looks like a servo cable. The white wire is the signal wire, the black is GND and the red is the regulated or unregulated output from the ESC. Yes the ESC might or might not regulate the power and if it does regulate the power, it might not regulate it enough. Mostly likely it is regulating your batteries power because those black and red wires are what powers the RC receiver. But does it regulate it enough? Check the voltage and see if it is around 5v. If so then you can use this to power your arduino, if not then you might have to purchase a UBEC to regulate the power from your battery down to 5v. In my case, using my custom board, i had to regulate the power because the compass can only take 5.2v and the ESC was putting out 5.7v. If you are using a Arduino board with a 3.3v regulator built in then power the compass with 3.3v. Refer to the basic directions above on how to do that.
To use servos with the newsoftserial library must download the old servo library and use it instead of the new servo library. And the old servo library will only drive 2 servos on pins 9 and 10. My custom board uses these pins to drive the ESC and servo. Using a normal arduino, connect the servo signal wire to pin 9 and the ESC to pin 10.
This is how i wired up the the ESC and servo
First of all i had to use a UBEC, so i cut into where the ESC plugs into the battery and soldered my UBEC onto there to regulate the power to power my custom board.
There are a lot of different ways to do what i did below. This is how it needs to be done for my custom board but if you are using any arduino board you could do it any way you want, basically we are powering the servo with the ESC output because that is what normally happens when you plug them into the receiver, we are just distributing the power.
Now take the end of the ESC cable and servo cable and cut the red wire
solder the ESC red wire to the servo red wire
If you are powering your Arduino with this (ie., you checked it with your multimeter and it is around 5v) then you can add another connection to power your Arduino, in my case remember i am powering my custom board through the UBEC.
Once you have done this, then connect the grounds, connect the ground to the Arduino and other components. I really do not need to tell you this you understand by now.
Download the old servo library ( i attached the file on this page)
Now test out your servo. You know how to do this also i am sure.
The ESC works a little different. It takes a certain pulse to activate it. You must determine what this pulse is. Mine was esc.write(80);. Mine ESC beeps when it isnt activated. What ever yours is, put this in the setup of your code to activate the ESC. Then anything higher than that should make it drive faster forward, and anything less should make it go backwards. I say anything, but its anything within bounds. And for me, it wouldnt drive forward till around esc.write(95); and it wont go backwards for me until esc.write(65);. You must figure these things out.
For my custom board you plug in the GPS into the SMT connector and the servo and ESC cable to the labeled spots and plug the compass wires into the labeled compass spot. The red wire goes in the middle. Remember if you are using my custom board, then you should have cut the red wires off of the connector. The UBEC powers the board where it says "BAT"
Refer to the basic directions for setting up the compass and the GPS. If you are using my custom board then your GPS rx is pin 8 and GPS tx is pin 11.
In the end, my vehicle was like thise
Time to edit the basic code so it works for our better setup. I have downloaded my latest code for this setup but it still needs a lot more work. If you ever make one and would like to contribute to the code, feel free to send me a message.
Below are just the edits and editions to the new code, not the full code.
The first thing in the new code is to include the servo library at the top of your sketch.
#include <Servo.h>
Servo servo; //this is the servo for turning
Servo esc; //the esc obviously
#define escspeed 110 //this is the speed you will drive, choose what you want
then in the setup
void setup(){
esc.attach(10); //esc is attached to pin 10
servo.attach(9); // servo is attached to pin 9
esc.write(80); //this is the value that will arm your ESC, mine was 80, yours might be different and probably is.
delay(1000);
}
in void leftturn and void right turn goes your servo value for turning those directions.
void leftturn(){
if(headinggps+2>heading){
if(headinggps-2<heading){
servo.write(90); //straighten the wheels if we are heading to the waypoint
delay(60);
return;
}
}
x4=headinggps-heading;
if(x4>=-180){
if(x4<=0){
return;
}
}
if(x4>=180){
return;
}
servo.write(110); //turn the wheels left
esc.write(speedesc); //our speed
Wire.beginTransmission(slaveAddress); //the wire stuff is for the compass module
Wire.send("A");
Wire.endTransmission();
delay(10);
Wire.requestFrom(slaveAddress, 2);
i = 0;
while(Wire.available() && i < 2)
{
headingData[i] = Wire.receive();
i++;
}
headingValue = headingData[0]*256 + headingData[1];
headinggps = headingValue / 10; // this is the heading of the compass
leftturn();
}
That is basically a some up of the what is new the better setup. Remember this is a guide that should help you write your own code, i by no way guarantee that this code will work for you or am not liable for any accidents or malfunctions caused by this setup or code. I hope this has been helpful.
