autonomous robot (GPS controlled)

Hi,

I recently built myself a simple robot, consisting of: an Arduino 328 board, motor-driver board, two DC-motors (with tank treads), batteries. See picture, you'll get the idea :wink:

Picture of my project:

hxxp://ttjcrew.com/forum/download/file.php?id=2 (change the xx)

After finishing my robot I found this amazing tutorial on how to build an autonomous car (over ttjcrew.com). The tutorial was for a car with one DC-motor for speed (forward/backward) and one servo for steering. The tutorial provided a code for this to work.

Eager to do the same I ordered the a GPS module (the one used in the tutorial, EM-406A). The only design change on my project was the lack of a servo, but i figured I could use the two motors for steering (like a tank). However I am no expert on Arduino so I did not manage to rewrite the provided code to use two DC-motors for steering instead of a servo.

So I was wondering if any of you know how to modify this code (ttjcrew all rights reserved) for my project:

/* GPS Car v0.1 by Eric Barch (ttjcrew.com) */

#include <SoftwareSerial.h>
#include "Printfloat.h"
#include <nmea.h>
#include <Servo.h>
#undef abs
#undef round
int wpt = 0;
float dest_latitude;
float dest_longitude;
NMEA gps(GPRMC);  // GPS data connection to GPRMC sentence type



/* BEGIN EDITABLE CONSTANTS SECTION */

//These define the positions for your steering servo
#define CENTER_SERVO 95
#define MAX_LEFT_SERVO 85
#define MAX_RIGHT_SERVO 108

//When the car is within this range (meters), move to the next waypoint
#define WPT_IN_RANGE_M 12

//These pins are your h-bridge output. If the car is running in reverse, swap these
#define MOTOR_OUTPUT_ONE 3
#define MOTOR_OUTPUT_TWO 4

/* DEFINE GPS WPTS HERE - Create more cases as needed */
void trackWpts() {
  switch(wpt) {
    case 0:
      dest_latitude = 40.756054;
      dest_longitude = -73.986951;
      break;
    case 1:
      dest_latitude = 37.775206;
      dest_longitude = -122.419209;
      break;
    default:
      dest_latitude = 0;
      dest_longitude = 0;
      break;
  }
  if (gps.gprmc_distance_to(dest_latitude,dest_longitude,MTR) < WPT_IN_RANGE_M)
    wpt++;
}



/* END EDITABLE CONSTANTS SECTION */




Servo steering;
float dir;          // relative direction to destination
int servo_pos = CENTER_SERVO;

void setup() {
  Serial.begin(4800);
  pinMode(MOTOR_OUTPUT_ONE, OUTPUT);
  pinMode(MOTOR_OUTPUT_TWO, OUTPUT);
  steering.attach(9);
}

void loop() {
  trackWpts();
  trackGPS();
}

void trackGPS() {
  if (dest_latitude != 0 && dest_longitude != 0)
  {
    if (Serial.available() > 0 ) {
      char c = Serial.read();

      if (gps.decode(c)) {
          
        // check if GPS positioning was active
        if (gps.gprmc_status() == 'A') {
          // calculate relative direction to destination
          dir = gps.gprmc_course_to(dest_latitude, dest_longitude) - gps.gprmc_course();
          
          if (dir < 0)
            dir += 360;
          if (dir > 180)
            dir -= 360;
            
          if (dir < -75)
            hardLeft();
          else if (dir > 75)
            hardRight();
          else
            driveToTarget();
          
        }
        else //No GPS Fix...Wait for signal
          stop();
            
        }
      }
  }
  else
    stop();
}

void driveStraight() {
  steering.write(CENTER_SERVO);
  digitalWrite(MOTOR_OUTPUT_ONE, LOW);
  digitalWrite(MOTOR_OUTPUT_TWO, HIGH);
  Serial.println("Driving straight...");
}

void driveToTarget() {
  
  Printfloat(dir);
  servo_pos = map(dir, -75, 75, MAX_LEFT_SERVO, MAX_RIGHT_SERVO);
  steering.write(servo_pos);
  digitalWrite(MOTOR_OUTPUT_ONE, LOW);
  digitalWrite(MOTOR_OUTPUT_TWO, HIGH);
  Serial.print("Driving at ");
  Serial.print(dir);
  Serial.print(". - ");
  Serial.print(gps.gprmc_distance_to(dest_latitude,dest_longitude,MTR));
  Serial.print("m to go");
  Serial.print("\n");
}

