View unanswered posts | View active topics It is currently Sat Aug 23, 2014 4:07 pm






Reply to topic  [ 12 posts ] 
GPS navigating robot code help 
Author Message
Novice

Joined: Sun Jun 13, 2010 9:26 am
Posts: 58
Post GPS navigating robot code help
Hello:

After many months of robot building and rebuilding I have finally settled on a robot based on the dual track robot. The only change that I made was to replace the front rotating tread assembly with a fixed design like the back treads. What I need help now with is the overall program code. Is it good? Are there potential problem spots? Also I seem to be having troubles with the Dexter Industries DGPS sensor and the code. Am I setting the destination longitude and latitude coordinates properly? I am having troubles with getting a stable longitude and latitude coordinates as it tends to jump around a bit. This causes the angle that the robot needs to head in to change. Also, I do not get a proper distance to target reading. Usually it says it is 9548 (and usually does not change) which is way too much even if measured in millimeters.

One last problem is that I need to include the common.h file for the driver includes to work. Is there a setting in Robotc that I need to set???

I have recharged the battery for testing, using the latest Robotc firmware and Xander's driver suite. Any help with how to improve my code or what is happening with the DGPS sensor would be greatly appreciated.

Mike,
Code:
#pragma config(Sensor, S1,     USSensor,            sensorSONAR)
#pragma config(Sensor, S2,     MSTMUX,              sensorLightInactive)
#pragma config(Sensor, S3,     HTMS,                sensorI2CCustom)
#pragma config(Sensor, S4,     DGPS,                sensorI2CCustom)
#pragma config(Motor,  motorA,          leftmoter,     tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorC,          rightmotor,    tmotorNormal, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "drivers/common.h"
#include "drivers/HTMC-driver.h"
#include "drivers/MSTMUX-driver.h"
#include "drivers/DGPS-driver.h"

long targetHeading;
int side1, side2;
int currentHeading, TouchValue, distance, DGPSdistance;
string direction = "center";
int change = 0;
bool linkstatus = false;
long longitude = 0;
long latitude = 0;
long utc = 0;
long longitudeWaypoint[4];
long latitudeWaypoint[4];
long targetDistance = 0;
bool searchWaypoint = true;
int count = 1;

void Drive_Forward (int speed1, int speed2, int time) {  //set direction and speed of the 4 drive motors
  motor[motorA] = speed1;
  motor[motorC] = speed2;
  wait1Msec(time);
}

void Move_Forward (int duration, int speed1, int speed2) {
  nMotorEncoder[motorC] = 0;  //clear the LEGO motor encoders
  nMotorEncoder[motorA] = 0;
  wait1Msec(10);

  while (nMotorEncoder[motorA] > duration) { // 90 degrees to the right
    motor[motorC] = speed1; //turn both motors on at 30 percent power
    motor[motorA] = speed2;
  }
  motor[motorC] = 0; //turn both motors off
  motor[motorA] = 0;
}

void Compass_Check () {

  currentHeading = HTMCreadRelativeHeading(HTMS);  //take current compass reading

  side1 = currentHeading - 10; //create left side of compass range
  if (side1 < -179) {
    side1 = side1 + 180;
  }

  side2 = currentHeading + 10;  //create right side of compass range
  if (side2 > 180) {
    side2 = side2 - 180;
  }
}

void OrientToTarget () { //move robot closer to target compass heading

  if (currentHeading > targetHeading) {
    Drive_Forward (-45, 45, 100);
  }
  else if (currentHeading < targetHeading) {
    Drive_Forward (45, -45, 100);
  }
  Compass_Check(); //get new compass range before returning to main task
}

