Help me with my rover programming

Hello, i’m making a new robot. The main thing about it is that it will use a Arduino Uno, Arduino MotorShield R3 ,and HC-SR04 UltraSonic sensor to make it autonomous. TrigPin is connected to digitalPin 6, and echoPin is connected to digitalPin 7. I already made a code, but it failed to make my robot move forward, and make it move autonomously. I will leave a link to download my code. I hope you guys can help me and give me a different piece of code, modify my code, or give me advice(pointers,tips).

-Thank you

Free_Thinker_Programming_Code.ino (1.33 KB)

Don’t worry about the ultrasound sensor yet. Concentrate on getting your motors working.

Here’s some code to check if the motors are turning in the correct direction.

Watch the text in the terminal window and check to make sure the motors on your robot are turning in the correct direction.

const byte PORT_SIDE = 0;
const byte STARBOARD_SIDE = 1;
const byte DIRECTION_PIN[] = {12, 13};
const byte BRAKE_PIN[] = {9, 8};
const byte ENABLE_PIN[] = {3, 11};
const int DEFAULT_SPEED = 85;
const int DEFAULT_TURN = DEFAULT_SPEED;
const byte DYNAMIC_BRAKE = HIGH; // use "LOW" to allow motor to coast to a stop
const int MOTOR_ON_TIME = 2000;
const int MOTOR_OFF_TIME = 1000;
const int BETWEEN_LOOP_DELAY = 3025;

int echoPin = 7;// define the pin connected to echo on the Ultrasonic Sensor
int trigPin = 6;// define the pin connected to trig on the Ultrasonic Sensor

void setup ()
{
  Serial.begin(9600);
  pinMode(echoPin, OUTPUT);// set echo as a output
  pinMode(trigPin, OUTPUT);// set trig as a output

  for (int motorIndex = 0; motorIndex < 2; motorIndex++)
  {
    pinMode(DIRECTION_PIN[motorIndex], OUTPUT);
    pinMode(BRAKE_PIN[motorIndex], OUTPUT);
    pinMode(ENABLE_PIN[motorIndex], OUTPUT);
    analogWrite(ENABLE_PIN[motorIndex], 0);
  }

  Serial.println(F("Motor Test Program"));
}

void loop()
{
  Serial.println();
  Serial.println(F("Start of loop."));
  Serial.println(F("The left motor should be spinning in a direction to propel the robot forward."));
  Serial.println(F("The right motor should be stopped. The robot should be turning right."));
  motorControl(DEFAULT_SPEED, DEFAULT_TURN);
  delay(MOTOR_ON_TIME);

  Serial.println();
  Serial.println(F("Both motors should be stopped."));
  motorControl(0, 0);
  delay(MOTOR_OFF_TIME);

  Serial.println();
  Serial.println(F("The right motor should be spinning in a direction to propel the robot forward."));
  Serial.println(F("The leftt motor should be stopped. The robot should be turning left."));
  motorControl(DEFAULT_SPEED, -DEFAULT_TURN);
  delay(MOTOR_ON_TIME);

  Serial.println();
  Serial.println(F("Both motors should be stopped."));
  motorControl(0, 0);
  delay(MOTOR_OFF_TIME);

  Serial.println();
  Serial.println(F("Both motors should be spinning in a direction to propel the robot forward."));
  motorControl(DEFAULT_SPEED, 0);
  delay(MOTOR_ON_TIME);

  Serial.println();
  Serial.println(F("Both motors should be stopped."));
  motorControl(0, 0);
  delay(MOTOR_OFF_TIME);

  Serial.println();
  Serial.println(F("The robot should turn in place, spinning to the right."));
  motorControl(0, DEFAULT_TURN);
  delay(MOTOR_ON_TIME);

  Serial.println();
  Serial.println(F("Both motors should be stopped."));
  motorControl(0, 0);
  delay(MOTOR_OFF_TIME);

  Serial.println();
  Serial.println(F("The robot should turn in place, spinning to the left."));
  motorControl(0, -DEFAULT_TURN);
  delay(MOTOR_ON_TIME);

  Serial.println();
  Serial.println(F("Both motors should be stopped."));
  motorControl(0, 0);
  //delay(MOTOR_OFF_TIME);
  Serial.println(F("End of loop."));
  Serial.println(F("Waiting before beginning loop again."));
  delay(BETWEEN_LOOP_DELAY);

}

