Uncontrollable speed shootup from interaction of multiple PIDs

Hi,

I have a project where I am supposed to synchronize the positions of two given motors at any point of time. I have achieved the same but notice that when I try to input the speed, randomly the motors run at full throttle even though it is constrained by the PID limits. Then the serial window stops responding and it is a dangerous situation which requires me to manually turn off power to the motors. Could you please help in figuring out if I have overlooked anything? THe code exceeded the 9000 character limit hence I have attached it with this.

Combined.ino (9.62 KB)

#include <Wire.h>
#include <PID_v1.h>
#include <AS5147P.h>
#include <Servo.h>

#define phasePotPin   A0
#define triggerPotPin A1
#define triggerPin    23

/* Anti clockwise rotation Bot Motor pins */
#define botMotorPin  11
#define encoderPinABot 33
#define encoderPinBBot 31
#define encoderPinIBot 35

/* Clockwise rotation Top Motor pins */
#define topMotorPin  12
#define encoderPinATop 43
#define encoderPinBTop 41
#define encoderPinITop 45

/* Encoders' SPI pins */
#define csnBot 29
#define csnTop 47
#define miso MISO
#define mosi MOSI
#define sclk SCK

/* Bot motor RPM PID variables */
double botMotorPIDInput;
double botMotorPIDOutput;
double botMotorPIDSetpoint;
double Kp1 = 0.2;
double Ki1 = 0.6;
double Kd1 = 0;

/* Top motor RPM PID variables */
double topMotorPIDInput;
double topMotorPIDOutput;
double topMotorPIDSetpoint;
double Kp2 = 0.2;
double Ki2 = 0.6;
double Kd2 = 0;

/* Bot Motor Yaw PID variables */
double yawPIDInputBot;
double yawPIDOutputBot;
double yawPIDSetpointBot;
double Kp3 = 0.1;
double Ki3 = 0.7;
double Kd3 = 0;

/* Top Motor Yaw PID variables */
double yawPIDInputTop;
double yawPIDOutputTop;
double yawPIDSetpointTop;
double Kp4 = 0.1;
double Ki4 = 0.7;
double Kd4 = 0;

int totalPIDBot;
int totalPIDTop;

volatile unsigned int Aold1 = 0;
volatile unsigned int Bnew1 = 0;
volatile unsigned int Aold2 = 0;
volatile unsigned int Bnew2 = 0;

uint32_t time_now = 0;
uint32_t currentTime = 0;

volatile int32_t dTimeBot = 0;
volatile int32_t dTimeTop = 0;
volatile uint32_t cTimeBot = 0;
volatile uint32_t cTimeTop = 0;
volatile uint32_t lTimeBot = 0;
volatile uint32_t lTimeTop = 0;

int topMotorSpeed;
int botMotorSpeed;
int inputSpeed;
int botMotorCount;
int topMotorCount;
int botMotorPos;
int topMotorPos;

int triggerPos;
int triggerRange = 5;
int phaseOffset;

volatile bool encoderBoolBot = true;
volatile bool encoderBoolTop = true;

double Multiplier = ((60 * 1000000)/1024);

volatile float SpeedInRPMBot = 0;
volatile float SpeedInRPMTop = 0;
volatile int encoderCountBot = 0;
volatile int encoderCountTop = 0;

byte myArray[4];

PID botMotorPID(&botMotorPIDInput,&botMotorPIDOutput,&botMotorPIDSetpoint,Kp1,Ki1,Kd1,DIRECT);
PID topMotorPID(&topMotorPIDInput,&topMotorPIDOutput,&topMotorPIDSetpoint,Kp2,Ki2,Kd2,DIRECT);
PID yawPIDBot(&yawPIDInputBot,&yawPIDOutputBot,&yawPIDSetpointBot,Kp3,Ki3,Kd3,DIRECT);
PID yawPIDTop(&yawPIDInputTop,&yawPIDOutputTop,&yawPIDSetpointTop,Kp4,Ki4,Kd4,DIRECT);
AS5147P as5147pBot(csnBot,mosi,miso,sclk);
AS5147P as5147pTop(csnTop,mosi,miso,sclk);
Servo botMotor;
Servo topMotor;

