Issue controlling Motors Using SparkFun Monster Moto Shield

Hello everyone,

This is my first post here, so please correct me if I’ve messed up any part of posting to this forum.

I am having some trouble making a following robot project. I am working on this project with my professor, and I have reached a point where I am stuck. I am going to show the picture of the robot first so that you all can see the parts in their locations before I explain the issues.

The robot is supposed to use the two ultrasonic sensors to gauge distance and maintain a certain distance away from whatever it is following. The point of having 2 sensors is so that the robot can turn when the object turns.

The issue that I am having is that when one motor wants to go one direction while the other motor is either stopped or wanting to go in the other direction, neither motor will work. I think this may be an issue with my motor controlling function.

I am open to any and all suggestions, so please let me know what you think I can do. I am not all to familiar with programming, so there is likely just a simple mistake.

Here is my (somewhat silly) code.

/*
 * Following Code V3.0
 * 
 * Nickalaus Clemmer
 * 
 * Updated to include the PID library.
 * 
 */

//Library inclusion

#include <NewPing.h>
#include <PID_v1.h>

//Pin Number Definitions

#define trigLpin 3        //trigger pin for the left sensor
#define trigRpin 10       //trigger pin for the right sensor   
#define echoLpin 2        //echo pin for the left sensor
#define echoRpin 11       //echo pin for the right sensor

#define statpin 13

int inCWpin[2] = {7, 4};  // INCW: Clockwise input
int inCCWpin[2] = {8, 9}; // INCCW: Counter-clockwise input
int pwmpin[2] = {5, 6};   // PWM input
int cspin[2] = {2, 3};    // CS: Current sense ANALOG input
int enpin[2] = {0, 1};    // EN: Status of switches output (Analog pin)
  //first number controls motor 1, and the second number controls motor 2

//Variable Definitions

#define maxdistance 200
#define sonarnum 2

double toldist=5;
double minaccep;
double maxaccep;
double distanceL;
double distanceR;
double input;
double output;
double setpoint=25;

#define BRAKEVCC 0
#define CW   1
#define CCW  2
#define BRAKEGND 3
#define CS_THRESHOLD 100
  //definitions for the motor controller

//Sensory array setup

NewPing sonar[sonarnum]={
  NewPing(trigLpin,echoLpin,maxdistance),
  NewPing(trigRpin,echoRpin,maxdistance)
};
  //0th sensor is the left sensor and the 1st is the right

//PID setup

PID myPID(&input,&output,&setpoint,2,5,1,DIRECT);

//Functions

void motorOff(int motor)
{
  // Initialize braked
  for (int i=0; i<2; i++)
  {
    digitalWrite(inCWpin[i], LOW);
    digitalWrite(inCCWpin[i], LOW);
  }
  analogWrite(pwmpin[motor], 0);
}

void motorGo(uint8_t motor, double dist) {

  input=dist;
  int direct;

  if (dist<setpoint) {
    direct=1;
  }
  else {
    direct=0;  
  }

  if (direct ==1) { //backwards
    digitalWrite(inCWpin[motor], HIGH);
    myPID.SetControllerDirection(DIRECT);
  }
  else if (direct==0) { //forwards
    digitalWrite(inCCWpin[motor], HIGH);
    myPID.SetControllerDirection(REVERSE);
  }
  else {
      digitalWrite(inCCWpin[motor], LOW);
  }

  myPID.Compute();
  analogWrite(pwmpin[motor],output);

  Serial.print("Input ");
  Serial.print(input);
  Serial.print(" Setpoint ");
  Serial.print(setpoint);
  Serial.print(" Direction ");
  Serial.print(direct);
  Serial.print(" PWM ");
  Serial.println(output);
    
}


