Go Down

Topic: How do I go from one point to another? (GPS used for my AVG) (Read 3 times) previous topic - next topic

sora628

But I need somewhere to start--Today!!  I need to have some progress for tomorrow.  What are some basics I could start with to put into my code?
-Matt-

cyberteque

Try this site

http://williams.best.vwh.net/

I'm using a Micromega FPU V3 with my GPS/Arduino, so none of my code will do you any good here.

Here is the link to the aviation formulae

http://williams.best.vwh.net/avform.htm


cyberteque

This is where I got my formulae

from the site
Quote
Local, flat earth approximation
If you stay in the vicinity of a given fixed point (lat0,lon0), it may be a good enough approximation to consider the earth as "flat", and use a North, East, Down rectangular coordinate system with origin at the fixed point. If we call the changes in latitude and longitude dlat=lat-lat0, dlon=lon-lon0 (Here treating North and East as positive!), then

       distance_North=R1*dlat
       distance_East=R2*cos(lat0)*dlon

R1 and R2 are called the meridional radius of curvature and the radius of curvature in the prime vertical, respectively.

      R1=a(1-e^2)/(1-e^2*(sin(lat0))^2)^(3/2)
      R2=a/sqrt(1-e^2*(sin(lat0))^2)

a is the equatorial radius of the earth (=6378.137000km for WGS84), and e^2=f*(2-f) with the flattening f=1/298.257223563 for WGS84.

In the spherical model used elsewhere in the Formulary, R1=R2=R, the earth's radius. (using R=1 we get distances in radians, using R=60*180/pi distances are in nm.)

In the flat earth approximation, distances and bearings are given by the usual plane trigonometry formulae, i.e:

    distance = sqrt(distance_North^2 + distance_East^2)
    bearing to (lat,lon) = mod(atan2(distance_East, distance_North), 2*pi)
                        (= mod(atan2(cos(lat0)*dlon, dlat), 2*pi) in the spherical case)

These approximations fail in the vicinity of either pole and at large distances. The fractional errors are of order (distance/R)^2.


I took the Micromega examples and changed the great circle method to the one above.


Jantje

cyberteque
Thanks for this info.
That should get me going.
Best regards
Jantje
Do not PM me a question unless you are prepared to pay for consultancy.
Nederlandse sectie - http://arduino.cc/forum/index.php/board,77.0.html -

sora628

But I'm still not completely sure how to start--Can you give me some literal starting point?
-Matt-

Nick Gammon


I'm building an Autonomous Ground Vehicle and it's coming along nicely except I'm having 2 problems---One) I don't know how to actually get the (rc) car to go from one place to the next---Two)


How far apart are "one place" and "the next"? A few meters? A few kilometers? A hundred kilometers?

Great circles will hardly be necessary for short distances.

Quote
I'm having trouble with obstacle avoidance


What sort of trouble?
http://www.gammon.com.au/electronics

jwatte


But I need somewhere to start--Today!!  I need to have some progress for tomorrow.  What are some basics I could start with to put into my code?


I posted exact code for how to make the determination whether to turn left, turn right, or go straight, given where your current GPS position (N, E) is, your target GPS position (N, W) is, and your current compass heading.

If you don't know how to actually read those values, or if you don't know how to control the car given that you know which direction you want to go, then you'll have to ask a different question.

P18F4550

Obsticle avoidance is quite easy, A rotating mirror, an infra red laser projects a beam, the mirror creats a horozontal line at a specific distance in front of the vehicle, a camera  mounded above the laser looks at the terrain ahead useing edge detection the laser line will stand out from the rest of the image and obsticles will apear as breaks in the line.

Everything is available to the homebrewer, scanning mirror from old photocopier or printer, Gameboy cameras have built in edge detection and are easy to use

Google LIDAR

cr0sh


Obsticle avoidance is quite easy, A rotating mirror, an infra red laser projects a beam, the mirror creats a horozontal line at a specific distance in front of the vehicle, a camera  mounded above the laser looks at the terrain ahead useing edge detection the laser line will stand out from the rest of the image and obsticles will apear as breaks in the line.


