2

So basically I have a DC motor that is self stabilizing a platform using an accelerometer as its input. When i hit the table, everything freaks out and the platform moves rapidly back and forth for a little bit. I would like to know if anyone has some sort of code that would filter this out.

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, 220);

      Serial.print(yAng);
      Serial.print(",");
      //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 = 2.4 * error + .01 * integral + 3.3 * deriv; //2.4; .01, 3.3
    //Serial.print(output);
    //Serial.print(",");
    //Serial.println(isr_time);
    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);
}

The code below is in the setup()

 for (int i = 0; i < 10; i++)
  {
    queue.enqueue(0);
    queue2.enqueue(0);
  }

The code below is apart of the averaging that is in the loop

if (firstQueue)
    {
        queue.dequeue();
        queue.enqueue(yAng);
        
        
      for (int i = 0; i < 10; i++) // copy the queue over
      {
        tempVal = queue.peek();
        sum = sum + tempVal;
        queue2.enqueue(tempVal);
        queue.dequeue();
      }
        yAngAverage = sum / 10;
        float pos = PID(yAngAverage);
        setMotor(pos);
        yAngAverage = 0;//set average back to 0
        firstQueue = false;
    }
    else
    {
        queue2.dequeue();
        queue2.enqueue(yAng);
      for (int i = 0; i < 10; i++) // copy the queue over
      {
        tempVal = queue2.peek();
        sum = sum + tempVal;
        queue.enqueue(tempVal);
        queue2.dequeue();
      }
        yAngAverage = sum / 10;
        float pos = PID(yAngAverage);
        setMotor(pos);
        yAngAverage = 0;//set average back to 0
        firstQueue = true; 
    }
  sum = 0;
  tempVal = 0;*/
  }

Basically it created two different queues and would switch between them averaging 10 numbers and using that as my new input value. Way too slow

  • try averaging the readings .... or read 5 times and discard highest and lowest – jsotola Oct 27 at 20:46
  • ill update my code so you can see my averaging code beneath it. It made it super duper slow. Like the code would barely work. – Thunder Dornhofer Oct 27 at 22:23
  • 2
    Without spending hours going through your code it appears the PID gain is to high. Also slow down the motor response. Be aware different motors will act different. – Gil Oct 27 at 23:41

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.