void ChangeDirection () {
  if (direction == "left") { //if robot turned left in the last 5 time continue to do so
    Drive_Forward (45, 45, 1000); //reverse
    Drive_Forward (45, -45, 1500); //turn left
    change = 5;  //now the robot should turn left unless it has gone forward 5 times in a row
  }
  else if (direction == "right") {
    Drive_Forward (45, 45, 1000); //reverse
    Drive_Forward (-45, 45, 1500); //turn right
    change = 5;  //now the robot should turn right unless it has gone forward 5 times in a row
  }
  else if (direction == "center") { // obstacle is detected look for a new direction to go for a bit
    Drive_Forward (45, 45, 2000); //reverse
    Drive_Forward (45, -45, 1500); //turn right
    int ScanRight = SensorValue(USSensor); //get ultrasonic sensor reading
    Drive_Forward (-45, 45, 3000); //turn to left side
    int ScanLeft = SensorValue(USSensor); //get ultrasonic sensor reading
    if (ScanRight < ScanLeft) {
//      Drive_Forward (45, -45, 1500); //turn left
      Drive_Forward (-75, -75, 2000);
      wait1Msec(2000);
      direction = "left"; //set direction to go left if obstacle is encountered before robot has gone forward less than 5 times
    }
    if (ScanRight > ScanLeft) {
      Drive_Forward (-45, 45, 3000);  //turn right
      Drive_Forward (-75, -75, 2000);
      wait1Msec(2000);
      direction = "right"; //set direction to go right if obstacle is encountered before robot has gone forward less than 5 times
    }
  }
}

void GPSSensorRead () {
  wait1Msec(500);
  utc = DGPSreadUTC(DGPS); // get coordinates and universal time code -- utc
  longitude = DGPSreadLongitude(DGPS);
  latitude = DGPSreadLatitude(DGPS);
  targetHeading = DGPSreadHeading(DGPS);
  targetHeading = targetHeading - 180;
  linkstatus = DGPSreadStatus(DGPS);
  DGPSdistance = DGPSreadDistToDestination(DGPS);
}

task main() {

  longitudeWaypoint[1] = -114097512;
  longitudeWaypoint[2] = -114097640;
  longitudeWaypoint[3] = -114097416;
  latitudeWaypoint[1] = 51793988;
  latitudeWaypoint[2] = 51793856;
  latitudeWaypoint[3] = 51793232;

  DGPSsetDestination(DGPS, latitudeWaypoint[count], longitudeWaypoint[count]); //input first of target coordinates

  while (true) {
//    while (linkstatus) {

      GPSSensorRead();  //get GPS information
      TouchValue = MSTMUXgetActive(MSTMUX); //get value from touch sensor multiplexer

      if (linkstatus) {
        Compass_Check (); //get compass reading and set range values
      }

      if ((side1 < targetHeading) && (side2 > targetHeading) && (linkstatus)) {
        distance = SensorValue(USSensor);
        if ((distance > 30) && (TouchValue == 0)) { // check for obstacles otherwise drive forward
          Drive_Forward (-75, -75, 100);
          change = change - 1;
          if (change < 0) {
            change = 0;
            direction = "center";
          }
        }
        else {
          ChangeDirection (); //obstacle detected move in a different direction
        }
      }

      if (TouchValue == 1) {  //left touch sensor pressed
        Drive_Forward (75, 75, 1000);  //backup and move to left side, then move forward
        Drive_Forward (-75, 75, 1000);
        Drive_Forward (-75, -75, 1000);
      }
      else if (TouchValue == 2) {  //right touch sensor pressed
        Drive_Forward (75, 75, 1000);  //backup and move to right side, then move forward
        Drive_Forward (75, -75, 1000);
        Drive_Forward (-75, -75, 1000);
      }
      else if (TouchValue == 3) {  //both touch sensors pressed use ChangeDirection to pick which side to use to avoid obstacle
        Drive_Forward (75, 75, 1000);
        ChangeDirection ();
      }

      if ((side1 > targetHeading) && (side2 > targetHeading) && (linkstatus)) {  //check which side of the target heading the robot is on and turn
        OrientToTarget ();
      }
      else if ((side1 < targetHeading) && (side2 < targetHeading) && (linkstatus)) {
        OrientToTarget ();
      }

      long a1 = longitudeWaypoint[count] - longitude;  //calculate distance to target
      long b1 = latitudeWaypoint[count] - latitude;
      long a2 = a1 * a1;
      long b2 = b1 * b1;
      targetDistance = sqrt(a2 + b2);
      if (targetDistance < 100) {
        count = count + 1;
      }
      eraseDisplay(); //display some imformation
      nxtDisplayTextLine(1, "Target: %4d", targetHeading);  //DGPS compass direction
      nxtDisplayTextLine(2, "Compass:   %4d", HTMCreadRelativeHeading(HTMS));  //HT compass heading
      nxtDisplayTextLine(3, "Lon: %d", longitude); //current longitude value
      nxtDisplayTextLine(4, "Lat: %d", latitude);  //current latitude value
      nxtDisplayTextLine(5, "Distance:  %4d", targetDistance);  //calculated distance to target
      nxtDisplayTextLine(6, "DGPS_dis: %4d", DGPSdistance);  //DGPS distance to target from sensor
    }
//  }
}



