Reversing DC motor - Positioning PID - Display Robot

I am working on a display project, it’s a robot that turns back and forth driven by a DC motor. The frame was custom built in a build competition, so installing a new motor and power transmission system is out of the question because of things such as, the mystery drive chain master link was welded along with the mystery sprocket welded to the robot. I am using an Uno to drive the motor and other accessories on the robot (LEDs, air and water solenoids). It’s also riding on an old trailer hub, it’s not the smoothest thing but not too bad if you leave slop. I tightened up the slop so there is a little more friction to hold the robot’s position on level ground.

To interact with bystanders, I developed a sonar array object detection system using six SR04 sensors across 120 degrees, this is managed by a standalone Nano. I have 11 target positions (for each sensor, then between sensors). The nano computes the closest object then through I2C sends this target value to the Uno when requested.

I have a pot to monitor the actual position of the robot, then depending on if the target is > or < the actual position, I drive the motor in that direction, using simple while() and if() statements. The only “PID” I am using is accounting for the magnitude of error, the farther it is from target position the more power I command. The problem is, over a short distance high power is needed to get the mass moving, so once target is reached, inertia cause an overshoot; same goes for longer distances, it will still overshoot.

I am envisioning a system what will automatically throttle it, if it needs more power to move it gives it more power, if it over shoots a position it goes back or even better it doesn’t over shoot.

I looked at the PID library, but I am not sure how to implement it to handle directional error, where it’s not on/off like a temperature PID, but forward/off/reverse. Any ideas or suggestions that could solve my problem?

void loop()
{
  Serial.println("Auto Mode");
  autoMode = digitalRead(modeSwitch);
  if(coolDownCount > coolDownLimit){
    coolDownStyle++;
    if(coolDownStyle > 3){
      coolDownStyle = 1;
    }
    delay(5000);
    Serial.println("Cool Down");
    coolDownDance(coolDownStyle);
    delay(5000);
    digitalWrite(AIRout, LOW);
    digitalWrite(ARM1out, LOW);
    digitalWrite(ARM2out, LOW);
    digitalWrite(FACEout, LOW);
  }
  
  yawMotor.write(STOPSPEED);
  potYaw = getPotPos();
  
  Wire.requestFrom(2, 1); //request 2 bytes from slave device 2  
  while(Wire.available()){
    target = Wire.read();
  }
  target = map(target, 10, 60, 30, 150); //map sonar target between 30 and 150 degrees
  targetPlus = target + targetError; //creating buffer zone to stop endless looping with positioning
  targetMinus = target - targetError;
  
  potYaw = getPotPos();
  
//correct overtravels from external sources
  while(potYaw >=  155){
    potYaw = getPotPos();
    yawMotor.write(STOPSPEED + speed3);
  }
  while(potYaw <= 25){
    potYaw = getPotPos();
    yawMotor.write(STOPSPEED - speed3);
  }
  
  //start turn direction 1
  timer = millis() + timeout;
  while(potYaw > targetPlus && millis() < timer){
    Serial.println("Direction 1 loop");
    coolDownCount++;
    potYaw = getPotPos();
    
    deltaTarget = potYaw - target; //grab the difference between target and actual position
    
    if(deltaTarget >= 80){
      yawMotor.write(STOPSPEED + speed1);
    }
    if(deltaTarget >= 60 && deltaTarget < 80){
      yawMotor.write(STOPSPEED + speed2);
    }
    if(deltaTarget >= 40 && deltaTarget < 60){
      yawMotor.write(STOPSPEED + speed3);
    }
    if(deltaTarget >= 20 && deltaTarget < 40){
      yawMotor.write(STOPSPEED + speed4);
    }
    if(deltaTarget >= 10 && deltaTarget < 20){
      yawMotor.write(STOPSPEED + speed5);
    }
    if(deltaTarget < 5){
      yawMotor.write(STOPSPEED + speed6);
    }
    danceRequest = true;
  }
  yawMotor.write(STOPSPEED);
  //end turn direction 1
  
  //correct over travels
  while(potYaw >=  155){
    potYaw = getPotPos();
    yawMotor.write(STOPSPEED + speed3);
  }
  while(potYaw <= 25){
    potYaw = getPotPos();
    yawMotor.write(STOPSPEED - speed3);
  }
  
  //start turn direction 2
  timer = millis() + timeout;
  while(potYaw < targetMinus && millis() < timer){
    Serial.println("Direction 2 loop");
    coolDownCount++;
    potYaw = getPotPos();
    
    deltaTarget = target - potYaw; //grab the difference between target and actual position
    
    if(deltaTarget >= 80){
      yawMotor.write(STOPSPEED - speed1);
    }
    if(deltaTarget >= 60 && deltaTarget < 80){
      yawMotor.write(STOPSPEED - speed2);
    }
    if(deltaTarget >= 40 && deltaTarget < 60){
      yawMotor.write(STOPSPEED - speed3);
    }
    if(deltaTarget >= 20 && deltaTarget < 40){
      yawMotor.write(STOPSPEED - speed4);
    }
    if(deltaTarget >= 10 && deltaTarget < 20){
      yawMotor.write(STOPSPEED - speed5);
    }
    if(deltaTarget < 5){
      yawMotor.write(STOPSPEED - speed6);
    }
    danceRequest = true;
  }
  yawMotor.write(STOPSPEED);
  
  //correct over travels
  while(potYaw >=  155){
    potYaw = getPotPos();
    yawMotor.write(STOPSPEED + speed3);
  }
  while(potYaw <= 25){
    potYaw = getPotPos();
    yawMotor.write(STOPSPEED - speed3);
  }
  
  if(danceRequest == true){
    danceRequest = false;
    danceStyle++;
    if(danceStyle > 3){
      danceStyle = 1;
    }
    dance(danceStyle);
    digitalWrite(AIRout, LOW);
    digitalWrite(ARM1out, LOW);
    digitalWrite(ARM2out, LOW);
    digitalWrite(FACEout, LOW);
    
  }
  //end turn direction 2
  
  //++++++++++++++++++AUTO MODE END++++++++++++++++++++         ++++++++++++++++++AUTO MODE END++++++++++++++++++++
  //++++++++++++++++++AUTO MODE END++++++++++++++++++++         ++++++++++++++++++AUTO MODE END++++++++++++++++++++
  //++++++++++++++++++AUTO MODE END++++++++++++++++++++         ++++++++++++++++++AUTO MODE END++++++++++++++++++++
  
}