void hardLeft() {
  steering.write(MAX_LEFT_SERVO);
  digitalWrite(MOTOR_OUTPUT_ONE, LOW);
  digitalWrite(MOTOR_OUTPUT_TWO, HIGH);
  Serial.print("Driving hard left. - ");
  Serial.print(gps.gprmc_distance_to(dest_latitude,dest_longitude,MTR));
  Serial.print("m to go");
  Serial.print("\n");
}

void hardRight() {
  steering.write(MAX_RIGHT_SERVO);
  digitalWrite(MOTOR_OUTPUT_ONE, LOW);
  digitalWrite(MOTOR_OUTPUT_TWO, HIGH);
  Serial.print("Driving hard right. - ");
  Serial.print(gps.gprmc_distance_to(dest_latitude,dest_longitude,MTR));
  Serial.print("m to go");
  Serial.print("\n");
}

void stop() {
  steering.write(CENTER_SERVO);
  digitalWrite(MOTOR_OUTPUT_ONE, HIGH);
  digitalWrite(MOTOR_OUTPUT_TWO, HIGH);
  Serial.print("Stopped.");
  Serial.print("\n");
}

I don't know if this code is any good, but they proved it worked over at ttjcrew.com. Anyways, I really want to finish my project :slight_smile:

The functions for moving the robot (driveStraight, driveToTarget, hardLeft, and hardRight) are the only ones that need to be changed to incorporate track steering.

How are you steering your robot, now? You must have similar functions, don't you?

Are you using PWM to control motor speed?

I figured out how to change the drivestraight, hardleft and hardright code(see code). But not the driveToTarget function.

void driveStraight() {
  digitalWrite(out_B_in_1, LOW);
  digitalWrite(out_B_in_2, HIGH);
  
  digitalWrite(out_A_in_1,LOW);
  digitalWrite(out_A_in_2,HIGH); 
  
  analogWrite(out_A/B_pwm,255);
  
  Serial.println("Driving straight...");
}


void driveToTarget() {
  
  Printfloat(dir);
  servo_pos = map(dir, -75, 75, MAX_LEFT_SERVO, MAX_RIGHT_SERVO);
  steering.write(servo_pos);
  digitalWrite(MOTOR_OUTPUT_ONE, LOW);
  digitalWrite(MOTOR_OUTPUT_TWO, HIGH);
  Serial.print("Driving at ");
  Serial.print(dir);
  Serial.print(". - ");
  Serial.print(gps.gprmc_distance_to(dest_latitude,dest_longitude,MTR));
  Serial.print("m to go");
  Serial.print("\n");
}
void hardLeft() {
  
  digitalWrite(out_B_in_1, LOW);
  digitalWrite(out_B_in_2, HIGH);

  digitalWrite(out_A_in_1, LOW);
  digitalWrite(out_A_in_2, LOW);
  
  analogWrite(out_A/B_pwm,255);
  
  Serial.print("Driving hard left. - ");
  Serial.print(gps.gprmc_distance_to(dest_latitude,dest_longitude,MTR));
  Serial.print("m to go");
  Serial.print("\n");
}

void hardRight() {
  
  digitalWrite(out_A_in_1,LOW);
  digitalWrite(out_A_in_2,HIGH);

  digitalWrite(out_B_in_1, LOW);
  digitalWrite(out_B_in_2, LOW);
  
  analogWrite(out_A/B_pwm,255);
  
  Serial.print("Driving hard right. - ");
  Serial.print(gps.gprmc_distance_to(dest_latitude,dest_longitude,MTR));
  Serial.print("m to go");
  Serial.print("\n");
}

void stop() {
  digitalWrite(out_B_in_1, LOW);
  digitalWrite(out_B_in_2, LOW);
  
  digitalWrite(out_A_in_1,LOW);
  digitalWrite(out_A_in_2,LOW);
  
  analogWrite(out_A/B_pwm,0);
  
  Serial.print("Stopped.");
  Serial.print("\n");

I thought of using PWM for steering, but I am missing one digital port for PWM on both motors, so I coupled the PWM to both, just to control the speed for forward and backward (may change this in the future with other Arduino)

Here is my current pin-layout for the motor-driver:

out_A/B_pwm 11
out_A_in_1 6
out_A_in_2 7
out_B_in_1 12
out_B_in_2 13
out_stby 8

With the controller, you should be able to supply two different PWM values, and two different directions.

The dir value in driveToTarget will define how much you need to turn left or right, based on whether dir is positive or negative.

It's a matter of experimentation to find the right combination of direction for each side, and speed for each side, to turn the amount needed, based on dir.

Okey, so how can I map the dir value to make my robot drive to the target (stay on course)?

Could I just use the if/else command? i.e.

if (dir < -75);
{
  digitalWrite(out_B_in_1, LOW);
  digitalWrite(out_B_in_2, HIGH);

  digitalWrite(out_A_in_1, LOW);
  digitalWrite(out_A_in_2, LOW);
  
  analogWrite(out_A/B_pwm,255);
}
          else if (dir > 75)
            ....

Or could I use the same map command used for the servo?

I know the ATmega328 supports more digital I/O's but my Ardupilot board only has 6 (need 7).

As you know, you're dealing with two different types of steering geometries; one called "differential", and the other (which, for some reason, most people don't know the name of) is called "Ackerman".