void motorControl(int robotSpeed, int robotTurn)
{
  int motorPower[2];
  motorPower[PORT_SIDE] = robotSpeed + robotTurn;
  motorPower[STARBOARD_SIDE] = robotSpeed - robotTurn;

  for (int motorIndex = 0; motorIndex < 2; motorIndex++)
  {
    if (motorPower[motorIndex] < 0)
    {
      digitalWrite(DIRECTION_PIN[motorIndex], LOW);
      digitalWrite(BRAKE_PIN[motorIndex], LOW);
      motorPower[motorIndex] *= -1; // make motorPower positive to use with analogWrite
    }
    else if (motorPower[motorIndex] > 0)
    {
      digitalWrite(DIRECTION_PIN[motorIndex], HIGH);
      digitalWrite(BRAKE_PIN[motorIndex], LOW);
    }
    else
    {
      digitalWrite(DIRECTION_PIN[motorIndex], LOW);
      digitalWrite(BRAKE_PIN[motorIndex], DYNAMIC_BRAKE);
    }
    analogWrite(ENABLE_PIN[motorIndex], motorPower[motorIndex]);
  }
}

IMO, it’s easier to control things like motors and servos if they’re part of an array. I changed the various control pin constants to arrays so the motor can be controlled using these arrays.

It’s not necessarily the easiest way for someone new to writing code but it’s a lot easier for me to do it this way.

I had originally added a lot more debug statements. These extra debug statements made it easier to see what was going on as the program ran, but these extra statements made it harder to read the actual code.

The code above is the “c” version. I attached both the “c” code and the earlier “b” code. You might want to load the “b” code into the Arduino to test the motor but use the “c” code to figure out how the program works.

Here’s the output from a full loop of the program using the “b” version of the code.

Motor Test Program

Start of loop.
The left motor should be spinning in a direction to propel the robot forward.
The right motor should be stopped. The robot should be turning right.
Controlling motors. robotSpeed = 85, robotTurn = 85, motor # 0 forward, motorPower[0] = 170, motor # 1 stopped, motorPower[1] = 0
Waiting two seconds.

Both motors should be stopped.
Controlling motors. robotSpeed = 0, robotTurn = 0, motor # 0 stopped, motorPower[0] = 0, motor # 1 stopped, motorPower[1] = 0
Waiting one second.

The right motor should be spinning in a direction to propel the robot forward.
The leftt motor should be stopped. The robot should be turning left.
Controlling motors. robotSpeed = 85, robotTurn = -85, motor # 0 stopped, motorPower[0] = 0, motor # 1 forward, motorPower[1] = 170
Waiting two seconds.

Both motors should be stopped.
Controlling motors. robotSpeed = 0, robotTurn = 0, motor # 0 stopped, motorPower[0] = 0, motor # 1 stopped, motorPower[1] = 0
Waiting one second.

Both motors should be spinning in a direction to propel the robot forward.
Controlling motors. robotSpeed = 85, robotTurn = 0, motor # 0 forward, motorPower[0] = 85, motor # 1 forward, motorPower[1] = 85
Waiting two seconds.

Both motors should be stopped.
Controlling motors. robotSpeed = 0, robotTurn = 0, motor # 0 stopped, motorPower[0] = 0, motor # 1 stopped, motorPower[1] = 0
Waiting one second.

The robot should turn in place, spinning to the right.
Controlling motors. robotSpeed = 0, robotTurn = 85, motor # 0 forward, motorPower[0] = 85, motor # 1 reversed, motorPower[1] = 85
Waiting two seconds.

