2

Running a DC motor(6.5A max input), motor driver (MD10C 7Amp peak output), and arduino Mega. Basically, I am creating a stabilized object based on the accelerometer's input. It works great but all of a sudden, very randomly, it glitches and gets stuck in whatever mode it was in last, so if it was motor A HIGH and motor B LOW at speed 200, itll just do that and I have to unplug it, put in a blank code, then my code again, then start it, itll work great then do it all over again. The time it takes for it to glitch out is random, I can't seem to find any patterns to it. here is my code

//Code for accelerometer controlling an encoded DC
#include <Encoder.h>
#include <Wire.h>
#include <MPU6050.h>
#include <QueueArray.h>

//Class to read the encoder 
Encoder myEnc(19,18);

//I2C address of the MPU-6050
const int MPU_addr=0x68;

const uint16_t t1_load = 0;

// Timer1 compare value:
// Clock frequency / Prescaler / Required frequency = Compare value
// 16 MHz          / 64        / 200 Hz              = 1250
const uint16_t t1_comp = 1250;

// Buffered data for sending/receiving to/from the ISR 
volatile bool isr_flag = false;  //Used to flag when the ISR when triggered 
volatile unsigned long isr_time = 0;  //Input updated by ISR
volatile byte isr_motor_direction; //Output calculated and updated in loop()
volatile byte isr_motor_speed = 0; // Output calculated and updated in loop()

//initializing the MPU6050 (accel/gyro input)
MPU6050 sensor;

//accerleromter in the y direction
int16_t AcY;
//not using
int16_t AcX, AcZ, GyX, GyY, GyZ, Tmp;

//new varible for mapping the accelerometer to degrees
int yAng;

//Variables to create PID method
int currentTheta;
float error;
int dt = 5;
long integral=0;
long deriv;
int previousError;

//accel on light, always want it at 0
int setpoint = 0;

//change speeds of the motor 
#define MOTOR_SPEED_PIN    3
//change direction of the motor 
#define MOTOR_DIRECTION_PIN   2 
#define MOTOR_DIRECTION_CW    LOW
#define MOTOR_DIRECTION_CCW   HIGH






void setup() 
{
  //Disable global interrupt 
  cli();
  
  // Reset Timer1 Control Reg A
  TCCR1A = 0;

  // Set CTC mode
  TCCR1B &= ~(1 << WGM13);
  TCCR1B |= (1 << WGM12);

  
  // Set to prescalar of 64
  TCCR1B &= ~(1 << CS12);
  TCCR1B |= (1 << CS11);
  TCCR1B |= (1 << CS10);

  // Reset Timer1 and set compare value
  TCNT1 = t1_load;
  OCR1A = t1_comp;

  // Enable Timer1 compare interrupt
  TIMSK1 = (1 << OCIE1A);

  // Enable global interrupt
  sei();

  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);
  
  Serial.begin(9600);

  pinMode(MOTOR_SPEED_PIN, OUTPUT);
  pinMode(MOTOR_DIRECTION_PIN, OUTPUT);

  sensor.initialize();
}







void loop()
{  
    //Begin critical section 
    //

    cli();
    bool flag = isr_flag; //Copy the ISR flag
    isr_flag = false; //Reset the ISR flag
    sei();

    //
    //End critical section 

    if (flag == true)
    {
      DebugMsgTime("PID algorithm begin");

      //Begin critical section
      //

      //Getting a copy of the data that has been modified by the ISR
      cli();
      unsigned long time = isr_time;
      sei();

      //
      //End critical section 

      static unsigned long previous_time = 0;
      previous_time = time;

      //Wire reading
      
      Wire.beginTransmission(MPU_addr);
      Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
      Wire.endTransmission(false);
      Wire.requestFrom(MPU_addr,14,true);
    
      //reading each of the 6 axis's of the MPU6050 and the temp
      AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
      //Only using AcY
      AcY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
      AcZ=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
      Tmp=Wire.read()<<8|Wire.read();  // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
      GyX=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
      GyY=Wire.read()<<8|Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
      GyZ=Wire.read()<<8|Wire.read();
  
      //mapping AcY to yAng from accel units to degrees
      yAng = map(AcY, -17000, 17000, -90, 90);

      //Calculate motor parameters locally
      long pos = PID(yAng);
      byte motor_direction = pos > 0 ? MOTOR_DIRECTION_CW : MOTOR_DIRECTION_CCW //make sure that this is correct
      pos = abs(pos);
      pos = constrain(pos, 0, 100);
      byte motor_speed = map(pos, 0, 100, 0, 200);

      //Begin Critical Section
      //

      // Update the ISR data ready for output at the next interrupt.
      // This keeps the outputs locked to each other and synchronised to the interrupt.
      cli();
      isr_motor_direction = motor_direction;
      isr_motor_speed = motor_speed;
      sei();

      //
      //End Critical Section

      DebugMsgTime("PID algorithm end");
    }  
}