R/C cars may or may not use a true Ackerman system; in a true system, the angle of the front steering wheels and the linkages are calculated from lengths of the centerline to the rear axle and some other values; ultimately, in a true system the front wheels are -not- parallel to each other. Rather, the inner wheel on the turn tends to be toed-in more than the outer wheel, which improves handling, and prevents the outer wheel from skidding, reducing stress on the system, and prevents premature/uneven wear on the tires.

Basically, you need an algorithm to alter the speed of your tracks in the differential system based on the angle of steering in an Ackerman system. I am sure that somewhere, given enough research (or some study on the problem) one could come up with a formula that basically does this conversion, but I honestly don't know where or what that would be. It could be possibly fudged.

If we suppose that the servo for steering has a value of 90 for "straight ahead", 180 for "full right" and 0 for "full left", then what you can do is first subtract 90 from the value, so that you have a range of -90 (full left) thru 0 (center) thru 90 (full right). Note that these values are for the sake of this discussion, the real values will differ based on your system and setup.

Anyhow, if you have a PWM value of 0-255 for each motor driving a track in a differential system, what you can do is take the value from the range above, and if it is greater than 0, speed up the left track, and if less than 0, speed up the right track. You might also want to slow down the opposite side as well.

You could say, for instance, that the centerline speed for the PWM is 128, then add or subtract from the appropriate side as needed the absolute value from your steering angle value (-90 thru 90, remember) multiplied by 128/90 (approx 1.42) - so at full left/right stops you would end up with the value 128 to add or subtract, and at lesser angles, a lesser value.

Of course, to truely simulate the same Ackerman steering angle you would need to adjust the 128/90 ratio, perhaps with another multiplier or something, because in a real car you don't "spin on a dime" like the above method would do if you implemented it on a tracked vehicle and went full left/right to the stops (unless you want or need this capability, of course).

That's the gist, anyway - I am certain that the above is full of holes and errors, but the idea is there, I think. Hopefully I have explained it somewhat well enough that you can figure out my meaning and can implement something that will work.

Good luck, and I hope this helps...

:slight_smile:

[edit]Had to change that - had it reversed before - doh! :)[/edit]

Thank you for the info cr0sh! It certainly gave me some more knowledge on steering.
I though of using a different speed on each track, but as I explained before, I don't have enough digital ports for PWM on both motors :(. So for the moment, I will steer my robot by turning just one track for left and the other for right.

However, I still don't get the driveToTarget part of the code?? They are mapping values from dir to servo turning-functions, but I still don't see have to this with the DC-motors? :-/

You need to do some testing. If you drive just one track, the robot should turn one way. If you drive the other track, it should turn the other way.
For various speeds, for a fixed time, you need to determine how far the robot turns.

Say that a pwm value of 25 for 1 second turns 15 degrees. If you know that the angle that you need to rotate the robot is 45 degrees, you can determine that you need to turn for 3 seconds, or turn faster.

To determine which produces the best results will require some experimentation.

Create a sketch that sends a pwm value to the board for a fixed length of time, in setup() (loop will be empty). Measure how far the robot turned. Repeat with higher pwm values, for the same time. Record your values.

Create a sketch that sends a pwm value to the board for a fixed length of time. Measure how far the robot turned. Repeat with higher longer times, for the same time. Record your values.

Which sketch produced the most linear graph of turn angle/variable input (speed or time)? If it's the constant time/variable speed on, map the dir value to get speed.

If it's the variable time/constant speed one, map the dir value to get time.

Set the appropriate direction and speed for the appropriate time, to turn to the appropriate heading.

Don't stress about being too exact. After a few seconds, you'll have a new heading anyway (hopefully a smaller value each time, as you converge on the target heading).