void setup(){
  
  Serial.begin(115200);   
  
  Wire.begin(8);
  Wire.setClock(1000000);
  Wire.onRequest(requestEvent);
  pinMode(triggerPin,OUTPUT);
  
  /* Bottom motor Interrupt ABI pins */
  pinMode(encoderPinABot,INPUT);
  pinMode(encoderPinBBot,INPUT);  
  pinMode(encoderPinIBot,INPUT);
  
  /* Top motor Interrupt ABI pins */
  pinMode(encoderPinATop,INPUT);
  pinMode(encoderPinBTop,INPUT);  
  pinMode(encoderPinITop,INPUT);

  /* Bottom motor interrupts */
  attachInterrupt(digitalPinToInterrupt(encoderPinABot),doEncoderABot,CHANGE);
  attachInterrupt(digitalPinToInterrupt(encoderPinBBot),doEncoderBBot,CHANGE);
  attachInterrupt(digitalPinToInterrupt(encoderPinIBot),doEncoderIBot,FALLING);

  /* Top motor interrupts */
  attachInterrupt(digitalPinToInterrupt(encoderPinATop),doEncoderATop,CHANGE);
  attachInterrupt(digitalPinToInterrupt(encoderPinBTop),doEncoderBTop,CHANGE);
  attachInterrupt(digitalPinToInterrupt(encoderPinITop),doEncoderITop,FALLING);
  
  as5147pBot.startup();
  as5147pTop.startup();
  
  botMotorPID.SetMode(AUTOMATIC);
  botMotorPID.SetOutputLimits(1120,1350);  
    
  topMotorPID.SetMode(AUTOMATIC);
  topMotorPID.SetOutputLimits(1120,1350);
  
  yawPIDBot.SetMode(AUTOMATIC);
  yawPIDBot.SetOutputLimits(0,20);
  
  yawPIDTop.SetMode(AUTOMATIC);
  yawPIDTop.SetOutputLimits(0,20);

  botMotor.attach(botMotorPin);
  topMotor.attach(topMotorPin);
  
  /* ESC calibration */
  botMotor.write(0);
  topMotor.write(0);
  delay(6000); 
  Serial.println("Initializing, please wait...");
}

/* Interrupt on A changing state for bot motor */
void doEncoderABot() {
  if(encoderBoolBot){
    Bnew1^Aold1 ? encoderCountBot++ : encoderCountBot--;
    Aold1 = digitalRead(encoderPinABot);
  }else{
    Bnew1^Aold1 ? encoderCountBot-- : encoderCountBot++;
    Aold1 = digitalRead(encoderPinABot);
  }
}

/* Interrupt on A changing state for bot motor */
void doEncoderBBot() {
  if(encoderBoolBot){
    Bnew1 = digitalRead(encoderPinBBot);
    Bnew1^Aold1 ? encoderCountBot++ : encoderCountBot--;
  }else{
    Bnew1 = digitalRead(encoderPinBBot);
    Bnew1^Aold1 ? encoderCountBot-- : encoderCountBot++;
  }
}

/* Interrupt on I changing state for bot motor */
void doEncoderIBot(){
  cTimeBot = micros();
  dTimeBot = cTimeBot - lTimeBot;
  lTimeBot = cTimeBot;
  
  if(encoderBoolBot){
    encoderCountBot = 4096;
  }else{
    encoderCountBot = 0;
  }
  encoderBoolBot = !encoderBoolBot;
}

/* Interrupt on A changing state for top motor */
void doEncoderATop() {
  if(encoderBoolTop){
    Bnew2^Aold2 ? encoderCountTop++ : encoderCountTop--;
    Aold2 = digitalRead(encoderPinATop);
  }else{
    Bnew2^Aold2 ? encoderCountTop-- : encoderCountTop++;
    Aold2 = digitalRead(encoderPinATop);
  }
}

/* Interrupt on B changing state for top motor */
void doEncoderBTop() {
  if(encoderBoolTop){
    Bnew2 = digitalRead(encoderPinBTop);
    Bnew2^Aold2 ? encoderCountTop++ : encoderCountTop--;
  }else{
    Bnew2 = digitalRead(encoderPinBTop);
    Bnew2^Aold2 ? encoderCountTop-- : encoderCountTop++;
  }
}

/* Interrupt on I changing state for top motor */
void doEncoderITop(){
  cTimeTop = micros();
  dTimeTop = cTimeTop - lTimeTop;
  lTimeTop = cTimeTop;
  
  if(encoderBoolTop){
    encoderCountTop = 4096;
  }else{
    encoderCountTop = 0;
  }
  encoderBoolTop = !encoderBoolTop;
}

/* IIC message */
void requestEvent(){
  myArray[0] = (totalPIDBot >> 8) & 0xFF;
  myArray[1] = totalPIDBot & 0xFF;
  myArray[2] = (totalPIDTop >> 8) & 0xFF;
  myArray[3] = totalPIDTop & 0xFF;
  Wire.write(myArray,4); 
}