Thu Jun 02, 2011 11:24 am
Profile
Moderator
Moderator
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3193
Location: Rotterdam, The Netherlands
Post Re: GPS navigating robot code help
I reduced your program to just the bare essentials and the distance from my place to the coordinates specified in your program are 19095 but yours is vastly different and keeps changing. I get the feeling your distance calculations may be a bit wrong. I think you may want to go back to the drawing board and read up on how to calculate the distance between decimal degree points.

In any case, the way point you really want to be sending your robot to is 51.795177 -114.102420, make sure you ask for extra cheese.

Btw, you can download the latest version of the drivers here: [LINK], I can really recommend using the beta 2 driver suite.

Regards,
Xander

Code:
#pragma config(Sensor, S4,     DGPS,                sensorI2CCustom)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "drivers/DGPS-driver.h"

long targetHeading;
int DGPSdistance;
bool linkstatus = false;
long longitude = 0;
long latitude = 0;
long utc = 0;
long longitudeWaypoint[4];
long latitudeWaypoint[4];
long targetDistance = 0;
int count = 1;




void GPSSensorRead () {
  wait1Msec(500);
  utc = DGPSreadUTC(DGPS); // get coordinates and universal time code -- utc
  longitude = DGPSreadLongitude(DGPS);
  latitude = DGPSreadLatitude(DGPS);
  targetHeading = DGPSreadHeading(DGPS);
  targetHeading = targetHeading - 180;
  linkstatus = DGPSreadStatus(DGPS);
  DGPSdistance = DGPSreadDistToDestination(DGPS);
}

task main() {

  longitudeWaypoint[1] = -114097512;
  longitudeWaypoint[2] = -114097640;
  longitudeWaypoint[3] = -114097416;
  latitudeWaypoint[1] = 51793988;
  latitudeWaypoint[2] = 51793856;
  latitudeWaypoint[3] = 51793232;

  DGPSsetDestination(DGPS, latitudeWaypoint[count], longitudeWaypoint[count]); //input first of target coordinates

  while (true) {
      GPSSensorRead();  //get GPS information

      long a1 = longitudeWaypoint[count] - longitude;  //calculate distance to target
      long b1 = latitudeWaypoint[count] - latitude;
      long a2 = a1 * a1;
      long b2 = b1 * b1;
      targetDistance = sqrt(a2 + b2);
      if (targetDistance < 100) {
        count = count + 1;
      }
      eraseDisplay(); //display some imformation
      nxtDisplayTextLine(1, "Target: %4d", targetHeading);  //DGPS compass direction
      nxtDisplayTextLine(3, "Lon: %d", longitude); //current longitude value
      nxtDisplayTextLine(4, "Lat: %d", latitude);  //current latitude value
      nxtDisplayTextLine(5, "Distance:  %4d", targetDistance);  //calculated distance to target
      nxtDisplayTextLine(6, "DGPS_dis: %4d", DGPSdistance);  //DGPS distance to target from sensor
    }
}