int getPotPos()
{
  potYaw = analogRead(potIn);
  potYaw = map(potYaw, 370, 650, 0, 180);
  return potYaw;
}

I wrote some prototype code, not a PID, just brute force positioning.

//Copyright STEELMESH 2014
#include <Wire.h>
#include <Servo.h>
Servo yawMotor;

//PIN DECLARATIONS
const int potIn = A1; //input pin for yaw position

//MANUAL CONSTANTS INPUTS
const int Dir1MaxSpeedAuto = 30;
const int Dir2MaxSpeedAuto = -30;
const int targetTolerance = 3; //tolerance allowed for positioning, actual degrees

const int stopSpeed = 90; //Motor controller stop speed, set to 90 for TI controller
const int forBoostDelay = 8; //boost delay within the boost power ramping for-loop
const int hardBrakePowerDir1 = -30; //amount to reverse motor speed for fast speeds
const int softBrakePowerDir1 = -15; //amount to reverse motor speed for slow speeds
const int hardBrakePowerDir2 = 30; //amount to reverse motor speed for fast speeds
const int softBrakePowerDir2 = 15; //amount to reverse motor speed for slow speeds
const int hardBrakeDelay = 200; //ms delay to reverse power, braking
const int softBrakeDelay = 125;

//OTHER DECLARATIONS
int lastTarget;

void setup()
{
  Wire.begin(); //I2C
  yawMotor.attach(5); //digital pin 5
  yawMotor.write(stopSpeed); //shut it down
  Serial.begin(9600);
}

void loop()
{
  yawMotor.write(stopSpeed);
  if(requestTarget() != lastTarget)
  {
    targetingSequence();
  }
  delay(20);
}