void setup() {

  //Math 

  minaccep=setpoint-toldist;
  maxaccep=setpoint+toldist;

  //Serial Initialization

  Serial.begin(9600);

  //Pin Initialization

  pinMode(statpin,OUTPUT);

  for (int i=0; i<2; i++)
  {
    pinMode(inCWpin[i],OUTPUT);
    pinMode(inCCWpin[i],OUTPUT);
    pinMode(pwmpin[i],OUTPUT);
  }
    //setup all digital pins as outputs

  //Motor Initialization
  
  for (int i=0; i<2; i++)
  {
    digitalWrite(inCWpin[i],LOW);
    digitalWrite(inCCWpin[i],LOW);
  }
    //setup motors as intially off

  myPID.SetMode(AUTOMATIC);
    //turn the PID on

}

void loop() {
  
  for (uint8_t i = 0; i < sonarnum; i++) { // Loop through each sensor and display results.
    delay(30); // Wait 30ms between pings (about 20 pings/sec). 29ms should be the shortest delay between pings.
    
    if (i==0){
      distanceL=sonar[i].ping_cm();
      /* 
      Serial.print("Left: ");
      Serial.print(sonar[i].ping_cm());
      Serial.print("cm ");
      */
    }
    else{
      distanceR=sonar[i].ping_cm();
      /*
      Serial.print("Right: ");
      Serial.print(sonar[i].ping_cm());
      Serial.print("cm ");
      */
    }
    
  }
    //this loop calculates the distance bewteen the object and each sensor
    
  /*Serial.print("Left: ");
  Serial.print(distanceL);
  Serial.print("cm ");
  Serial.print("Right: ");
  Serial.print(distanceR);
  Serial.println("cm");
    //For debugging only
  */

  if (distanceL>maxaccep | distanceL<minaccep){
    myPID.SetControllerDirection(DIRECT);
    motorGo(1,distanceL);
    delay(1);
  }
  else {
    myPID.SetOutputLimits(0.0, 1.0);  // Forces minimum up to 0.0
    myPID.SetOutputLimits(-1.0, 0.0);  // Forces maximum down to 0.0
    myPID.SetOutputLimits(0, 255);  // Set the limits back to normal
    motorOff(1);
  }
    //controlling left hand motor

  if (distanceR>maxaccep | distanceR<minaccep){
    myPID.SetControllerDirection(REVERSE);
    motorGo(0,distanceR);
    delay(1);
  }
  else {
    myPID.SetOutputLimits(0.0, 1.0);  // Forces minimum up to 0.0
    myPID.SetOutputLimits(-1.0, 0.0);  // Forces maximum down to 0.0
    myPID.SetOutputLimits(0, 255);  // Set the limits back to normal
    motorOff(0);
  }
    //controlling right hand motor
  
  if ((analogRead(cspin[0]) < CS_THRESHOLD) && (analogRead(cspin[1]) < CS_THRESHOLD))
    digitalWrite(statpin, HIGH);

}

So I edited the code a bit to include more comments about what I am doing, and I removed some redundant code. I realize that it was rather difficult to read. I also took a snapshot of the serial monitor on the output when it’s doing that weird thing.

Here is the new code:

/*
 * Following Code V3.1
 * 
 * Nickalaus Clemmer
 * Removed some unecessary code and lines of debugging code for things that already work.
 * Also added comments to try and get the code to be more readable.
 */

//******Library inclusion******

#include <NewPing.h>              //library for the ultra sonic sensor
#include <PID_v1.h>               //library for the PID control 

//******Pin Number Definitions******

#define trigLpin 3                //trigger pin for the left sensor
#define trigRpin 10               //trigger pin for the right sensor   
#define echoLpin 2                //echo pin for the left sensor
#define echoRpin 11               //echo pin for the right sensor

#define statpin 13                //motor status pin

int inCWpin[2] = {7, 4};          // INCW: Clockwise input
int inCCWpin[2] = {8, 9};         // INCCW: Counter-clockwise input
int pwmpin[2] = {5, 6};           // PWM input
int cspin[2] = {2, 3};            // CS: Current sense ANALOG input
int enpin[2] = {0, 1};            // EN: Status of switches output (Analog pin)
  /*
   * first number controls motor 1, and the second number controls motor 2
   */