_________________
| Professional Conduit of Reasonableness
| (Title bestowed upon on the 8th day of November, 2013)
| My Blog: I'd Rather Be Building Robots
| ROBOTC 3rd Party Driver Suite: [Project Page]


Mon Jun 06, 2011 2:19 pm
Profile WWW
Rookie

Joined: Wed Dec 01, 2010 7:33 pm
Posts: 10
Post Re: GPS navigating robot code help
Hello,

Just a few quick thoughts back on this issue.

First, the distance to destination function returns distance in meters, not mm. The number you returned shows a 9.5 KM distance from the destination.

I'm confused about the distance returned, even if in MM, being "way too much". Are you standing on the destination point when you take that distance?

Next, the most common problem we've been contacted about entering the destination coordinates incorrectly. If you enter this in one way, you get a location in Canada: http://maps.google.com/maps?f=q&source= ... 820801&z=7

If you enter it in another way, you get an incorrect destination but the chip will calculate it regardless.

_________________
John Cole
Dexter Industries
http://www.dexterindustries.com
http://www.dexterindustries.com/blog
Image


Wed Jun 08, 2011 10:17 am
Profile WWW
Novice

Joined: Sun Jun 13, 2010 9:26 am
Posts: 58
Post Re: GPS navigating robot code help
Hello Xander and John:

Xander: I had a chance to try out your reduced program code last night, but I have not changed how I calculate the distance. I will try that today. I had to teach a Digital Photography class in a small town about 30 kilometers away. So, I separated the NXT brick and the DGPS sensor from the robot and took them with me on my short road trip. The DGPS sensor was mounted to the side of the NXT brick. Does the sensor need to be placed a certain distance away from the NXT brick (and motors) like the compass sesnor? During the drive I did get a blue connection light from the sensor. Here is what I saw along the way:
The DGPS heading value stayed pretty steady on target and changed when I either came to a corner and changed directions or came to a bend in the road and changed directions. The DGPS distance value would change quiet a bit. A lot of times it showed the value 214745. Which I believe is the distance calculation when the DGPS sensor does not have a true status link. Other times the value would shift quiet a bit from 30000 to 40000. This was as I left from my class and headed back home. The value did get smaller but often with the wide distance variance in the calculated distance. Once I got home I never did get a value that would indicate I was close to my target. I think the smallest value I get is 1000, often a lot more.

John: I did check to make sure that I was entering the latitude and longitude values in the correct order.

Thank you for all your help. I will keep plugging away and figuring out how to use the DGPS sensor to help my robot navigate around.

Mike,

Mike,


Wed Jun 08, 2011 11:33 am
Profile
Rookie

Joined: Wed Dec 01, 2010 7:33 pm
Posts: 10
Post Re: GPS navigating robot code help
Hey Folks,

No worries. It sounds like the sensor itself is working fine then. Have you looked to verify on google earth at all? There are some great tools on there in terms of measuring distances and angles from one point to another. I used it a lot while developing the firmware to verify different readings and calculations.

You can see a tutorial on it here: http://dexterindustries.com/blog/2010/12/02/integrating-lego-and-google-earth-with-robotc/
And a quick guide we wrote specifically on that subject here: http://dexterindustries.com/files/DI_GPS_Sensor_and_Google_Maps.pdf

_________________
John Cole
Dexter Industries
http://www.dexterindustries.com
http://www.dexterindustries.com/blog
Image


Wed Jun 08, 2011 11:50 am
Profile WWW
Novice

Joined: Sun Jun 13, 2010 9:26 am
Posts: 58
Post Re: GPS navigating robot code help
Hello :D

I did a little more looking around on the Internet. There are a lot of samples of using the Great Circle formula - with a lot of trig functions calculations happening. I did find this site with 2 simplified methods. http://www.meridianworlddata.com/Distance-Calculation.asp I do not know how the constants were created or the units involved. The two formulas are close in agreement. The error gets bigger the farther away from the destination point you get. Further reading indicates that I can remove the division by 57.3 in the targetDistance2 calculation. The division by 57.3 converts the lat1 value into radians which is not needed??? I will make the change later today and see what happens. The DGPS distance value I get is way off from the other 2 calculation methods. Unless the unit measure is in millimeters????? Than it is only twice as large as my calculated values. If anyone knows more details, particularly if I have made an error with the calculations please let me know.