ISR(TIMER1_COMPA_vect)
{
  //Output
  digitalWrite(MOTOR_DIRECTION_PIN, isr_motor_direction);
  analogWrite(MOTOR_SPEED_PIN, isr_motor_speed);

  //Input
  isr_time = micros();
  isr_flag = true;
}







//PID loop to find error between setpoint and currentTheta
long PID(int currentTheta)
{
    error = setpoint - currentTheta;
    integral = integral + error*dt;
    deriv = (error-previousError)/dt;
    long output = 1.9 * error; + .8 * integral + 40 * deriv; //2.8,.7,70 //1.3,.8,40
    previousError = error;
    return output; 
}







void DebugMsgTime(const char *msg)
{
  static unsigned long previous_time = 0;
  unsigned long time = micros();
  unsigned long delta_time = time - previous_time;
  previous_time = time;

  Serial.println(msg);
  Serial.println(time);
  Serial.println(delta_time);
}
3
  • What is that while loop for? I cannot see any sense in it, considering, that you don't have any other code inside void loop(), but outside of the while loop, and void loop() is already looping. Also you seem to be reading the raw accelerometer data, which can be quite noisy. You can use a complementary filter to do sensor fusion with the gyroscope and get more stable results. And lastly you are using a library for the MPU, but then you are reading the data directly via Wire, instead of using the libraries functions. Is there a reason for this?
    – chrisl
    Commented Sep 18, 2020 at 10:58
  • How would I use a complementary? what would that look like? Would using the library be better than reading directly from the wire? I thought they were one in the same. Commented Sep 20, 2020 at 21:40
  • You can google for the complementary filter. Basically you mix the acceleration with the integral of the gyroscope measurements to get a more stable reading. About the library: I think its the same, was just curious why you are first using the library and then not.
    – chrisl
    Commented Sep 21, 2020 at 7:49

1 Answer 1

1

What happens if the while condition fails? i.e. if millis() - time is greater than or equal to 20. Is this what you intended? If there's too much other stuff in the while loop, it may quit it and never re-enter it.

loop()

I've added a call to a function that prints debug information.

unsigned long time = 0;

void loop()
{
  DebugMsgTime("loop() begin");

  while (millis() - time < 20)
  {
    DebugMsgTime("while begin");

    time = millis();

    //delay(10);  // Short delay OK.
    delay(20);  // Long delay causes while loop to quit.

    DebugMsgTime("while end");
  }
  DebugMsgTime("loop() end");
}

DebugMsgTime()

This function prints a message along with the current time and the delta time since it was last called.

void DebugMsgTime(const char *msg)
{
  static unsigned long previous_time = 0;
  unsigned long time = millis();
  unsigned long delta_time = time - previous_time;
  previous_time = time;

  Serial.println(msg);
  Serial.println(time);
  Serial.println(delta_time);
}

Debug Output

It works great but all of a sudden, very randomly, it glitches and gets stuck in whatever mode it was in last

Basic Encoder Test:
loop() begin
0
0
while begin  // It works great...
1
1
while end
2
1
while begin
12
10
while end
34
22           // but all of a sudden, very randomly, it glitches...
loop() end   // and gets stuck in whatever mode it was in last
54
20
loop() begin
74
20
loop() end
97
23
loop() begin
118
21
loop() end

As you can see, using this while loop is a very inaccurate way to generate regular sampling times.

loop() FIX

As @FerryBig pointed out in the comments, you need to change the time comparison < at the start of the loop. You also don't need the while loop because loop() is already called cyclically. Instead, put the time comparison in an if statement, something like this which gives you a rough 20 ms:

void loop()
{
  static unsigned long previous_time = 0;
  unsigned long time = millis();
  unsigned long delta_time = time - previous_time;

  if (delta_time >= 20)
  {
    previous_time = time;

    // Do PID algorithm.
  }
}

However, for a PID controller you may need more accuracy.

Timer Interrupt

A better method would be to use a timer interrupt to call an Interrupt Service Routine (ISR) at regular intervals. This ISR would do two simple things in this order:

  1. Output: digitalWrite(motor_direction); and analogWrite(3, mtr2);
  2. Input: set a flag that indicates that the gyro Wire needs reading.

Then, when the flag is set, the main loop() would read the gyro Wire, and calculate motor_direction and mtr2 in readiness for the next interrupt.

const uint16_t t1_load = 0;

// Timer1 compare value:
// Clock frequency / Prescaler / Required frequency = Compare value
// 16 MHz          / 64        / 10 Hz              = 25000
const uint16_t t1_comp = 25000;

volatile bool isr_flag = false;       // Used to flag when the ISR has updated the data.
volatile unsigned long isr_time = 0;  // The data.