//******Variable Definitions******

#define maxdistance 200           //max distance that the ultra sonic sendsors will read
#define sonarnum 2                //the number of sensors attached
#define CS_THRESHOLD 100          //some kinda over power threshold for the motor sheild

double setpoint = 25;             //distance which the bot should follow (for PID too)
double toldist = 5;               //tollerance (+-) that the bot can be within
double minaccep;                  //minimum acceptable distance to the bot
double maxaccep;                  //maximum acceptable distacne to the bot
double distanceL;                 //distance from the left side of the bot to the object
double distanceR;                 //distance from the left side of the bot to the object
double input;                     //this is the input distacne for the PID
double output;                    //this is the PWM output from the PID

//******Sensory array setup******

NewPing sonar[sonarnum] = {
  NewPing(trigLpin, echoLpin, maxdistance),
  NewPing(trigRpin, echoRpin, maxdistance)
};
  /*
   * 0th sensor is the left sensor and the 1st is the right
   */

//******PID setup******

PID myPID(&input, &output, &setpoint, 2, 5, 1, DIRECT);
  /*
   * this can be changed during opperation, but just needs to be initialized
   */

//******Functions******

void motorOff(int motor) {  
  digitalWrite(inCWpin[motor], LOW);
  digitalWrite(inCCWpin[motor], LOW);
  analogWrite(pwmpin[motor], 0);  
}
  /*
   * this function removes all power from the motor
   */

void motorGo(uint8_t motor, double dist) {
  input = dist;
  int direct;

  if (dist < setpoint) {
    direct = 1;
  }
  else {
    direct = 0;
  }

  if (direct == 1) { //backwards
    digitalWrite(inCWpin[motor], HIGH);
    myPID.SetControllerDirection(DIRECT);
  }
  else if (direct == 0) { //forwards
    digitalWrite(inCCWpin[motor], HIGH);
    myPID.SetControllerDirection(REVERSE);
  }
  else {
    digitalWrite(inCCWpin[motor], LOW);
    digitalWrite(inCWpin[motor], LOW);
  }

  myPID.Compute();
  analogWrite(pwmpin[motor], output);

  Serial.print("Input ");
  Serial.print(input);
  Serial.print(" Setpoint ");
  Serial.print(setpoint);
  Serial.print(" Direction ");
  Serial.print(direct);
  Serial.print(" PWM ");
  Serial.println(output);
}


void setup() {
  
  //******Math******

  minaccep = setpoint - toldist;  //sets the minaccept distance
  maxaccep = setpoint + toldist;  //sets the maxaccept distance

  //******Serial Initialization******

  Serial.begin(9600);
  Serial.println("Program Started");
  delay(1000);

  //******Pin Initialization******

  pinMode(statpin, OUTPUT);

  for (int i = 0; i < 2; i++)
  {
    pinMode(inCWpin[i], OUTPUT);
    pinMode(inCCWpin[i], OUTPUT);
    pinMode(pwmpin[i], OUTPUT);
  }
    /*
     * setup all digital pins as outputs
     */

  //******Motor Initialization******

  for (int i = 0; i < 2; i++)
  {
    digitalWrite(inCWpin[i], LOW);
    digitalWrite(inCCWpin[i], LOW);
  }
    /*
     * setup motors as intially off
     */

  //******PID Initialization******

  myPID.SetMode(AUTOMATIC);
    /*
     * turn the PID on
     */
     
}