Easier to build (may or may not be cheaper depending on what you have on hand, though) would be to use a laser module with a built-in line diffraction grating. Such a grating is also sold separately (if you already have the laser, for instance).


Everything is available to the homebrewer, scanning mirror from old photocopier or printer, Gameboy cameras have built in edge detection and are easy to use


...but finding a gameboy camera is starting to get difficult; a better method might be to use something like the Nootropics Design Video Experimenter shield (or a similar circuit) and a composite video camera (aka - a common small security "spy" camera).

Personally, I think his array of IR sensors might be enough for short distances, provided the vehicle doesn't move to quickly...
I will not respond to Arduino help PM's from random forum users; if you have such a question, start a new topic thread.

sora628

Quote
How far apart are "one place" and "the next"? A few meters? A few kilometers? A hundred kilometers?


It's actually just around 50-100metes, nothing too far.

Quote
If you don't know how to actually read those values, or if you don't know how to control the car given that you know which direction you want to go, then you'll have to ask a different question.


I understand it mostly, I'm just not entirely sure of the break down and how to fully integrate that into my code!

Quote
Obsticle avoidance is quite easy, A rotating mirror, an infra red laser projects a beam, the mirror creats a horozontal line at a specific distance in front of the vehicle, a camera  mounded above the laser looks at the terrain ahead useing edge detection the laser line will stand out from the rest of the image and obsticles will apear as breaks in the line.


I have all the sensors I need and they all work as I want them to, it's just the matter of using "for" loops and incrementing values that I'm not to sure how to use--- Let me make another post with two sections of code--One just has the car attempting to avoid walls in a hallway, the other I tried incorporating interrupts and failing miserably.....
-Matt-

sora628

Code: [Select]
/* Using the 5 IR Distance to control the servo to avoid obstacles

Created August 13th, 2012
*/

#include "pitches.h"

#include <Servo.h>  //include the Servo library

#include <DistanceGP2Y0A21YK.h>  //includes the proximity sensor's library

Servo Steering;  //Creates the servo object named "Steering"
Servo Motor;     //Creates the servo object for the motor

DistanceGP2Y0A21YK Dist01;  //defines Dist01 as the sharp GP2Y0A21YK
DistanceGP2Y0A21YK Dist02;  //defines Dist02 as the sharp GP2Y0A21YK
DistanceGP2Y0A21YK Dist03;  //defines Dist03 as the sharp GP2Y0A21YK
DistanceGP2Y0A21YK Dist04;  //defines Dist04 as the sharp GP2Y0A21YK
DistanceGP2Y0A21YK Dist05;  //defines Dist05 as the sharp GP2Y0A21YK

int Prox01;  //creates the integer for the first IR sensor
int Prox02;  //creates the integer for the second IR sensor
int Prox03;  //creates the integer for the third IR sensor
int Prox04;  //creates the integer for the fourth IR sensor
int Prox05;  //creates the integer for the fifth IR sensor

int spd = 100;  //sets the speed of the dc motor

int melody[] = {
  NOTE_F6, 0, NOTE_F6, 0, NOTE_F6, 0, NOTE_F6, 0};  //melody to be played when backing the car up
int noteDurations[] = {
   2,6,2,6,2,6,2,6 };

int ResetLed = 12;    //Reset LED
int ReverseLed = 11;  //Reverse LED
int AlertLed = 13;    //Alert (Backwards) LED

