Howdy all.
Been struggling with this for a week now, any advice or direction would be appreciated. I have an inverted pendelum mechanical system with a motor atop spinning a decent moment of inertia - the angular acceleration of this reaction wheel should balance the inverted pendulum (think self balancing bicycle robot etc) . For this I am using a PID control to control overshoot/provide smoothing and an overall balance. My issue is that the brushed DC motor, having that mass/moment of interia on the shaft, has some degree of static friction (output of 0-40 / 255 does not move the motor). This has proven very troublesome to overcome with the PID.
I have approached the problem by simply providing a deadband of +-40 so that any output value >0 is increased by 40, and any <0 is decreased by 40. This results in violent twitching of the system about the setpoint - progressively larger and violent overshoots. My goal is balance like this video (03:40):
https://www.youtube.com/watch?v=4kfBEaTncjI
I am using the same motor and motor controller as him as well. Any thoughts? Thanks guys
Another note: I think this setup has some nasty noise coming from the motor leads or something it's very common for the system to, upon a very rapid change in speed or direction and therefore an increased line current, go crazy and flash randomly between -255, 255, and 0 and trip my Supply safeties.
Here's a schematic of the wiring and my code:
Brushed (Geartrain) DC Motor Controlled by Ln298N motor driver
MPU6050 Gyro/Accel
Arduino Nano
12V DC Power Supply (Suppling 14V to overcome 2V drop by Ln298N)
#include "I2Cdev.h"
#include <PID_v1.h>
#include "MPU6050_6Axis_MotionApps20.h"
MPU6050 mpu;
bool dmpReady = false;
uint8_t mpuIntStatus;
uint8_t devStatus;
uint16_t packetSize;
uint16_t fifoCount;
uint8_t fifoBuffer[64];
Quaternion q;
VectorFloat gravity;
float ypr[3];
double setpoint = 178.5;
double Kp = 10;
double Kd = 0;
double Ki = 0;
double torqueOffset = 40;
double accelerometerAngle = 0;
const double alpha = 0.98;
const int IN1 = 8;
const int IN2 = 7;
const int EN1 = 9;
double input, output;
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);
volatile bool mpuInterrupt = false;
void dmpDataReady()
{
mpuInterrupt = true;
}
void setup()
{
Serial.begin(115200);
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
devStatus = mpu.dmpInitialize();
mpu.setXGyroOffset(45);
mpu.setYGyroOffset(46);
mpu.setZGyroOffset(64);
mpu.setXAccelOffset(-1468);
mpu.setYAccelOffset(716);
mpu.setZAccelOffset(639);
if (devStatus == 0)
{
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
Serial.println(F("DMP ready! Waiting for first interrupt..."));
dmpReady = true;
packetSize = mpu.dmpGetFIFOPacketSize();
pid.SetMode(AUTOMATIC);
pid.SetSampleTime(1);
pid.SetOutputLimits(-255 + torqueOffset, 255 - torqueOffset);
}
else
{
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
digitalWrite(IN1, LOW);
digitalWrite(IN2, LOW);
}
double adjustedOutput = 0;
double lastPitch = 0;
void loop()
{
unsigned long currentMillis = millis();
if (!dmpReady)
return;
while (!mpuInterrupt && fifoCount < packetSize)
{
pid.Compute();
Serial.print(input);
Serial.print("\t");
Serial.print(output);
if (output > 0)
{
adjustedOutput = output + torqueOffset;
}
else
{
adjustedOutput = output - torqueOffset;
}
Serial.print("\t");
Serial.println(adjustedOutput);
if (input > 120 && input < 250)
{
if (adjustedOutput > 0)
Forward();
else if (adjustedOutput < 0)
Reverse();
}
else
{
adjustedOutput = 0;
Stop();
}
}
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
fifoCount = mpu.getFIFOCount();
if ((mpuIntStatus & 0x10) || fifoCount == 1024)
{
mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));
}
else if (mpuIntStatus & 0x02)
{
while (fifoCount < packetSize)
fifoCount = mpu.getFIFOCount();
mpu.getFIFOBytes(fifoBuffer, packetSize);
fifoCount -= packetSize;
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
input = ypr[1] * 180 / M_PI + 180;
}
//prevent motor from rapidly occilating between -255, 0, and 255 randomly once PID output calls for 255 or -255
if (currentMillis >= 1000)
{
if (abs(input - lastPitch) > 100.00)
{
while (true)
{
Stop();
}
}
}
lastPitch = input;
}
void Forward()
{
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
analogWrite(EN1, adjustedOutput);
Serial.print("F");
}
void Reverse()
{
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
analogWrite(EN1, adjustedOutput * -1);
Serial.print("R");
}
void Stop()
{
digitalWrite(IN1, LOW);
digitalWrite(IN2, LOW);
Serial.print("S");
}