Mike,

Code:
#pragma config(Sensor, S4,     DGPS,                sensorI2CCustom)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "drivers/common.h" //needed by my setup - I'm not sure why??? If someone knows please let me know.
#include "drivers/DGPS-driver.h"

long targetHeading;
int DGPSdistance;
bool linkstatus = false;
float longitude;
float latitude;
long utc;
float longitudeWaypoint = -114.097528;
float latitudeWaypoint = 51.793932;
float targetDistance1;
float targetDistance2;
float a1, a2, b1, b2, b3, bb;

void GPSSensorRead () {
  wait1Msec(500);
  utc = DGPSreadUTC(DGPS); // get coordinates and universal time code -- utc
  longitude = (float)DGPSreadLongitude(DGPS) / 1000000;
  latitude = (float)DGPSreadLatitude(DGPS) / 1000000;
  targetHeading = DGPSreadHeading(DGPS);
  targetHeading = targetHeading - 180;
  linkstatus = DGPSreadStatus(DGPS);
  DGPSdistance = DGPSreadDistToDestination(DGPS);
}

task main() {

  DGPSsetDestination(DGPS, latitudeWaypoint, longitudeWaypoint); //input first of target coordinates

  while (true) {

      GPSSensorRead();  //get GPS information

      b1 = 53.0 * (longitudeWaypoint - longitude);  //calculate distance to target
      a1 = 69.1 * (latitudeWaypoint - latitude);
      bb = 69.1 * (longitudeWaypoint - longitude) * cosDegrees(latitude);
      a2 = a1 * a1;
      b2 = b1 * b1;
      b3 = bb * bb;
      targetDistance1 = sqrt(a2 + b2); //sqrt(x*x + y*y) where x = 69.1 *(lat2 - lat1), y = 53.0 * (long2 - long1)
      targetDistance2 = sqrt(a2 + b3); //sqrt(x*x + y*y) where x = 69.1 *(lat2 - lat1), y = 69.1 * (long2 - long1) * cosDegrees(lat1/57.3)

      eraseDisplay(); //display some imformation
      nxtDisplayTextLine(1, "Target: %4d", targetHeading);  //DGPS compass direction
      nxtDisplayTextLine(2, "Lon: %+3.6f", longitude); //current longitude value
      nxtDisplayTextLine(3, "Lat: %+3.6f", latitude);  //current latitude value
      nxtDisplayTextLine(4, "Di1:  %+1.6f", targetDistance1);  //calculated distance to target
      nxtDisplayTextLine(5, "Di2:  %+1.6f", targetDistance2);  //calculated distance to target with cosine added
      nxtDisplayTextLine(6, "DGPS_dis: %4d", DGPSdistance);  //DGPS distance to target from sensor
    }
}


Thu Jun 09, 2011 12:52 pm
Profile
Novice

Joined: Sun Jun 13, 2010 9:26 am
Posts: 58
Post Re: GPS navigating robot code help
Update:

Yes, removing the 57.3 division brings the 2 distance calculations more in line with each other. If fact, where the targetDistance2 was larger than the the targetDistance1 value, it is now very slightly less now. I will see if I can go on an extended walk tonight, or more likely tomorrow morning and see how the calculated values compare over a larger distance.

As always, thank you for your help with this.
Mike,


Thu Jun 09, 2011 3:14 pm
Profile
Rookie

Joined: Mon Jun 04, 2012 9:31 am
Posts: 6
Post Re: GPS navigating robot code help
Mike,
I am curious if you ever got your GPS navigating robot running right, because I will be attempting at using robotc to run the dexter industries dgps for navigating from point a to point b.

Joe


Sat Dec 15, 2012 11:49 am
Profile
Rookie

