Hello everyone,
I’m working on a project that involves controlling an N20 motor using an AS5600 encoder for precise angle measurements and an MG90D motor driver. I'm utilizing the ArduPID library for PID control. My goal is to achieve smooth and accurate motor control based on the desired angles input via serial communication.
Setup:
Motor: N20 DC Motor
Encoder: AS5600 Magnetic Encoder
Motor Gears: MG90D Servo Motor (harvested from an MG90D servo)
Motor Driver: DRV8214
Library: ArduPID
Issue: When I input target angles of 0 or 180 degrees, the system works fine, and the motor moves smoothly to the desired position. However, when I input any other angle, the motor oscillates and cannot settle at the desird position. I suspect there might be an issue with my PID tuning or implementation, but I'm not sure how to resolve it. Also how can I improve my approach to make it fast?
My trials:
I tried everything from increase and trying to tune the PID to using different libraries to making sure my servo has low tolerances. It all doesn't work. I think there is something wrong my logic.
Code:
#include "ArduPID.h"
#include <Wire.h>
#define AS5600_ADDRESS 0x36 // I2C address of the AS5600 encoder, PINS A4, A5
#define RAW_ANGLE_REGISTER 0x0C
#define motor_IN1 A2
#define motor_IN2 A3
#define DEADBAND 1.0 // Define the deadband range
int MIN_PWM = 0;
ArduPID myController;
double input, output;
// Arbitrary setpoint and gains - adjust these as fit for your project:
double setpoint = 0;
double KP = 10.0;
double KI = 0.0;
double KD = 0.0;
double OUTPUT_MIN = -255;
double OUTPUT_MAX = 255;
int i = 0;
int pwmValue;
int Delta_angle;
const int numReadings = 10;
float readings[numReadings]; // the readings from the analog input
int readIndex = 0; // the index of the current reading
float total = 0; // the running total
float average = 0; // the average
void setup() {
Serial.begin(115200);
Wire.begin();
Wire.setClock(400000);
delay(500);
myController.begin(&input, &output, &setpoint, KP, KI, KD);
//myController.setSampleTime(40); // OPTIONAL - will ensure at least 50ms have past between successful compute() calls
myController.setWindUpLimits(-10, 10); // Growth bounds for the integral term to prevent integral wind-up
myController.setOutputLimits(OUTPUT_MIN, OUTPUT_MAX); // Set output limits
myController.start();
}
void loop() {
// Read the desired setpoint from serial input
while (Serial.available() > 0) {
String inputString = Serial.readStringUntil('\n');
Delta_angle = inputString.toFloat();
myController.reset();
}
double angle_error = calculateShortestPath(Delta_angle, readAngle());
input = angle_error;
myController.compute();
if (abs(input) > DEADBAND) {
moveMotor(output);
} else {
moveMotor(0);
}
serialprint();
}
float readAngle() {
unsigned long startMillis = millis();
Wire.beginTransmission(AS5600_ADDRESS);
Wire.write(RAW_ANGLE_REGISTER);
if (Wire.endTransmission() != 0) {
return -1;
}
Wire.requestFrom(AS5600_ADDRESS, 2);
while (Wire.available() < 2) {
if (millis() - startMillis > 100) {
Serial.println("I2C timeout in readAngle");
return -1;
}
}
uint8_t highByte = Wire.read();
uint8_t lowByte = Wire.read();
uint16_t rawAngle = (highByte << 8) | lowByte;
float angle = (rawAngle * 360.0) / 4096.0;
// Subtract the last reading
total = total - readings[readIndex];
// Read from the sensor
readings[readIndex] = angle;
// Add the reading to the total
total = total + readings[readIndex];
// Advance to the next position in the array
readIndex = readIndex + 1;
// If we're at the end of the array...
if (readIndex >= numReadings) {
readIndex = 0;
}
// Calculate the average
average = total / numReadings;
return average;
}
void moveMotor(float error) {
pwmValue = (int)abs(error);
if (pwmValue < MIN_PWM && pwmValue > 0) {
pwmValue = MIN_PWM;
}
if (error > 0) {
analogWrite(motor_IN1, pwmValue);
analogWrite(motor_IN2, 0);
} else if (error < 0) {
analogWrite(motor_IN1, 0);
analogWrite(motor_IN2, pwmValue);
} else {
analogWrite(motor_IN1, 0);
analogWrite(motor_IN2, 0);
}
}
void serialprint() {
if (i == 100) {
Serial.print("Angle:");
Serial.print(readAngle());
Serial.print(" ");
Serial.print("Delta_angle:");
Serial.print(Delta_angle);
Serial.print(" ");
Serial.print("INPUT:");
Serial.print(input);
Serial.print(" ");
Serial.print("SETPOINT:");
Serial.print(setpoint);
Serial.print(" ");
Serial.print("OUTPUT:");
Serial.print(output);
Serial.print(" ");
Serial.print("PWM:");
Serial.println(pwmValue);
i = 0;
} else {
i++;
}
}
double calculateShortestPath(double target_angle, double measureDelta_angle) {
double angle_error = target_angle - measureDelta_angle;
// Normalize the angle_error to be within -180 to 180 degrees
angle_error = fmod((angle_error + 180), 360) - 180;
if (angle_error < -180) {
angle_error += 360;
}
return angle_error;
}
This is 0 or 180 angle.
This is 90.
This is me asking to go to 90 with Min pmw at 200 and deadban at 2. Doesnt matter if i increase the deadband it does the same thing.