void setup()
{
  // Disable global interrupt
  cli();

  // Reset Timer1 Control Reg A
  TCCR1A = 0;

  // Set CTC mode
  TCCR1B &= ~(1 << WGM13);
  TCCR1B |= (1 << WGM12);

  // Set to prescalar of 64
  TCCR1B &= ~(1 << CS12);
  TCCR1B |= (1 << CS11);
  TCCR1B |= (1 << CS10);

  // Reset Timer1 and set compare value
  TCNT1 = t1_load;
  OCR1A = t1_comp;

  // Enable Timer1 compare interrupt
  TIMSK1 = (1 << OCIE1A);

  // Enable global interrupt
  sei();

  Serial.begin(9600);
}

void loop()
{
  //
  // BEGIN CRITICAL SECTION (Atomic)
  //

  cli();
  bool flag = isr_flag;  // Copy the ISR flag.
  isr_flag = false;      // Reset the ISR flag.
  sei();

  //
  // END CRITICAL SECTION
  //

  Serial.println(flag);

  if (flag == true)
  {
    //
    // BEGIN CRITICAL SECTION (Atomic)
    //

    // Get a copy of the data that has been modified by the ISR.
    cli();
    unsigned long time = isr_time;
    sei();

    //
    // END CRITICAL SECTION
    //

    static unsigned long previous_time = 0;
    Serial.println(time - previous_time);
    previous_time = time;
  }

  delay(25);
}

ISR(TIMER1_COMPA_vect)
{
  //TCNT1 = t1_load;  // No need to reload here because it auto reloads.
  isr_time = micros();
  isr_flag = true;  // Indicate that new data is available.
}

PID Controller

This is the full implementation of the PID controller. It should give you a solid foundation to develop the PID algorithm for your motor, and to filter the gyro inputs.

//Code for accelerometer controlling an encoded DC
#include <Encoder.h>
#include <Wire.h>
#include <MPU6050.h>
#include <QueueArray.h>

//Class to read the encoder
Encoder myEnc(19, 18);

//I2C address of the MPU-6050 idk if i need
const int MPU_addr = 0x68;

const uint16_t t1_load = 0;

// Timer1 compare value:
// Clock frequency / Prescaler / Required frequency = Compare value
// 16 MHz          / 64        / 10 Hz              = 25000
//const uint16_t t1_comp = 25000;

// If the PID algorithm takes less than 5 ms, it's possible to run Timer1 at a maximum of about 200 Hz.
// 16 MHz          / 64        / 200 Hz             = 1250
const uint16_t t1_comp = 1250;

// Buffered data for sending/receiving to/from the ISR.
volatile bool          isr_flag = false;            // Used to flag when the ISR has triggered.
volatile unsigned long isr_time = 0;                // Input updated by ISR.
volatile byte          isr_motor_direction = LOW;   // Output calculated and updated in loop().
volatile byte          isr_motor_speed = 0;         // Output calculated and updated in loop().

//initializing the MPU6050 (accel/gyro input)
MPU6050 sensor;

//accerleromter in the y direction
int16_t AcY;
//not using
int16_t AcX, AcZ, GyX, GyY, GyZ, Tmp;

//new varible for mapping the accelerometer to degrees
int yAng;

//Variables to create PID method
int currentTheta;
float error;
int dt = 5;
long integral = 0;
long deriv;
int previousError;

//accel on light, always want it at 0
int setpoint = 0;

//this is the enable pin (PWM) to run speeds to the motor
#define MOTOR_SPEED_PIN         3

//change direction of the motor
// TODO: Check whether LOW and HIGH are correct.
#define MOTOR_DIRECTION_PIN     2
#define MOTOR_DIRECTION_CW      LOW
#define MOTOR_DIRECTION_CCW     HIGH

void setup()
{
  // Disable global interrupt.
  cli();

  // Reset Timer1 Control Reg A
  TCCR1A = 0;

  // Set CTC mode
  TCCR1B &= ~(1 << WGM13);
  TCCR1B |= (1 << WGM12);

  // Set to prescalar of 64
  TCCR1B &= ~(1 << CS12);
  TCCR1B |= (1 << CS11);
  TCCR1B |= (1 << CS10);

  // Reset Timer1 and set compare value
  TCNT1 = t1_load;
  OCR1A = t1_comp;

  // Enable Timer1 compare interrupt
  TIMSK1 = (1 << OCIE1A);

  // Enable global interrupt
  sei();

  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);

  Serial.begin(9600);
  Serial.println("Basic Encoder Test:");

  //pinMode(MOTOR_SPEED_PIN, OUTPUT);  // Not needed for analogWrite().
  pinMode(MOTOR_DIRECTION_PIN, OUTPUT);

  sensor.initialize();
}