void loop(){  
  
  currentTime = micros();
  if(currentTime < 2*dTimeBot + lTimeBot){
    SpeedInRPMBot = Multiplier/dTimeBot;
  }else{
    SpeedInRPMBot = 0;
  }
  if(currentTime < 2*dTimeTop + lTimeTop){
    SpeedInRPMTop = Multiplier/dTimeTop;
  }else{
    SpeedInRPMTop = 0;
  }
  
  if(Serial.available()){
    String incoming = Serial.readString();
    inputSpeed = incoming.toInt();
  } 
    
  botMotorSpeed = SpeedInRPMBot*1000;
  topMotorSpeed = SpeedInRPMTop*1000;
  botMotorCount = encoderCountBot;
  topMotorCount = encoderCountTop;    
    
  if((encoderBoolBot && encoderBoolTop) || (encoderBoolTop && encoderBoolBot)){
    botMotorCount = map(botMotorCount,0,4096,0,360);
    topMotorCount = map(topMotorCount,0,4096,0,360);
  }
  
  if((!(encoderBoolBot && encoderBoolTop)) || (!(encoderBoolBot && encoderBoolTop))){
    botMotorCount = map(botMotorCount,4096,0,360,0);
    topMotorCount = map(topMotorCount,4096,0,360,0); 
  }

  if((encoderBoolBot && !encoderBoolTop) || (encoderBoolTop && !encoderBoolBot)){
    botMotorCount = map(botMotorCount,0,4096,0,360);
    topMotorCount = map(topMotorCount,0,4096,0,360);
  }

  if((!encoderBoolBot && encoderBoolTop) || (!encoderBoolTop && !encoderBoolBot)){
    botMotorCount = map(botMotorCount,4096,0,360,0);
    topMotorCount = map(topMotorCount,4096,0,360,0);
  }
  
  phaseOffset = readPhaseOffsetPot();
  triggerPos = readTriggerPot();
  botMotorPos = as5147pBot.getPosition();
  botMotorPos = map(botMotorPos,0,3600,0,360);
  topMotorPos = as5147pTop.getPosition();
  topMotorPos = map(topMotorPos,0,3600,0,360);

  /* Top motor PID */
  topMotorPIDInput = topMotorSpeed;
  topMotorPIDSetpoint = inputSpeed;
  topMotorPID.Compute();

  /* YAW PID top */
  yawPIDInputTop = topMotorCount;
  yawPIDSetpointTop = botMotorCount;
  yawPIDTop.Compute();

  totalPIDTop = topMotorPIDOutput + yawPIDOutputTop;
  
  /* Top motor clockwise */
  //topMotor.writeMicroseconds(totalPIDTop);
    
  /* Bottom motor PID */
  botMotorPIDInput = botMotorSpeed;
  botMotorPIDSetpoint = inputSpeed;
  botMotorPID.Compute();
  
  /* YAW PID bottom */
  yawPIDInputBot = botMotorCount;
  yawPIDSetpointBot = topMotorCount;
  yawPIDBot.Compute();

  totalPIDBot = botMotorPIDOutput + yawPIDOutputBot;
  
  /* Bottom motor anti-clockwise */
  //botMotor.writeMicroseconds(totalPIDBot);
  
}

You don't seem to have any Serial.print statements to allow you to see how the calculations are progressing.

Reading in data with Serial.readString() can be very slow and is not really suitable for a responsive program with a lot of other work to do.

It is not a good idea to use the String (capital S) class on an Arduino as it can cause memory corruption in the small memory on an Arduino. This can happen after the program has been running perfectly for some time. Just use cstrings - char arrays terminated with '\0' (NULL).

Have a look at the examples in Serial Input Basics - simple reliable no-blocking ways to receive data. There is also a parse example to illustrate how to extract numbers from the received text.

The technique in the 3rd example will be the most reliable .

You seem to be calculating yawPID separately for the two motors. I suspect it would be much better to apply the same yaw correction to both motors.

...R

Thank you for the input. I removed the Serial print statements to accommodate the code here. I will look into the replacing the String with what you suggested. I would like to mention that I am using the Due, and when tried with the Mega, faced the same issue. is it possible that the PIDs are interfering with each other?

An update, I tried the method suggested for the non blocking Serial read but had the same problem after a particular speed it just went bonkers. I even removed the Serial way of inputting the speed and tried a potentiometer. all resulted in the same problem of the motors shooting up to the maximum speed after a while. Any more thought as to why this is happening?

Everything seems fine when I disconnect the A,B Inputs upon which i can reach any speed. But my entire synchronization strategy depends on the A and B signals of the rotary encoder.

shankarkumarj:
Everything seems fine when I disconnect the A,B Inputs upon which i can reach any speed. But my entire synchronization strategy depends on the A and B signals of the rotary encoder.

I can't think of anything else to suggest. I don't have your hardware so I cannot carry out tests.

The usual way to debug a problem is to form a hypothesis about the cause of the problem and then add some code to prove or disprove the hypothesis.

It is usually impossible to print out all the values for every iteration of loop() because it is just too fast. I have found it useful to collect the values from (say) 100 iterations into an array and when it is full print the array.

Another option is to set up a non-blocking timer with millis() and print values every (say) 250 millisecs.

...R

Could it be that the Arduino is resetting for some reason? This happens when I go to higher speeds, so something might be causing the control signal to the ESC to be lost and the ESC just randomly draws full power from the power supply?

shankarkumarj:
Could it be that the Arduino is resetting for some reason?

That is a testable hypothesis. Add some code to print a message when setup() starts.

...R