Both motors should be stopped.
Controlling motors. robotSpeed = 0, robotTurn = 0, motor # 0 stopped, motorPower[0] = 0, motor # 1 stopped, motorPower[1] = 0
Waiting one second.

The robot should turn in place, spinning to the left.
Controlling motors. robotSpeed = 0, robotTurn = -85, motor # 0 reversed, motorPower[0] = 85, motor # 1 forward, motorPower[1] = 85
Waiting two seconds.

Both motors should be stopped.
Controlling motors. robotSpeed = 0, robotTurn = 0, motor # 0 stopped, motorPower[0] = 0, motor # 1 stopped, motorPower[1] = 0
End of loop.
Waiting before beginning loop again.
Waiting 3 seconds, 25 ms.

Start of loop.
The left motor should be spinning in a direction to propel the robot forward.
The right motor should be stopped. The robot should be turning right.
Controlling motors. robotSpeed = 85, robotTurn = 85, motor # 0 forward, motorPower[0] = 170, motor # 1 stopped, motorPower[1] = 0
Waiting two seconds.

Let us know if the action of the motors matches the output from the program.

SimpleMotorTest160102c.ino (3.62 KB)

SimpleMotorTest160102b.ino (5.22 KB)

Hey duane I got the motors working the problem is that when I was trying to use the ultra sonic sensor and the motors at the same time, I forgot that I used the same pins the motors were on with the same pins the ultrasonic sensor was connected to, but I still can’t get the ultrasonic sensor working. The reason the motors are connected to pins 12 and 13 even though i’m using a shield is because dirA is connected to digitalPin 12 and dirB is connected to digitalPin 13. DirA & dirB is the direction the motor rotates. I still can’t get the ultra sonic sensor working with the motors. The main problem is that I have no knowledge about using HC-SR04 Ultra Sonic Sensor. So I was hoping you can help me with getting my ultra sonic sensor to work with my motors and make it drive autonomously.

DuaneDegn:
Let us know if the action of the motors matches the output from the program.

Did the direction the motors turned match the directions shown in the terminal window?

DuaneDegn:
Don’t worry about the ultrasound sensor yet. Concentrate on getting your motors working.

If the motors are working correctly then great, we can think about the ultrasound sensor but it’s really easy for the direction a motor turns not to match the direction the program thinks the motor is turning. It would be good to be sure if the motor control algorithm is working correctly.

It would be good to know what sort of speeds work well with the “robotSpeed” and “robotTurn” parameters.


"Did the direction the motors turned match the directions shown in the terminal window?"
The direction of the motors turning do match the directions shown in the terminal window.


Let's get started on the Ultra Sonic Sensor

Here’s some code to test the ultrasound sensor. It’s based on the example “Ping” sketch.

/* 
   Modified by Duane Degn
   January 2, 2016

   This sketch reads an ultrasonic rangefinder and returns the
   distance to the closest object in range. To do this, it sends a pulse
   to the sensor to initiate a reading, then listens for a pulse
   to return.  The length of the returning pulse is proportional to
   the distance of the object from the sensor.                                                                                                                                                    

   The circuit:
	* +V connection of the ultrasoound sensor attached to +5V
	* GND connection of the ultrasoound sensor attached to ground
	* TRIGGER connection of the ultrasoound sensor attached to digital pin 6
  * ECHO connection of the ultrasoound sensor attached to digital pin 7

   http://www.arduino.cc/en/Tutorial/Ping

   created 3 Nov 2008
   by David A. Mellis
   modified 30 Aug 2011
   by Tom Igoe

   This example code is in the public domain.

 */

const byte TRIGGER_PIN = 6;
const byte ECHO_PIN = 7;