Joined: Thu Sep 13, 2012 9:43 am
Posts: 47
Post Re: GPS navigating robot code help
Lego Geek wrote:
Hello:

After many months of robot building and rebuilding I have finally settled on a robot based on the dual track robot. The only change that I made was to replace the front rotating tread assembly with a fixed design like the back treads. What I need help now with is the overall program code. Is it good? Are there potential problem spots? Also I seem to be having troubles with the Dexter Industries DGPS sensor and the code. Am I setting the destination longitude and latitude coordinates properly? I am having troubles with getting a stable longitude and latitude coordinates as it tends to jump around a bit. This causes the angle that the robot needs to head in to change. Also, I do not get a proper distance to target reading. Usually it says it is 9548 (and usually does not change) which is way too much even if measured in millimeters.

One last problem is that I need to include the common.h file for the driver includes to work. Is there a setting in Robotc that I need to set???

I have recharged the battery for testing, using the latest Robotc firmware and Xander's driver suite. Any help with how to improve my code or what is happening with the DGPS sensor would be greatly appreciated.

Mike,


It is normal for any GPS location to jump around by a few meters, even in areas of strong signal. I'm not sure what would be normal for the Dexter Industries unit. Are you wanting to use the robot inside or outside? How much is the location jumping around with the corrected code?


Tue Dec 18, 2012 6:07 am
Profile
Novice

Joined: Sun Jun 13, 2010 9:26 am
Posts: 58
Post Re: GPS navigating robot code help
I do not remember how much the signal bounces around but what I have found out is that the sensor can be accurate to about 5 to 10 feet. Which for a small 1 foot sized robot can be a big difference. Also, concrete and other hard surfaces can cause the signal to bounce and also skew the reading. I have not gotten that far but was thinking that when the robot is close (not sure how I would tell, but within a few decimal points of the target heading) the robot would use some other means to home onto the target. One of my thoughts was using an infrared beacon, or a large colored object.


Tue Dec 18, 2012 1:52 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: GPS navigating robot code help
Lego Geek wrote:
I do not remember how much the signal bounces around but what I have found out is that the sensor can be accurate to about 5 to 10 feet. Which for a small 1 foot sized robot can be a big difference. Also, concrete and other hard surfaces can cause the signal to bounce and also skew the reading. I have not gotten that far but was thinking that when the robot is close (not sure how I would tell, but within a few decimal points of the target heading) the robot would use some other means to home onto the target. One of my thoughts was using an infrared beacon, or a large colored object.

In that case, you may consider applying some filtering on the data. Kalman filter may work for you.


Tue Dec 18, 2012 3:33 pm
Profile
Rookie

Joined: Thu Sep 13, 2012 9:43 am
Posts: 47
Post Re: GPS navigating robot code help
MHTS wrote:
Lego Geek wrote:
I do not remember how much the signal bounces around but what I have found out is that the sensor can be accurate to about 5 to 10 feet. Which for a small 1 foot sized robot can be a big difference. Also, concrete and other hard surfaces can cause the signal to bounce and also skew the reading. I have not gotten that far but was thinking that when the robot is close (not sure how I would tell, but within a few decimal points of the target heading) the robot would use some other means to home onto the target. One of my thoughts was using an infrared beacon, or a large colored object.

In that case, you may consider applying some filtering on the data. Kalman filter may work for you.


You can find some robotC code for Kalman filters at:
https://nxttime.wordpress.com/category/filter/kalman-filter/
and
http://stackoverflow.com/questions/1586658/combine-gyroscope-and-accelerometer-data
and
I thought Lauren Valks mentioned them as part of his Segway project http://robotsquare.com/2012/02/13/tutorial-segway-with-robotc/ but I can't find anything now.

The idea of extra sensors to home in sounds good. I'm using a colour sensor for my current project.


Wed Dec 19, 2012 4:49 am
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 12 posts ] 

Who is online

Users browsing this forum: No registered users and 2 guests


You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot post attachments in this forum

Search for:
Jump to:  



Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group.
Designed by ST Software for PTF.