void setup()
{
  Serial.begin(9600);  //opens the serial monitor at 9600 baud
  Dist01.begin(A0);    //tells program "Dist01" will be used by the serial monitor
  Dist02.begin(A1);    //tells program "Dist02" will be used by the serial monitor
  Dist03.begin(A2);    //tells program "Dist03" will be used by the serial monitor
  Dist04.begin(A3);    //tells program "Dist04" will be used by the serial monitor
  Dist05.begin(A4);    //tells program "Dist05" will be used by the serial monitor
 
 
  Steering.attach(10);  //attaches the steering servo to pin 10
  Motor.attach(9);      //attached the motor to PWM pin 9
 
   
  Serial.println("\n  --Sensor01--Sensor02--Sensor03--Sensor04--Sensor05--");

  pinMode(ReverseLed, OUTPUT);
  digitalWrite(ReverseLed, LOW);

  pinMode(AlertLed,OUTPUT);
  digitalWrite(AlertLed, LOW);

  pinMode(ResetLed, OUTPUT);
  digitalWrite(ResetLed, HIGH);
  delay(300);
  digitalWrite(ResetLed, LOW);
  delay(300);
  digitalWrite(ResetLed, HIGH);
  delay(300);
  digitalWrite(ResetLed, LOW);
  delay(300);
  digitalWrite(ResetLed, HIGH);
  delay(300);
  digitalWrite(ResetLed, LOW);
  delay(300);
  digitalWrite(ResetLed, HIGH);
  delay(300);
  digitalWrite(ResetLed, LOW);
  delay(300);
  digitalWrite(ResetLed, HIGH);
  delay(300);
  digitalWrite(ResetLed, LOW);
  Motor.write(spd);
 
}

/*

Although below, it says "getDistanceCentimeter", it really converts the data into Inches---make a very big note for this!!

*/


void loop()
{
  Prox01 = Dist01.getDistanceCentimeter();  //Obtains and converts the voltage from the sensor to Inches (Dist01)
  Prox02 = Dist02.getDistanceCentimeter();  //Obtains and converts the voltage from the sensor to Inches (Dist02)
  Prox03 = Dist03.getDistanceCentimeter();  //Obtains and converts the voltage from the sensor to Inches (Dist03)
  Prox04 = Dist04.getDistanceCentimeter();  //Obtains and converts the voltage from the sensor to Inches (Dist04)
  Prox05 = Dist05.getDistanceCentimeter();  //Obtains and converts the voltage from the sensor to Inches (Dist05)

Serial.print(" ------");
Serial.print(Prox01);
Serial.print("--------");
Serial.print(Prox02);
Serial.print("--------");
Serial.print(Prox03);
Serial.print("--------");
Serial.print(Prox04);
Serial.print("--------");
Serial.print(Prox05);
Serial.println("------");


if ((Prox01 <= 10) || (Prox02 <= 10))        //If the distance is less than 15 inches
  Steering.write(125);    //turn right
else if ((Prox04 <= 10) || (Prox05 <= 10))   //If the distance is less then 15 inches
  Steering.write(65);                        //turn left
else if ((Prox01 <= 15) || (Prox02 <= 15))
  Steering.write(115);
else if ((Prox04 <= 15) || (Prox05 <= 15))
  Steering.write(75);
else if ((Prox01 <= 25) || (Prox02 <= 25))
  Steering.write(105);
else if ((Prox04 <= 25) || (Prox05 <= 25))
  Steering.write(85);
else if ((Prox01 >15) && (Prox02 >15) && (Prox03 >15) && (Prox04 >15) && (Prox05 >15))
  Steering.write(95);
else if (Prox03 <= 20)
  FrontAlert();


/* 
else if ((Prox03 <= 15) || ((Prox01 >15) && (Prox02 >15) && (Prox03 >15) && (Prox04 >15) && (Prox05 >15)) || ((Prox01 <=15) && (Prox02 <=15) && (Prox04 <=15) && (Prox05 <=15)))  //If the distance is less then 15 inches
  Steering.write(105);                       //Go Straight
else if (((Prox01 <=15) && (Prox02 <=15)) || ((Prox01 <=15) && (Prox03 <=15)) || ((Prox01 <=15) && (Prox04 <=15)))
  Steering.write(140);
else if (((Prox01 <=15) && (Prox05 <=15)) || ((Prox02 <=15) && (Prox04 <=15)))
  Steering.write(105);
else if (((Prox05 <=15) && (Prox04 <=15)) || ((Prox05 <= 15) && (Prox03 <=15)) || ((Prox05 <=15) && (Prox02 <=15)))
  Steering.write(50);
*/

delay(100);
}