void setup() 
{
  // initialize serial communication:
  Serial.begin(9600);
  pinMode(TRIGGER_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
}

void loop()
{
  // establish variables for duration of the ping,
  // and the distance result in inches and centimeters:
  long duration, inches, cm;

  // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
  digitalWrite(TRIGGER_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIGGER_PIN, HIGH);
  delayMicroseconds(5);
  digitalWrite(TRIGGER_PIN, LOW);

  duration = pulseIn(ECHO_PIN, HIGH);

  // convert the time into a distance
  inches = microsecondsToInches(duration);
  cm = microsecondsToCentimeters(duration);

  Serial.print("echo duration = ");
  Serial.print(duration);
  Serial.print(" us, distance = ");
  Serial.print(inches);
  Serial.print(" inches or ");
  Serial.print(cm);
  Serial.println(" centimeters");
  
  delay(100);
}

long microsecondsToInches(long microseconds)
{
  // According to Parallax's datasheet for the PING))), there are
  // 73.746 microseconds per inch (i.e. sound travels at 1130 feet per
  // second).  This gives the distance travelled by the ping, outbound
  // and return, so we divide by 2 to get the distance of the obstacle.
  // See: http://www.parallax.com/dl/docs/prod/acc/28015-PING-v1.3.pdf
  return microseconds / 148; 
}

long microsecondsToCentimeters(long microseconds)
{
  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
  // The ping travels out and back, so to find the distance of the
  // object we take half of the distance travelled.
  return microseconds / 58; 
}

I noticed if the duration was over about 230,000us, then the reading was likely not valid.

Here’s some code which ignores these bogus readings.

/* b version of code
   Modified by Duane Degn
   January 2, 2016

   This sketch reads an ultrasonic rangefinder and returns the
   distance to the closest object in range. To do this, it sends a pulse
   to the sensor to initiate a reading, then listens for a pulse
   to return.  The length of the returning pulse is proportional to
   the distance of the object from the sensor.                                                                                                                                                    

   The circuit:
	* +V connection of the ultrasoound sensor attached to +5V
	* GND connection of the ultrasoound sensor attached to ground
	* TRIGGER connection of the ultrasoound sensor attached to digital pin 6
  * ECHO connection of the ultrasoound sensor attached to digital pin 7

   http://www.arduino.cc/en/Tutorial/Ping

   created 3 Nov 2008
   by David A. Mellis
   modified 30 Aug 2011
   by Tom Igoe

   This example code is in the public domain.

 */

const byte TRIGGER_PIN = 6;
const byte ECHO_PIN = 7;
const long BAD_DURATION = 230000;
const long NOTIFY_BAD_DATA_THRESHOLD = 20;
const int DELAY_AFTER_GOOD_READING = 100; 

long badDataCount = 0;

void setup() 
{
  // initialize serial communication:
  Serial.begin(9600);
  pinMode(TRIGGER_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
}

void loop()
{
  // establish variables for duration of the ping,
  // and the distance result in inches and centimeters:
  long duration, inches, cm;
  boolean badDataFlag = 0;
  // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:

  do
  {
  digitalWrite(TRIGGER_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIGGER_PIN, HIGH);
  delayMicroseconds(5);
  digitalWrite(TRIGGER_PIN, LOW);

  duration = pulseIn(ECHO_PIN, HIGH);

  if (duration >= BAD_DURATION)
  {
     badDataFlag = 1;
     badDataCount++;
     if (badDataCount % NOTIFY_BAD_DATA_THRESHOLD == NOTIFY_BAD_DATA_THRESHOLD - 1)
     {
        Serial.print("badDataCount = ");
        Serial.println(badDataCount);
     }
  }
  else
  {
    badDataFlag = 0;
    badDataCount = 0;
  }

  } while (badDataFlag);
  // convert the time into a distance
  inches = microsecondsToInches(duration);
  cm = microsecondsToCentimeters(duration);

  Serial.print("echo duration = ");
  Serial.print(duration);
  Serial.print(" us, distance = ");
  Serial.print(inches);
  Serial.print(" inches or ");
  Serial.print(cm);
  Serial.println(" centimeters");
  
  delay(DELAY_AFTER_GOOD_READING);
}

long microsecondsToInches(long microseconds)
{
  // According to Parallax's datasheet for the PING))), there are
  // 73.746 microseconds per inch (i.e. sound travels at 1130 feet per
  // second).  This gives the distance travelled by the ping, outbound
  // and return, so we divide by 2 to get the distance of the obstacle.
  // See: http://www.parallax.com/dl/docs/prod/acc/28015-PING-v1.3.pdf
  return microseconds / 148; 
}

long microsecondsToCentimeters(long microseconds)
{
  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
  // The ping travels out and back, so to find the distance of the
  // object we take half of the distance travelled.
  return microseconds / 58; 
}

If there are a bunch of bad readings in a row, the code will let you know.

echo duration = 1353 us, distance = 9 inches or 23 centimeters
echo duration = 3264 us, distance = 22 inches or 56 centimeters
echo duration = 1617 us, distance = 10 inches or 27 centimeters
badDataCount = 19
badDataCount = 39
badDataCount = 59
badDataCount = 79

It takes about 20 bad readings before the program lets you know it’s having trouble.

You might want to take a look at the other ultrasound code available. I believe the new Ping library will average readings for you.

It’s fine with me if you want to use the New Ping library but you’ll need to figure out how to use it and post some demo code.

Test your sensor with the above code (I’m also attaching it). Let me know if the output agrees with the actual distances (or if it’s close).

If your sensor is working correctly then need to decide what you want the robot to do when an obstacle is detected.

Is the robot always going to turn one direction? Is it going to look around for an open area? You need to tell me what sort of decisions you want the program to make.

How far away should an obstacle be before the robot takes action. How much distance do you want before the robot continues moving forward again?

Do you want the robot to stop when an obstacle is found or should the robot gently turn to avoid the obstacle.

Whatever you decide you want the robot to do, you need to try to program it first. If the code doesn’t work like you hoped, tell me what you expected the robot to do and tell me what unexpected things happened.

Please read any replies I make carefully. Please don’t make me request information twice.

Let me know if your sensor is working correctly and what you want to do next (and try it yourself first).

Ultrasound160102b.ino (3.18 KB)

Ultrasound160102a.ino (2.65 KB)


The ultra sonic sensor program given to me works perfectly, nothing wrong is happening, thank you for the ultra sonic sensor help.


"Let me know if your sensor is working correctly and what you want to do next (and try it yourself first)."
The next thing I plan to do is make it autonomous. I'm want to make it so every time the object is 4 inches away, the robot will back up and turn right. NOTEThis rover will not use a servo motor


  • Thank you for your help, I will try to do my objective first, but I hope you will help me make my rover autonomous.

amriechert:
I will try to do my objective first, but I hope you will help me make my rover autonomous.

If you start with one of the programs I attached to reply #1, then it will be easier for me to help you.

I haven't looked at your original code lately but go ahead and do what you can with the reply #1 code.

To back up you'd use:

motorControl(BACKUP_SPEED, 0);

"BACKUP_SPEED" would be a negative value. Set is as a constant. You'll want to decide how fast you want the robot to back up.

The way the code is written, right turn use positive turn speeds.

To turn in place use:

motorControl(0, OBSTACLE_TURN);

Or to turn while driving forward, use:

motorControl(DEFAULT_SPEED, OBSTACLE_TURN);

We'll probably need to figure out a good way to keep track of time. We don't want to keep using delays. You should try to understand the "Blink without delay" example and offer suggestion on how to keep track of time.


Here’s what I made before you replied to my last reply, so the code isn’t how you wanted it. I also need help with the if statement. I don’t know how to make it a certain distance, so it can activate the motors to move backwards then left. What I mean is would you put(did not put the motor movement in this example):
if (inches < 4)
{
}

or

if (distance < 4)
{
}

If the code I made confuses you or you don’t like it I will be happy to use my first one and modify it.


-Thank you

Sorry forgot to put the link to the code in my last reply

Autonomous_Rover_Prototype_Code.ino (4.14 KB)

Here’s a new code I made, but I can’t get the if statements right.

Prototype_AB.ino (3.02 KB)

For such small amounts of code, please, just post the damn code - don’t make people download it.

/*Written January 3rd 2015
*By: amriechert
*This code allows autonomouse movement with s Ultrasonic Sensor
*/


/*__________Ultrasonic Sensor Variables__________*/
int ECHO_PIN = 7;// Echo is connected to digital pin 7
int TRIGGER_PIN = 6;// Trigger is connected to digital pin 6
/*__________Speed Variables______________________*/
int HALF_SPEED = 127.5;// 127.5 is 1/2 the speed of 255
int FULL_SPEED = 255;// 255 is the maximum speed of a motor
int SPEED_SET_A = 3;
int SPEED_SET_B = 11;
/*__________Motor Variables______________________*/
int DIR_A = 12;// direction A is connected to digital pin 12
int BRAKE_A = 9;// brake A is connected to digital pin 9
int DIR_B = 13;// direction B is connected to digital pin 13
int BRAKE_B = 8;
int BRAKE_OFF = LOW;
int BRAKE_ON = HIGH;
/*__________Direction Variables__________________*/
int FORWARD = HIGH;
int BACKWARD = LOW;
/*_______________________________________________*/
            // nothing else

void setup() {
  Serial.begin(9600);// intialize serial connection at 9600 baud
  /*________OUTPUTS______________________________*/
  pinMode(DIR_A, OUTPUT);// sets direction A as a OUTPUT
  pinMode(BRAKE_A, OUTPUT);// sets brake A as a OUTPUT
  pinMode(DIR_B, OUTPUT);// sets direction B as a OUTPUT
  pinMode(BRAKE_B, OUTPUT);// sets brake B as a OUTPUT
  pinMode(TRIGGER_PIN, OUTPUT);// sets trigger pin as a OUTPUT
  /*________INPUTS_______________________________*/
  pinMode(ECHO_PIN, OUTPUT);// set echo pin as a INPUT
  /*_____________________________________________*/
            // nothing else
  
}

void loop() {
  /*______ Ultrasonic Sensor Code_______________*/
  long duration, distance;// <2 undeclared variables
  digitalWrite(TRIGGER_PIN, LOW);// trigger pin does nothing right now
  delayMicroseconds(2);// delay action for 0.0002 seconds
  digitalWrite(TRIGGER_PIN, HIGH);// trigger pin emits a sound
  delayMicroseconds(10);// delay action for 0.0010
  digitalWrite(TRIGGER_PIN, LOW);// trigger pin does nothing again
  duration = pulseIn(ECHO_PIN, HIGH);// declares what the variable duration is equivalent to
  distance = (duration / 2) / 29.1; 
  /*______Motor Code_____________________________*/
  /*______Motor A________________________________*/
  digitalWrite(DIR_A, HIGH);
  digitalWrite(BRAKE_A, BRAKE_OFF);
  analogWrite(SPEED_SET_A, FULL_SPEED);
  /*______Motor B________________________________*/
  digitalWrite(DIR_B, HIGH);
  digitalWrite(BRAKE_B, BRAKE_OFF);
  analogWrite(SPEED_SET_B, FULL_SPEED);
  /*______Autononmous Code_______________________*/
  if (distance < 4)
  {
    // Motor A
    digitalWrite(DIR_A, HIGH);
    digitalWrite(BRAKE_A, BRAKE_OFF);
    analogWrite(SPEED_SET_A, FULL_SPEED);
    // Motor B
    digitalWrite(DIR_B, HIGH);
    digitalWrite(BRAKE_B, BRAKE_OFF);
    analogWrite(SPEED_SET_A, FULL_SPEED);
  }
  else
  {
    // Motor A
    digitalWrite(DIR_A, HIGH);
    digitalWrite(BRAKE_A, BRAKE_OFF);
    analogWrite(SPEED_SET_A, FULL_SPEED);
    // Motor B
    digitalWrite(DIR_B, HIGH);
    digitalWrite(BRAKE_B, BRAKE_OFF);
    analogWrite(SPEED_SET_A, FULL_SPEED);
  }

}