Can someone write me a PID algorithm to keep the pendulum upright? I am struggling with this.
My servo goes from 0 to 270 degrees (0 degrees is all the way right, 270 is all the way left).
The pendulum is upright at the encoder position of 0 degrees.
The variables are servoPosition and pendulumAngle.
What I have so far is:
(keep in mind most of this is to read the encoder position)
/* Include the SPI library for the Arduino boards */
#include <SPI.h>
#include <Servo.h>
Servo myservo; // create Servo object to control a servo
// twelve Servo objects can be created on most boards
int pos = 0; // variable to store the servo position
#define CONTROL_THRESHOLD 15 // Only apply control if pendulum is within ±15 degrees of vertical
/* PID Constants for Pendulum Stabilization */
float Kp_pendulum = -1.5, Ki_pendulum = 2, Kd_pendulum = 0;
float pendulumSetpoint = 0; // Set pendulum upright reference to 133 degrees
float prevPendulumError = 0, integralPendulum = 0;
/* Serial rates for UART */
#define BAUDRATE 115200
/* SPI commands */
#define AMT22_NOP 0x00
#define AMT22_ZERO 0x70
#define AMT22_TURNS 0xA0
uint8_t cs_pins[] = {2}; // Only one encoder connected, using pin 2 on Arduino for CS
int RESOLUTION = 12;
float position_degrees;
/*
* Using the equation on the datasheet we can calculate the checksums and then make sure they match what the encoder sent.
*/
bool verifyChecksumSPI(uint16_t message)
{
// Checksum is the invert of XOR of bits, so start with 0b11, so things end up inverted
uint16_t checksum = 0x3;
for (int i = 0; i < 14; i += 2)
{
checksum ^= (message >> i) & 0x3;
}
return checksum == (message >> 14);
}
float unwrapAngle(float angle) {
static float lastAngle = 0;
static float revolutions = 0;
float delta = angle - lastAngle;
if (delta > 180) revolutions -= 360; // Wrapped from 359 to 0
if (delta < -180) revolutions += 360; // Wrapped from 0 to 359
lastAngle = angle;
return angle + revolutions;}
void setup()
{
uint8_t cs_pin = 2;
myservo.attach(9); // attaches the servo on pin 9 to the Servo object
// Set the modes for the SPI CS
pinMode(cs_pin, OUTPUT);
// Get the CS line high which is the default inactive state
digitalWrite(cs_pin, HIGH);
// Initialize the UART serial connection for debugging
Serial.begin(BAUDRATE);
// Set the clock rate. Uno clock rate is 16Mhz, divider of 32 gives 500 kHz.
SPI.setClockDivider(SPI_CLOCK_DIV32); // 500 kHz
// Start SPI bus
SPI.begin();
}
void loop()
{
for (int encoder = 0; encoder < sizeof(cs_pins); ++encoder)
{
uint8_t cs_pin = cs_pins[encoder];
// Set the CS signal to low
digitalWrite(cs_pin, LOW);
delayMicroseconds(3);
// Read the two bytes for position from the encoder, starting with the high byte
uint16_t encoderPosition = SPI.transfer(AMT22_NOP) << 8; // Shift up 8 bits because this is the high byte
delayMicroseconds(3);
encoderPosition |= SPI.transfer(AMT22_NOP); // No-op command to get the encoder position
// Set the CS signal to high
digitalWrite(cs_pin, HIGH);
if (verifyChecksumSPI(encoderPosition)) // Position was good, print to serial stream
{
encoderPosition &= 0x3FFF; // Discard upper two checksum bits
if (RESOLUTION == 12) encoderPosition = encoderPosition >> 2; // On a 12-bit encoder, the lower two bits will always be zero
// Convert position to degrees
position_degrees = (encoderPosition * 360.0) / (1 << RESOLUTION);
Serial.print("Encoder #");
Serial.print(encoder, DEC);
Serial.print(" position: ");
Serial.print(position_degrees, 2); // Print the position in degrees with two decimal places
Serial.println(" degrees");
}
else // Position is bad, let the user know
{
Serial.print("Encoder #");
Serial.print(encoder, DEC);
Serial.println(" position error.");
}
}
// Simulated pendulum angle (assume IMU input in future)
float pendulumAngle = unwrapAngle(position_degrees) - 329.3;
// Calculate PID for Pendulum
float pendulumError = pendulumSetpoint - pendulumAngle;
integralPendulum += pendulumError;
float derivativePendulum = pendulumError - prevPendulumError;
float pendulumPID = Kp_pendulum * pendulumError + Ki_pendulum * integralPendulum + Kd_pendulum * derivativePendulum;
prevPendulumError = pendulumError;
// Combine both PID outputs
float controlSignal =pendulumPID;
// Set servo position based on control output
int servoPosition = map(constrain(pendulumPID, -45, 45), -45, 45, 0, 270);
myservo.write(servoPosition*2/3);
// Debug Output
Serial.print("\tPendulum Angle: "); Serial.print(pendulumAngle);
Serial.print("\tServo: "); Serial.println(servoPosition);
// Delay between reads
delay(20);
}