Hi,
I am using an UNO R3 on IDE 1.8.13 on windows.
I am trying to get a DC motor (large gearing ratio so it turns pretty slowly, only a few rpms) to turn to a specific angle which is measured by a magnetic encoder, specifically the as5600. I am using PIDv1.h library to adjust the motor based on input from the as5600.
AS5600 datasheet: https://ams.com/documents/20143/36005/AS5600_DS000365_5-00.pdf/649ee61c-8f9a-20df-9e10-43173a3eb323
PID Library: Arduino Playground - HomePage
The issue that I am running into is that the sensor will "wrap-around" from 0-4095 and vice versa. When this happens the error calculated by the PID library will cause a change in direction forcing the motor to turn all the way around again to try and meet the setpoint.
For example: if the setpoint is close to either 0 or 4095, if the slightest bit of overshoot happens - passes 4095 and goes to 0, 1, or 2. Rather than simply reverse directions a few positions, it will continue in the same direction because the error changes signs.
Here is the code I am currently working with:
#include <PID_v1.h>
#include <A4990DualMotorDriverCarrier.h>
#include <Wire.h>
#include <AS5600.h>
const int BUFFER_SIZE = 8;
char opCode[BUFFER_SIZE];
int dest = 0;
double Setpoint, Input, Output;
//Specify the links and initial tuning parameters
double Kp = 1, Ki = 0, Kd = 0;
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);
//Specific Class declarations
A4990DualMotorDriverCarrier motors(3, 11, 9, 10); //Motor Driver pins (need to be PWM capable)
AMS_5600 ams5600; //Magnetic Encoder
void setup()
{
Serial.begin(9600);
Wire.begin();
//Initialize and detect ams5600 magnetic encoder
if (ams5600.detectMagnet() == 1 )
{
Serial.println(F("Pan Detected."));
Serial.print(F("Pan - "));
switch (int x = ams5600.getMagnetStrength())
{
case 0:
Serial.println(F("No Magnet."));
break;
case 1:
Serial.println(F("Magnet too weak."));
break;
case 2:
Serial.println(F("Magnet just right."));
break;
case 3:
Serial.println(F("Magnet too strong."));
break;
}
}
else
{
Serial.println(F("Can not detect pan magnet"));
}
motors.setM1Speed(0);
Setpoint = 1000;
//turn the PID on
myPID.SetOutputLimits(-255, 255);
myPID.SetSampleTime(10);
myPID.SetMode(AUTOMATIC);
}
void loop()
{
//Used to change the setpoint via serial for testing purposes
if (Serial.available() > 0)
{
int incData = Serial.readBytesUntil('\n', opCode, BUFFER_SIZE);
if (incData > 0)
{
for (int i = 0; i < incData; i++)
{
switch (i)
{
case 0:
if (opCode[0] == 'S')
break;
case 1:
dest = opCode[i] - '0';
break;
case 2:
dest = (dest * 10) + (opCode[i] - '0');
break;
case 3:
dest = (dest * 10) + (opCode[i] - '0');
break;
case 4:
dest = (dest * 10) + (opCode[i] - '0');
break;
}
}
Setpoint = constrain(dest, 0, 4095);
}
}
Input = ams5600.getRawAngle();
myPID.Compute();
if (Output > 0)
Output = map(Output, 0, 255, 20, 255);
else if (Output < 0)
Output = map(Output, -255, 0, -255, -20);
motors.setM1Speed(-Output); //Output gets inverted to correct motor direction
Serial.print(Input); Serial.print(" "); Serial.println(Output);
}
Any suggestions to deal with this?