Ultrasonic & GPS in one project

hello, can me solve problem final project? problem didn't consistent value ultrasonic sensor. right now, i'm using 5 ultrasonic sensors , 1 shield gps project. hope guys can me. thanks.
here code:

code: [select]

#include <tinygps.h>
#include <softwareserial.h>

softwareserial gps(52, 53);
tinygps shield;

float x2lat, x2lon;         // latitude , longitude reach
float head, distance;        // calculation 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 an2 @ mds10
int pwm_motor_left = 5;               //pwm mottor left connect an1 @ mds10
int dir_motor_right = 4;              //direction mottor right connect dig2 @ mds10
int dir_motor_left = 7;               //direction mottor left connect dig1 @ 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 data coming gps shield
 {
   char = gps.read(); // byte of data
   if (shield.encode(a)) // if there valid gps data...
     newdata = true;
 }
}
if (newdata)
{
 float latitude, longitude, age;
 float x2lat, x2lon;

 // call 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 lat: ");
 serial.print(x2lat, 6);
 serial.print(" go 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 must done in radians
 diflon = (x2lon) - (longitude); //subtract , convert longitudes 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;
}

}

check power supply adequate.

please edit post , add code tags forgot.

Arduino Forum > Using Arduino > Project Guidance > Ultrasonic & GPS in one project

arduino