void BackUpNoise() {                                   //Makes the "Backing Up" noise
  for (int thisNote = 0; thisNote < 9; thisNote++) {
    int noteDuration = 1000/noteDurations[thisNote];
    tone(8, melody[thisNote], noteDuration);
    int pauseBetweenNotes = noteDuration * 1.30;
    delay(pauseBetweenNotes);
    noTone(8);
  }
}


void REVERSE() {                                        //Throws the car in reverse and turns left
  Motor.write(140);
  digitalWrite(ReverseLed, HIGH);
  delay(500);
  Steering.write(95);
  Motor.write(130);
  delay(500);
  Motor.write(110);
  delay(500);
  Motor.write(115);
  BackUpNoise();
  Motor.write(110);
  Steering.write(120);
  delay(1500);
  Steering.write(60);
  Motor.write(100);
  delay(500);
  Motor.write(spd);
  digitalWrite(ReverseLed, LOW);
 
}

void FrontAlert() {                                     //Warns that the car senses something in front of it
  Steering.write(95);
  digitalWrite(AlertLed, HIGH);
  delay(100);
  digitalWrite(AlertLed, LOW);
  delay(100);
  digitalWrite(AlertLed, HIGH);
  delay(100);
  digitalWrite(AlertLed, LOW);
  delay(100);
  digitalWrite(AlertLed, HIGH);
  delay(100);
  digitalWrite(AlertLed, LOW);
  delay(100);
  REVERSE();
 
}


/*  Serial.println("\nDistance in Inches Sensor 1: ");  //Prints the line "Distance in Inches Sensor 1:"
  Serial.print(Prox01);                               //Prints the data from Prox01
  Serial.println("\nDistance in Inches Sensor 2: ");  //Prints the line "Distance in Inches Sensor 2:"
  Serial.print(Prox02);                               //Prints the data from Prox02
  Serial.println("\nDistance in Inches Sensor 3: ");  //Prints the line "Distance in Inches Sensor 3:"
  Serial.print(Prox03);                               //Prints the data from Prox03
  Serial.println("\nDistance in Inches Sensor 4: ");  //Prints the line "Distance in Inches Sensor 4:"
  Serial.print(Prox04);                               //Prints the data from Prox04
  Serial.println("\nDistance in Inches Sensor 5: ");  //Prints the line "Distance in Inches Sensor 5:"
  Serial.print(Prox05);                               //Prints the data from Prox05
  Serial.println("\n-------------------------------");
*/
-Matt-

sora628

Attached is the other code--It's too large to write out as code, so I've uploaded it as and .ino file

The second file is the "pitches" that go along side of the code.

This large thread of code has mostly everything integrated, but most commented out.  Can anyone help me fix my interrupt problem?
-Matt-

jwatte

Quote
I'm just not entirely sure of the break down and how to fully integrate that into my code!


Code: [Select]

initialize everything

forever {
  read sensors
  calculate whether to turn left, right, or straight
  output motor control
  if (am I at current goal?) {
    set next goal as the goal
    if (no more goal) {
      break;
    }
  }
}

Turn of motors
Play victory melody


Your "read sensors" function probably needs to deal with asynchronous results. For example, if your GPS only emits one sample per second, you can't afford to wait for it. Instead, update the "current GPS position" field only if there's a new reading waiting in the input buffer, else grab what you can and continue (keeping the data you've gotten so far until next time.)


sora628

Well that's helpful, but I don't know the simplified formula I would have to use!!  Where would input the coordinates, the heading, etc?  If you could supply me with the formula, then I think I'd be able to get a good start where to go.    Also, is anyone familiar with how to use interrupts?
-Matt-

Nick Gammon

http://www.gammon.com.au/electronics

Go Up