Hi, I’m having some issues with the DualVNH5019MotorShield and Ping Sensors. Any instructions I send to the motor driver execute fine, until I introduce the event-driven Ping loop. Then, the code will execute (I know because I have placed Serial.print() lines in said code), but the motors stop functioning.
I spoke to Support, and they said it’s likely because of conflicts in the Arduino timers. That makes sense to me, but leaves me in a bit of a quandary. If I move away from event-driven Ping polling, I lose a lot of functionality. But obviously the way I’m trying to do it now just won’t work. I’ve included a pared-down version of my code below, and welcome any advice or suggestions.
#include <NewPing.h>
#include "DualVNH5019MotorShield.h"
#define SONAR_NUM 5 // Number or sensors.
#define MAX_DISTANCE 30 // Maximum distance (in cm) to ping.
#define PING_INTERVAL 29 // Milliseconds between sensor pings (29ms is about the min to avoid cross-sensor echo).
DualVNH5019MotorShield motorDriver;
const int RM_FWD = -1;
const int LM_FWD = 1;
const int F_LEFT = 0;
const int F_RIGHT = 1;
const int RIGHT = 2;
const int BACK = 3;
const int LEFT = 4;
const int F_LEFT_PIN = 51;
const int F_RIGHT_PIN = 53;
const int RIGHT_PIN = 47;
const int BACK_PIN = 45;
const int LEFT_PIN = 49;
const int F_LEFT_QTR_PIN = 43;
const int F_RIGHT_QTR_PIN = 41;
unsigned long pingTimer[SONAR_NUM]; // Holds the times when the next ping should happen for each sensor.
unsigned int cm[SONAR_NUM]; // Where the ping distances are stored.
uint8_t currentSensor = 0; // Keeps track of which sensor is active.
NewPing sonar[SONAR_NUM] = { // Sensor object array.
NewPing(F_LEFT_PIN, F_LEFT_PIN, MAX_DISTANCE), // Each sensor's trigger pin, echo pin, and max distance to ping.
NewPing(F_RIGHT_PIN, F_RIGHT_PIN, MAX_DISTANCE),
NewPing(RIGHT_PIN, RIGHT_PIN, MAX_DISTANCE),
NewPing(BACK_PIN, BACK_PIN, MAX_DISTANCE),
NewPing(LEFT_PIN, LEFT_PIN, MAX_DISTANCE)
};
void setup() {
Serial.begin(115200);
pingTimer[0] = millis() + 75; // First ping starts at 75ms, gives time for the Arduino to chill before starting.
for (uint8_t i = 1; i < SONAR_NUM; i++) // Set the starting time for each sensor.
pingTimer[i] = pingTimer[i - 1] + PING_INTERVAL;
motorDriver.init();
delay(5000);
}
void loop() {
motorDriver.setSpeeds(-100,100);
delay(500);
motorDriver.setSpeeds(0,0);
loopAllPings();
}
void turnLeft(int duration){
Serial.print("Turning Left");
// right motor forward
motorDriver.setM1Speed(RM_FWD * 100);
// left motor backward
motorDriver.setM2Speed(-LM_FWD * 100);
stopIfFault();
// if not sentinel value
if(duration != -1){
// stop after delay
delay(duration);
motorDriver.setSpeeds(0,0);
stopIfFault();
}
}
void turnRight(int duration){
Serial.print("Turning Right");
// right motor backward
motorDriver.setM1Speed(-RM_FWD * 100);
// left motor forward
motorDriver.setM2Speed(LM_FWD * 100);
stopIfFault();
// if not sentinel value
if(duration != -1){
// stop after delay
delay(duration);
motorDriver.setSpeeds(0,0);
stopIfFault();
}
}
void stopMotors(){
Serial.print("Stopping");
motorDriver.setSpeeds(0,0);
}
void loopAllPings(){
for (uint8_t i = 0; i < SONAR_NUM; i++) { // Loop through all the sensors.
if (millis() >= pingTimer[i]) { // Is it this sensor's time to ping?
pingTimer[i] += PING_INTERVAL * SONAR_NUM; // Set next time this sensor will be pinged.
if (i == 0 && currentSensor == SONAR_NUM - 1) oneSensorCycle(); // Sensor ping cycle complete, do something with the results.
sonar[currentSensor].timer_stop(); // Make sure previous timer is canceled before starting a new ping (insurance).
currentSensor = i; // Sensor being accessed.
cm[currentSensor] = 0; // Make distance zero in case there's no ping echo for this sensor.
sonar[currentSensor].ping_timer(echoCheck); // Do the ping (processing continues, interrupt will call echoCheck to look for echo).
}
}
}
void trackToFront(){
Serial.print("I <3 TRACKING!");
// if front left sees something
if(cm[F_LEFT] > 0){
// if both front sensors see something
if(cm[F_RIGHT] > 0){
stopMotors();
}
turnLeft(-1);
}
// if only front right sees something
else if(cm[F_RIGHT] > 0){
turnRight(-1);
}
else if(cm[RIGHT] > 0){
turnRight(-1);
}
else if(cm[BACK] > 0){
turnRight(-1);
}
else if(cm[LEFT] > 0){
turnLeft(-1);
}
}
void echoCheck() { // If ping received, set the sensor distance to array.
if (sonar[currentSensor].check_timer())
cm[currentSensor] = sonar[currentSensor].ping_result / US_ROUNDTRIP_CM;
}
void oneSensorCycle() { // Sensor ping cycle complete, do something with the results.
trackToFront();
}
void stopIfFault()
{
if (motorDriver.getM1Fault())
{
Serial.println("M1 fault");
while(1);
}
if (motorDriver.getM2Fault())
{
Serial.println("M2 fault");
while(1);
}
}
If you run this, note that the instructions to the motor driver above the ‘loopAllPings’ function work fine. All others afterward are ignored.
Thanks for the help. Oh, and I’m new to Arduino, so if there are any other glaring issues with my code, please let me know