As the topic said i'm controlling Car acceleration with 2 servo attached with accelerators
but it doesn't seem to be moving can somebody take a look please
im using Sparkfun Venus GPS and GY-87 and when i tested them individually they seemed to be working fine though the gps isn't exactly accurate. But when i tried to use location from gps to use with compass they aren't working? BTW sorry for hidden code in different language i put them with // to mark something for me to remember.
#include <MPU6050.h>
#include <SoftwareSerial.h>
#include <HMC5883L.h>
#include <TinyGPS.h>
#include <Wire.h>
#include <Servo.h>
SoftwareSerial gpsSerial(10, 11);
float i, headingValue,diff;
float flat, flon;
float heading = 0;
int headinggps = 0;
float distance = 0;
HMC5883L compass;
MPU6050 mpu;
float flat2 = 13.967572;
float flon2 = 100.586989;
float flat3 = 13.967823;
float flon3 = 100.58751;
float flat4 = 13.966553;
float flon4 = 100.587923;
TinyGPS gps;
int radius, x4=0;
#define waypoints 3
int waycont=1;
float x2lat;
float x2lon;
float head;
int pos = 0;
Servo ServoL, ServoR;
void Forward(){
ServoL.write(20);
delay(150);
ServoR.write(20);
delay(150);
}
void Stop(){
ServoL.write(0);
delay(100);
ServoR.write(0);
delay(100);
}
void Right(){
ServoL.write(10);
delay(100);
ServoR.write(0);
delay(100);
}
void Left(){
ServoL.write(0);
delay(100);
ServoR.write(10);
delay(100);
}
void setup() {
ServoL.attach(8);
ServoR.attach(9);
Serial.begin(9600);
gpsSerial.begin(9600);
Serial.println("Initialize HMC5883L");
while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G))
{
Serial.println("Could not find a valid MPU6050 sensor, check wiring!");
delay(500);
}
mpu.setI2CMasterModeEnabled(false);
mpu.setI2CBypassEnabled(true) ;
mpu.setSleepEnabled(false);
// Initialize Initialize HMC5883L
Serial.println("Initialize HMC5883L");
while (!compass.begin())
{
Serial.println("Could not find a valid HMC5883L sensor, check wiring!");
delay(500);
}
// Set measurement range
compass.setRange(HMC5883L_RANGE_1_3GA);
// Set measurement mode
compass.setMeasurementMode(HMC5883L_CONTINOUS);
// Set data rate
compass.setDataRate(HMC5883L_DATARATE_30HZ);
// Set number of samples averaged
compass.setSamples(HMC5883L_SAMPLES_8);
// Set calibration offset. See HMC5883L_calibration.ino
compass.setOffset(0, 0);
}
bool newdata()
{
while (gpsSerial.available())
{
if (gps.encode(gpsSerial.read()))
return true;
}
return false;
}
void loop() {
if(waycont==1)
{
x2lat = flat2; // เป้าหมาย 1
x2lon = flon2;
}
if(waycont==2)
{
x2lat = flat3; // เป้าหมาย 2
x2lon = flon3;
}
if(waycont==3)
{
x2lat = flat4; // เป้าหมาย 3
x2lon = flon4;
}
//ระยะทาง
gps.f_get_position(&flat, &flon);
float diflat=0;
float diflon=0;
float dist_calc=0;
float dist_calc2=0;
float Int_Dist=0;
float Final_Dist=0;
diflat = radians(x2lat-flat);
flat = radians(flat);
diflon = radians(x2lon-flon);
x2lat = radians(x2lat);
dist_calc = (sin(diflat/2)*sin(diflat/2));
dist_calc2 = (cos(flat)*cos(x2lat)*sin(diflon/2)*sin(diflon/2));
Int_Dist = dist_calc + dist_calc2;
Final_Dist = (2*atan2(sqrt(Int_Dist),sqrt(1-Int_Dist)));
Final_Dist = Final_Dist*6371000; // ระยะทางสุดท้ายเป็นหน่วยเมตร
distance = Final_Dist;
if(distance<5)
{
if(waycont==waypoints)
{
Stop();
}
waycont+=1;
}
//ทิศทาง
Vector norm = compass.readNormalize();
// สูตร declination Angle (deg + (min / 60.0)) / (180 / M_PI);
float declinationAngle = (0 + (35.0 / 60.0)) / (180 / M_PI);
//heading += declinationAngle;
flon = radians(flon);
x2lat = radians(x2lat);
heading = atan2(sin(x2lon-flon)*cos(x2lat), cos(flat)*(sin(x2lat)-sin(flat))*cos(x2lat)*cos(x2lon-flon));
//
heading = heading*180/3.1415926536; //แปลงเรเดียนเป็นองศาดีกรี
int head = heading;
if (head<0)
{
heading = ((head+360)); // ถ้าทิศทางเป็นลบ ทำให้เป็นบวก
}
Serial.print("Formula Heading: ");
Serial.println(heading);
heading += declinationAngle;
// หมุนด้านหากเกิดการกลับด้าน
if(heading < 0)
heading += 2*PI;
// หากเกิดการคลาดเคลื่อนจากมุม Declination
if(heading > 2*PI)
heading -= 2*PI;
// แปลงเรเดียนเป็นองศา
float headingDegrees = heading * 180/M_PI;
//ทิศทางที่ต้องการ
x4 = headingDegrees - heading;
if(x4<2)
{
x4 = x4+360; //ถ้าได้ค่าเป็นลบ
}
else
{
x4 = x4;
}
Serial.print("Difference Angle: ");
Serial.println(x4);
delay(500);
if(x4!=0)
{
if(x4>225&&x4<315) //เลี้ยวซ้าย
{
Left();
Serial.println("Turn Left");
delay(300);
Stop();
delay(500);
}
else if(x4>=45&&x4<135) //เลี้ยวขวา
{
Right();
Serial.println("Turn Right");
delay(500);
Stop();
delay(500);
}
else
if(headingDegrees>315&&headingDegrees<45)// วิ่งตรง
{
Forward();
delay(1000);
Stop();
delay(600);
}
}
else
{
Forward();
delay(300);
}
if(distance<1)
{
Stop();
Serial.println("Destination has reached");
}
}