Hi, I keep fighting with my boat model to keep it flowing straight to my destination. The boat is steerable by two motors.
I just started with Arduino, please forgive me for my banal questions and dilemmas.
And now I have a few questions:
- GPS, would you add or correct something in the gpsdata (), gpsheading () section?
- Compass - I've read a bit about Auto Calibration but it's too hard for me to do. Do you have to calibrate the magnetometer every time? It's about pointing me in the direction. And if you would change anything in the headingcal () section
Here is also the "Declination Angle" can it be calculated automatically ?? - The last steering of the motors, steering () is it ok, would you change something?
Thank you in advance for your help
static void smartDelay(unsigned long ms)
{
unsigned long start = millis();
do
{
while (GpsSerial.available())
gps.encode(GpsSerial.read());
} while (millis() - start < ms);
}
//GPS:-
void gpsdata()
{
smartDelay(1000);
unsigned long start;
double lat_val, lng_val, alt_m_val, speedkmph;
bool loc_valid, alt_valid;
lat_val = gps.location.lat();
loc_valid = gps.location.isValid();
lng_val = gps.location.lng();
alt_m_val = gps.altitude.meters();
alt_valid = gps.altitude.isValid();
speedkmph= gps.speed.kmph();
if (!loc_valid)
{
Serial.print("Latitude : ");
Serial.println("*****");
Serial.print("Longitude : ");
Serial.println("*****");
delay(100);
}
else
{
Serial.println("GPS READING: ");
//DegMinSec(lat_val);
Serial.print("Latitude in Decimal Degrees : ");
Serial.println(lat_val, 6);
//DegMinSec(lng_val);
Serial.print("Longitude in Decimal Degrees : ");
Serial.println(lng_val, 6);
delay(100);
}
IBusSensor.setSensorMeasurement(1,speedBoat);
speedBoat = speedkmph;
latc=lat_val;
logc=lng_val;
distance();
}
//GPS Heading:-
void gpsheading()
{
float x,y,deltalog,deltalat;
deltalog= logd-logc;
deltalat=latd-latc;
x=cos(latd)*sin(deltalog);
y=(cos(latc)*sin(latd))-(sin(latc)*cos(latd)*cos(deltalog));
bearing=(atan2(x,y))*(180/3.14);
Serial.print("bearing");
Serial.println(bearing);
float a,d,c;
a=(((sin(deltalat/2)))*(sin(deltalat/2))) + ((cos(latc))*(cos(latd))* (((sin(deltalog/2)))*(sin(deltalog/2))) );
c=2*(atan2(sqrt(a),sqrt(1-a)));
d=6371*c;
Serial.println(d);
}
//Heading:-
void headingcal()
{
int x,y,z;
qmc.read(&x,&y,&z);
delay(100);
// You must then add your 'Declination Angle' to the compass, which is the 'Error' of the magnetic field in your location.
// Find yours here: http://www.magnetic-declination.com/
heading=atan2(x, y)/0.0937241808;
if(heading < 0)
{
heading+=360;
}
heading=360-heading; // N=0/360, E=90, S=180, W=270
Serial.print("heading: ");
Serial.println(heading);
}
//Motor:-
void steering()
{
float finalv;
finalv=heading/bearing;
Serial.print("finalv: ");
Serial.println(finalv);
if(finalv>=0&&finalv<=1)
{
forward();
}
else if(finalv >1 && finalv <=8)
{
left();
}
else if(finalv <=13 && finalv >=8)
{
right();
}
else if(logd==logc && latc==latd)
{
stop1();
}
}
void forward()
{
// Set Motor A forward
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
// Set Motor B forward
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
}
void right()
{
digitalWrite(in1,HIGH);
digitalWrite(in2,LOW);
digitalWrite(in3,LOW);
digitalWrite(in4,HIGH);
}
void left()
{
digitalWrite(in1,LOW);
digitalWrite(in2,HIGH);
digitalWrite(in3,HIGH);
digitalWrite(in4,LOW);
}
void back()
{
digitalWrite(in1,HIGH);
digitalWrite(in2,LOW);
digitalWrite(in3,HIGH);
digitalWrite(in4,LOW);
}
void stop1()
{
digitalWrite(in1,LOW);
digitalWrite(in2,LOW);
digitalWrite(in3,LOW);
digitalWrite(in4,LOW);
}
void distance()
{
const double LAKE_LAT = 50.391551;
const double LAKE_LNG = 18.451354;
distanceM = gps.distanceBetween(gps.location.lat(), gps.location.lng(), LAKE_LAT, LAKE_LNG);
Serial.print("Distance (m) to Point: ");
Serial.println(distanceM);
IBusSensor.setSensorMeasurement(2,distanceM);
}
