Hello there, higher level genius people,
I am having problem here with the coding.
My Objective is to move UGV auto based on two waypoints by using compass HMC5883L
and GPS SKM53. Other than that im using motor sheild L298P.
First i am try move both motor using shield L298P,
here’s the code
// This motor shield use Pin 4,5,6,7, to control the motor
// Through serial monitor, type 'w','s','a','d','o' to control the motor directions
int E1 = 6;
int M1 = 7;
int E2 = 5;
int M2 = 4;
void Motor1(int pwm, boolean reverse)
{
analogWrite(E1,pwm); //set pwm control, 0 for stop, and 255 for maximum speed
if(reverse)
{
digitalWrite(M1,HIGH);
}
else
{
digitalWrite(M1,LOW);
}
}
void Motor2(int pwm, boolean reverse)
{
analogWrite(E2,pwm);
if(reverse)
{
digitalWrite(M2,HIGH);
}
else
{
digitalWrite(M2,LOW);
}
}
void setup()
{
int i;
for(i=4;i<=7;i++) //For Arduino Motor Shield,set pin 4,5,6,7 to output mode
pinMode(i, OUTPUT);
Serial.begin(9600);
}
void loop()
{
int x,delay_en;
char val;
while(1)
{
val = Serial.read();
if(val!=-1)
{
switch(val)
{
case 'w': //move ahead
Motor1(150,true);
Motor2(150,true);
break;
case 's': //Move back
Motor1(150,false);
Motor2(150,false);
break;
case 'a': //turn left
Motor1(150,false);
Motor2(150,true);
break;
case 'd': //turn right
Motor1(150,true);
Motor2(150,false);
break;
case 'o': //stop
Motor1(0,false);
Motor2(0,false);
break;
}
}
}
}
its work perfectly,
2nd step i setup my compass HMC5883L alone to get the degree (when rotate)
here the code :
#include <Wire.h>
#include <HMC5883L.h>
HMC5883L compass;
int error = 0;
void setup()
{
Serial.begin(9600);
Serial.println("Serial started.");
Wire.begin();
Serial.println("Constructing new HMC5883L");
compass = HMC5883L();
Serial.println("Setting scale to +/- 1.3 Ga");
int error = compass.SetScale(1.3); // Set the scale of the compass.
if(error != 0) // If there is an error, print it out.
Serial.println(compass.GetErrorText(error));
Serial.println("Setting measurement mode to continuous.");
error = compass.SetMeasurementMode(Measurement_Continuous); // Set the measurement mode to Continuous
if(error != 0) // If there is an error, print it out.
Serial.println(compass.GetErrorText(error));
}
void loop()
{
MagnetometerRaw raw = compass.ReadRawAxis();
MagnetometerScaled scaled = compass.ReadScaledAxis();
int MilliGauss_OnThe_XAxis = scaled.XAxis;
float heading = atan2(raw.YAxis, raw.XAxis);
int xAxis = raw.XAxis;
int yAxis = raw.YAxis;
int zAxis = raw.ZAxis;
if(heading < 0)
heading += 2*PI;
float headingDegrees = heading * 180/M_PI;
Output(raw, scaled, heading, headingDegrees);
}
// last ouput
void Output(MagnetometerRaw raw, MagnetometerScaled scaled, float heading, float headingDegrees)
{
Serial.print("Raw:\t");
Serial.print(raw.XAxis);
Serial.print(" ");
Serial.print(raw.YAxis);
Serial.print(" ");
Serial.print(raw.ZAxis);
Serial.print(" \tScaled:\t");
Serial.print(scaled.XAxis);
Serial.print(" ");
Serial.print(scaled.YAxis);
Serial.print(" ");
Serial.print(scaled.ZAxis);
Serial.print(" \tHeading:\t");
Serial.print(heading);
Serial.print(" Radians \t");
Serial.print(headingDegrees);
Serial.println(" Degrees \t");
}
Its show the degree that i want,
3rd step : i setup GPS SKM53
the code are
/*
This Sketch will run with the SkyNav SKM53 GPS
RXD Arduino Pin 3
TXD Arduino Pin 2
RST Leave Open
NC Leave Open
GND Ground
VCC +5
Make sure you download and save to your Arduino/Libraries folder TinyGPS.h
and NewSoftSerial.h files.
*/
#include <TinyGPS.h>
#include <SoftwareSerial.h>
unsigned long fix_age;
SoftwareSerial GPS(2,3);
TinyGPS gps;
void gpsdump(TinyGPS &gps);
bool feedgps();
void getGPS();
long lat, lon;
float LAT, LON;
void setup(){
GPS.begin(9600);
Serial.begin(9600);
}
void loop(){
long lat, lon;
unsigned long fix_age, time, date, speed, course;
unsigned long chars;
unsigned short sentences, failed_checksum;
// retrieves +/- lat/long in 100000ths of a degree
gps.get_position(&lat, &lon, &fix_age);
getGPS();
Serial.print("Latitude : ");
Serial.print(LAT/100000,7);
Serial.print(" :: Longitude : ");
Serial.println(LON/100000,7);
}
void getGPS(){
bool newdata = false;
unsigned long start = millis();
// Every 1 seconds we print an update
while (millis() - start < 1000)
{
if (feedgps ()){
newdata = true;
}
}
if (newdata)
{
gpsdump(gps);
}
}
bool feedgps(){
while (GPS.available())
{
if (gps.encode(GPS.read()))
return true;
}
return 0;
}
void gpsdump(TinyGPS &gps)
{
//byte month, day, hour, minute, second, hundredths;
gps.get_position(&lat, &lon);
LAT = lat;
LON = lon;
{
feedgps(); // If we don't feed the gps during this long routine, we may drop characters and get checksum errors
}
}
4th step: i should be able to calculate the bearing and heading from the compass and compare the waypoints then
should make UGV moving.
Here the code :
/*
This Sketch will run with the SkyNav SKM53 GPS
RXD Arduino Pin 3
TXD Arduino Pin 2
RST Leave Open
NC Leave Open
GND Ground
VCC +5
Make sure you download and save to your Arduino/Libraries folder TinyGPS.h
and NewSoftSerial.h files.
*/
#include <TinyGPS.h>
#include <SoftwareSerial.h>
unsigned long fix_age;
SoftwareSerial GPS(2,3);
TinyGPS gps;
void gpsdump(TinyGPS &gps);
bool feedgps();
void getGPS();
long lat, lon;
float LAT, LON;
void setup(){
GPS.begin(9600);
Serial.begin(9600);
}
void loop(){
long lat, lon;
unsigned long fix_age, time, date, speed, course;
unsigned long chars;
unsigned short sentences, failed_checksum;
// retrieves +/- lat/long in 100000ths of a degree
gps.get_position(&lat, &lon, &fix_age);
getGPS();
Serial.print("Latitude : ");
Serial.print(LAT/100000,7);
Serial.print(" :: Longitude : ");
Serial.println(LON/100000,7);
}
void getGPS(){
bool newdata = false;
unsigned long start = millis();
// Every 1 seconds we print an update
while (millis() - start < 1000)
{
if (feedgps ()){
newdata = true;
}
}
if (newdata)
{
gpsdump(gps);
}
}
bool feedgps(){
while (GPS.available())
{
if (gps.encode(GPS.read()))
return true;
}
return 0;
}
void gpsdump(TinyGPS &gps)
{
//byte month, day, hour, minute, second, hundredths;
gps.get_position(&lat, &lon);
LAT = lat;
LON = lon;
{
feedgps(); // If we don't feed the gps during this long routine, we may drop characters and get checksum errors
}
}
But its failed, n wont moving. I didnt know where the fault.