Hello, can anyone help me to solve my problem for my final project? My problem is I didn't get a consistent value for my ultrasonic sensor. Right now, I'm using 5 ultrasonic sensors and 1 shield GPS for my project. Hope you guys can help me. Thanks.
Here is my code:
#include <TinyGPS.h>
#include <SoftwareSerial.h>
SoftwareSerial GPS(52, 53);
TinyGPS shield;
float x2lat, x2lon; // latitude and longitude to reach
float head, distance; // calculation for gps
void gpshead();
float latitude, longitude, age;
int trigger_left = A10;
int echo_left = A11;
int trigger_front2 = A2;
int echo_front2 = A3;
int trigger_front1 = A4;
int echo_front1 = A5;
int trigger_front3 = A6;
int echo_front3 = A7;
int trigger_right = A8;
int echo_right = A9;
float frontSensor2, frontSensor1, frontSensor3, rightSensor, leftSensor;
long duration_front1, duration_front2, duration_front3, duration_left, duration_right;
void forward();
void Backward();
void Stop();
void rightturn();
void leftturn();
int pwm_motor_right = 6; //pwm mottor right connect to AN2 at MDS10
int pwm_motor_left = 5; //pwm mottor left connect to AN1 at MDS10
int dir_motor_right = 4; //direction mottor right connect to DIG2 at MDS10
int dir_motor_left = 7; //direction mottor left connect to DIG1 at MDS
int pwm_val = 0;
void setup() {
Serial.begin(9600);
GPS.begin(9600);
pinMode(trigger_front1, OUTPUT);
pinMode(echo_front1, INPUT);
pinMode(trigger_front2, OUTPUT);
pinMode(echo_front2, INPUT);
pinMode(trigger_front3, OUTPUT);
pinMode(echo_front3, INPUT);
pinMode(trigger_left, OUTPUT);
pinMode(echo_left, INPUT);
pinMode(trigger_right, OUTPUT);
pinMode(echo_right, INPUT);
pinMode(4, OUTPUT);
pinMode(5, OUTPUT);
pinMode(6, OUTPUT);
pinMode(7, OUTPUT);
}
void loop() {
gpshead();
digitalWrite(trigger_left, LOW);
delayMicroseconds(2);
digitalWrite(trigger_left, HIGH);
delayMicroseconds(5);
digitalWrite(trigger_left, LOW);
duration_left = pulseIn(echo_left, HIGH);
leftSensor = (duration_left / 2) / 29.1;
digitalWrite(trigger_front2, LOW);
delayMicroseconds(2);
digitalWrite(trigger_front2, HIGH);
delayMicroseconds(5);
digitalWrite(trigger_front2, LOW);
duration_front2 = pulseIn(echo_front2, HIGH);
frontSensor2 = (duration_front2 / 2) / 29.1;
digitalWrite(trigger_right, LOW);
delayMicroseconds(2);
digitalWrite(trigger_right, HIGH);
delayMicroseconds(5);
digitalWrite(trigger_right, LOW);
duration_right = pulseIn(echo_right, HIGH);
rightSensor = (duration_right / 2) / 29.1;
digitalWrite(trigger_front1, LOW);
delayMicroseconds(2);
digitalWrite(trigger_front1, HIGH);
delayMicroseconds(5);
digitalWrite(trigger_front1, LOW);
duration_front1 = pulseIn(echo_front1, HIGH);
frontSensor1 = (duration_front1 / 2) / 29.1;
digitalWrite(trigger_front3, LOW);
delayMicroseconds(2);
digitalWrite(trigger_front3, HIGH);
delayMicroseconds(5);
digitalWrite(trigger_front3, LOW);
duration_front3 = pulseIn(echo_front3, HIGH);
frontSensor3 = (duration_front3 / 2) / 29.1;
Serial.println("--------------------------------------------------------------");
Serial.print("head: ");
Serial.print(head);
Serial.print(" distance: ");
Serial.println(distance);
Serial.print(" 1:");
Serial.print(leftSensor); //left sensor
Serial.print(" 2:");
Serial.print(frontSensor2); //front left sensor
Serial.print(" 3:");
Serial.print(rightSensor); //right sensor
Serial.print(" 4:");
Serial.print(frontSensor1); //center sensor
Serial.print(" 5:");
Serial.print(frontSensor3); //front right sensor
Serial.println("");
}
void gpshead()
{
bool newData = false;
for ( unsigned long start = millis(); millis() - start < 1000;)
{
while ( GPS.available() ) // if there is data coming from the GPS shield
{
char a = GPS.read(); // get the byte of data
if (shield.encode(a)) // if there is valid GPS data...
newData = true;
}
}
if (newData)
{
float latitude, longitude, age;
float x2lat, x2lon;
// Then call this function
shield.f_get_position(&latitude, &longitude);
Serial.println("--------------------------------------------------------------");
Serial.print("Current Lat: ");
Serial.print(latitude, 6);
Serial.print(" Current Long: ");
Serial.println(longitude, 6);
x2lat = 2.928890; //desired latitude
x2lon = 101.767662; //desired longitude
Serial.println("--------------------------------------------------------------");
Serial.print("Go to Lat: ");
Serial.print(x2lat, 6);
Serial.print(" Go to Long: ");
Serial.println(x2lon, 6);
longitude = radians(longitude);
latitude = radians(latitude);
x2lat = radians(x2lat);
x2lon = radians(x2lon);
head = atan2(sin(x2lon - longitude) * cos(x2lat), cos(latitude) * sin(x2lat) - sin(latitude) * cos(x2lat) * cos(x2lon - longitude));
head = head * 180 / 3.1415926535;
head -= 90;
float dist_calc = 0;
float diflat = 0;
float diflon = 0;
diflat = x2lat - latitude; //notice it must be done in radians
diflon = (x2lon) - (longitude); //subtract and convert longitudes to radians
distance = (sin(diflat / 2.0) * sin(diflat / 2.0));
dist_calc = cos(latitude);
dist_calc *= cos(x2lat);
dist_calc *= sin(diflon / 2.0);
dist_calc *= sin(diflon / 2.0);
distance += dist_calc;
distance = (2 * atan2(sqrt(distance), sqrt(1.0 - distance)));
distance *= 6371000.00;
if (head < 0)
{
head += 360;
}
}
else
{
head = 0;
distance = 0;
}
}
I had to stare at that for a minute.