void targetingSequence()
{
  int targetSetPoint = requestTarget(); //get target from sonar
  int targetPlus = targetSetPoint + targetTolerance; //setpoint tolerance +
  int targetMinus = targetSetPoint - targetTolerance; //setpoint tolerance -
  lastTarget = targetSetPoint;
 
 //reset booleans
  boolean highSpeed = false; //was it in high speed to begin with since a target was requested?
  boolean boost = false; //boost mode for short distance movement
  boolean hardBrakeDir1 = false; //to request hard braking for long distance move
  boolean hardBrakeDir2 = false;
  boolean softBrakeDir1 = false; //to request soft braking for short distance move
  boolean softBrakeDir2 = false;
  boolean toggleHardBrake = false;
  boolean toggleSoftBrake = false;
  boolean brakeCommand = false;
  
  while(getYawPos() >= targetPlus || getYawPos() <= targetMinus) //while (outside target tolerance band)
  {
    int targetError = getYawPos() - targetSetPoint; //difference between target and actual, negative number allowed to set direction *linear relationship between error and power (more error, more power)
    unsigned int deltaError = getYawPos() - targetSetPoint; //get the magnitude of the difference, unsigned integer
    int speedSet = map(targetError, -120, 120, Dir2MaxSpeedAuto, Dir1MaxSpeedAuto); //map the error to the max error and max motor speed variables
    
    if(deltaError >= 30) //if the error is greater than or equal to 25 degrees
    {
      highSpeed == true;
      yawMotor.write(speedSet);
    }
    if(deltaError <= 30 && deltaError > 0 && highSpeed == true)
    {
      yawMotor.write(speedSet);
      if(toggleHardBrake == false) //toggleHardBrake boolean to check only once the direction of movement
      {
        if(speedSet > 0)
        {
          hardBrakeDir1 = true;
        }
        if(speedSet < 0)
        {
          hardBrakeDir2 = true;
        }
        Serial.println("Hard Brake requested");
        toggleHardBrake = true;
      }
    }
    if(deltaError <= 25 && deltaError > 0 && highSpeed == false) //short distance movement, BOOSTING MODE included
    {
        if(boost = false) //if it hasn't boosted yet, then...
        {
            if(speedSet > 0) //ramping power, direction 1
              {
                int boostSpeed = speedSet + 15;
                boostSpeed = constrain(boostSpeed, 15, 30);
                for(int forBoost = 0; forBoost <= boostSpeed; forBoost++)
                {
                  yawMotor.write(forBoost);
                  delay(forBoostDelay);
                }
                //boolean for direction 1 soft braking
              }
            if(speedSet < 0) //ramping power direction 2
              {
                int boostSpeed = speedSet - 15;
                boostSpeed = constrain(boostSpeed, -15, -30);
                for(int forBoost = 0; forBoost >= boostSpeed; forBoost--)
                {
                  yawMotor.write(forBoost);
                  delay(forBoostDelay);
                }
                //boolean for direction 2 soft braking
              }
          boost = true;
        }
      yawMotor.write(speedSet);
      if(toggleSoftBrake == false) //immediately grab direction traveling to request braking and braking direction
      {
        if(speedSet > 0)
        {
          softBrakeDir1 = true;
        }
        if(speedSet < 0)
        {
          softBrakeDir2 = true;
        }
        Serial.println("Soft Brake requested");
        toggleSoftBrake = true;
      }
    }
    
  }
  
  if(getYawPos() < targetPlus || getYawPos() > targetMinus && brakeCommand == false)
  {
  if(hardBrakeDir1 == true)
  {
    yawMotor.write(hardBrakePowerDir1);
    delay(hardBrakeDelay);
    Serial.println("Hard Brake Dir 1");
  }
  if(hardBrakeDir2 == true)
  {
    yawMotor.write(hardBrakePowerDir2);
    delay(hardBrakeDelay);
    Serial.println("Hard Brake Dir 2");
  }
  if(softBrakeDir1 == true)
  {
    yawMotor.write(softBrakePowerDir1);
    delay(softBrakeDelay);
    Serial.println("Soft Brake Dir 1");
  }
  if(softBrakeDir2 == true)
  {
    yawMotor.write(softBrakePowerDir2);
    delay(softBrakeDelay);
    Serial.println("Soft Brake Dir 2");
  }
  brakeCommand = true;
  }
}

int getYawPos()
{
  int potYaw = analogRead(potIn); //read raw pot
  potYaw = constrain(potYaw, 0, 1023);
  potYaw = map(potYaw, 370, 650, 0, 180); //convert to degrees
  Serial.print("getYawPos = ");
  Serial.println(potYaw);
  return potYaw;
}

int requestTarget()
{
  Wire.requestFrom(2, 1); //request 2 bytes from slave device 2 
  while(Wire.available()){
    int target = Wire.read();
  }
  int target = constrain(target, 10, 60);
  target = map(target, 10, 60, 30, 150); //map sonar target between 30 and 150 degrees
  Serial.print("requestTarget = ");
  Serial.println(target);
  return target;
}