void loop()
{
  //
  // BEGIN CRITICAL SECTION (Atomic)
  //

  cli();
  bool flag = isr_flag;  // Copy the ISR flag.
  isr_flag = false;      // Reset the ISR flag.
  sei();

  //
  // END CRITICAL SECTION
  //

  //Serial.println(flag);

  if (flag == true)
  {
    DebugMsgTime("PID algorithm begin");

    //
    // BEGIN CRITICAL SECTION (Atomic)
    //

    // Get a copy of the data that has been modified by the ISR.
    cli();
    unsigned long time = isr_time;
    sei();

    //
    // END CRITICAL SECTION
    //

    static unsigned long previous_time = 0;
    //Serial.println(time);
    //Serial.println(time - previous_time);
    previous_time = time;

    //
    // Read gyro Wire.
    //

    Wire.beginTransmission(MPU_addr);
    Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
    Wire.endTransmission(false);
    Wire.requestFrom(MPU_addr, 14, true);

    //reading each of the 6 axis's of the MPU6050 and the temp
    AcX = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
    //Only using AcY
    AcY = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
    AcZ = Wire.read() << 8 | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
    Tmp = Wire.read() << 8 | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
    GyX = Wire.read() << 8 | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
    GyY = Wire.read() << 8 | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
    GyZ = Wire.read() << 8 | Wire.read();

    //mapping AcY to yAng from accel units to degrees
    yAng = map(AcY, -17000, 17000, -90, 90);

    //
    // Calculate motor parameters locally.
    //

    long pos = PID(yAng);
    byte motor_direction = pos > 0 ? MOTOR_DIRECTION_CW : MOTOR_DIRECTION_CCW;  // TODO: Check whether defines are correct.
    pos = abs(pos);
    pos = constrain(pos, 0, 100);
    byte motor_speed = map(pos, 0, 100, 0, 200);

    //
    // BEGIN CRITICAL SECTION (Atomic)
    //

    // Update the ISR data ready for output at the next interrupt.
    // This keeps the outputs locked to each other and synchronised to the interrupt.
    cli();
    isr_motor_direction = motor_direction;
    isr_motor_speed = motor_speed;
    sei();

    //
    // END CRITICAL SECTION
    //

    DebugMsgTime("PID algorithm end");
  }

  //delay(1);
}

ISR(TIMER1_COMPA_vect)
{
  // Output.
  digitalWrite(MOTOR_DIRECTION_PIN, isr_motor_direction);
  analogWrite(MOTOR_SPEED_PIN, isr_motor_speed);

  // Input.
  isr_time = micros();
  isr_flag = true;
}

//PID loop to find error between setpoint and currentTheta
long PID(int currentTheta)
{
  error = setpoint - currentTheta;
  integral = integral + error * dt;
  deriv = (error - previousError) / dt;
  long output = 1.9 * error + .8 * integral + 40 * deriv; //2.8,.7,70 //1.3,.8,40
  previousError = error;
  return output;
}

void DebugMsgTime(const char *msg)
{
  static unsigned long previous_time = 0;
  unsigned long time = micros();
  unsigned long delta_time = time - previous_time;
  previous_time = time;

  Serial.println(msg);
  Serial.println(time);
  Serial.println(delta_time);
}

And I think there might be a typo in your PID controller:

long PID(int currentTheta)
{
    ...
    long output = 1.9*error; + .8*integral + 40*deriv;
                           |
                         typo?

This is equivalent to:

    long output = 1.9*error;

Which is not a PID controller.

14
  • For some reason that needs to be there. If i dont have that semi colon there, it wont work. I had that while loop to run it every 20 ms. I think someone else on here told me that using that way instead of delay was better. It made it so a time was controlling each run time instead of every time it got information. Commented Sep 20, 2020 at 21:43
  • 1
    @ThunderDornhofer The semicolon will end the expression there. So the rest of the line has no effect. What exactly do you mean by "if i don't have that semicolon there, it won't work"? How exactly does it now work?
    – chrisl
    Commented Sep 21, 2020 at 7:50
  • @ThunderDornhofer, I've updated my post to show why your while loop doesn't work. A better method to generate regular time intervals is to use timer interrupts.
    – tim
    Commented Sep 21, 2020 at 11:40
  • 1
    @tim the loop still breaks in the way you explained, but the loop is fixed by changing < to >, as that wil l prevent going into the loop for 20 MS< and only then go into the loop, resetting the "timer"
    – Ferrybig
    Commented Sep 21, 2020 at 12:34
  • 1
    @ThunderDornhofer, I've added a full implementation of the PID controller. There are a couple of TODO comments you need to verify are correct. Not sure what frequency you want to run the timer at, but I got about 200 Hz max. Hope that helps.
    – tim
    Commented Sep 24, 2020 at 0:05

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.