void loop() {

  for (uint8_t i = 0; i < sonarnum; i++) {
    
    delay(30); 
    /*
     * Wait 30ms between pings (about 20 pings/sec).
     */
     
    if (i == 0) {
      distanceL = sonar[i].ping_cm();
    }
    else {
      distanceR = sonar[i].ping_cm();
    }

  }
    /*
     * this loop calculates the distance bewteen the object and each sensor
     */

  if (distanceL > maxaccep | distanceL < minaccep) {
    motorGo(1, distanceL);
    delay(1);
  }
  else {
    myPID.SetOutputLimits(0.0, 1.0);    // Forces minimum up to 0.0
    myPID.SetOutputLimits(-1.0, 0.0);   // Forces maximum down to 0.0
    myPID.SetOutputLimits(0, 255);      // Set the limits back to normal
    motorOff(1);
    delay(1);
  }
    /*
     * controlling left hand motor
     */

  if (distanceR > maxaccep | distanceR < minaccep) {
    motorGo(0, distanceR);
    delay(1);
  }
  else {
    myPID.SetOutputLimits(0.0, 1.0);    // Forces minimum up to 0.0
    myPID.SetOutputLimits(-1.0, 0.0);   // Forces maximum down to 0.0
    myPID.SetOutputLimits(0, 255);      // Set the limits back to normal
    motorOff(0);
    delay(1);
  }
    /*
     * controlling right hand motor
     */

  if ((analogRead(cspin[0]) < CS_THRESHOLD) && (analogRead(cspin[1]) < CS_THRESHOLD))
    digitalWrite(statpin, HIGH);
    /*
     * using the status pin
     */

}

Here is the serial monitor screen:

Input 36.00 Setpoint 25.00 Direction 0 PWM 17.50
Input 37.00 Setpoint 25.00 Direction 0 PWM 0.00
Input 35.00 Setpoint 25.00 Direction 0 PWM 15.00
Input 36.00 Setpoint 25.00 Direction 0 PWM 0.00
Input 36.00 Setpoint 25.00 Direction 0 PWM 37.50
Input 36.00 Setpoint 25.00 Direction 0 PWM 0.00
Input 35.00 Setpoint 25.00 Direction 0 PWM 15.00
Input 35.00 Setpoint 25.00 Direction 0 PWM 0.00
Input 36.00 Setpoint 25.00 Direction 0 PWM 37.50
Input 35.00 Setpoint 25.00 Direction 0 PWM 0.00
Input 36.00 Setpoint 25.00 Direction 0 PWM 27.50
Input 35.00 Setpoint 25.00 Direction 0 PWM 0.00
Input 36.00 Setpoint 25.00 Direction 0 PWM 27.50
Input 36.00 Setpoint 25.00 Direction 0 PWM 0.00
Input 37.00 Setpoint 25.00 Direction 0 PWM 40.00
Input 37.00 Setpoint 25.00 Direction 0 PWM 0.00
Input 37.00 Setpoint 25.00 Direction 0 PWM 30.00
Input 35.00 Setpoint 25.00 Direction 0 PWM 0.00
Input 35.00 Setpoint 25.00 Direction 0 PWM 5.00
Input 36.00 Setpoint 25.00 Direction 0 PWM 0.00
Input 35.00 Setpoint 25.00 Direction 0 PWM 25.00
Input 37.00 Setpoint 25.00 Direction 0 PWM 0.00
Input 37.00 Setpoint 25.00 Direction 0 PWM 50.00
Input 37.00 Setpoint 25.00 Direction 0 PWM 0.00
Input 36.00 Setpoint 25.00 Direction 0 PWM 17.50
Input 36.00 Setpoint 25.00 Direction 0 PWM 0.00
Input 36.00 Setpoint 25.00 Direction 0 PWM 27.50
Input 35.00 Setpoint 25.00 Direction 0 PWM 0.00
Input 36.00 Setpoint 25.00 Direction 0 PWM 27.50
Input 35.00 Setpoint 25.00 Direction 0 PWM 0.00
Input 36.00 Setpoint 25.00 Direction 0 PWM 27.50
Input 36.00 Setpoint 25.00 Direction 0 PWM 0.00
Input 35.00 Setpoint 25.00 Direction 0 PWM 15.00
Input 34.00 Setpoint 25.00 Direction 0 PWM 0.00

This is when the right-hand motor is too far away from the object, and the sensor can tell. However, the motor just oscillates back and forth from zero and whatever the PWM value should be. I think that it may be resetting itself every other meausrement, but I am unsure how to fix this.

Please let me know if you have any suggestions.