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)