First, some exposition on my setup:
I have a PID algorithm controlling the rotational speed of a quadcopter motor attached to a 1-DOF pivoting arm. I am trying to get the arm-motor assembly to hover in a horizontal position. Feedback for the algorithm comes from the internal IMU of an Uno Wifi Rev2 board mounted at the pivot point so that it rotates with the arm. Acceleration in the X direction is read, and the PID algorithm is given a setpoint of Ax = 0.
The algorithm controls a fractional microsecond pulsewidth value that is rounded and written to an ESC using the writeMicroseconds() function from Servo.h. This allows for higher resolution in the algorithm even though only whole microsecond values can be written. The sketch can be seen below (PID not tuned yet, might try deriving ideal constants from equations of motion).
My issue is in the writing of the pulsewidth value to pin 10. When the value is rounded and written to the pin, it seems to only register values about 4 numbers apart. For example, an output of 1100.24 from the algorithm would be written to pin 10, then I'll read back 1100 from the pin. The output will gradually increase to 1104.00, but the value given by readMicroseconds() remains at 1100 until 1104.00 when the read value becomes 1104. This continues in seemingly regular intervals as the written value increases.
I thought this might have something to do with the microsecond range mapping to the angle representation used by the write() function, but that should cause 5.5us gaps. I'm losing resolution in my control with this happening, and every us counts since the point where the motor wants to hover is only around 12%(ish).
Has anyone experienced this before and know the cause or see anything I'm missing in my code/approach?
Thanks.
#include <Servo.h>
#include <Arduino_LSM6DS3.h>
// create servo objects to control the ESCs
Servo ESC1;
float percentPower = 0; // initialize servo power
float pwmOut = 0; // scaled power from percentage to servo signal
// set PID gain values and setpoint
float kp = 0.1;
float ki = 0;
float kd = 0;
float set = 0;
// set power limits (percent)
int maxP = 15;
int minP = 2;
// initialize PID and accel data variables
float p, d, err, errLast, Ax, Ay, Az;
float i = 0;
void setup() {
Serial.begin(9600);
IMU.begin();
// Attach ESC to pin, calibrate max and min thrust
ESC1.attach(10, 1000, 2000);
ESC1.writeMicroseconds(2000);
delay(10000);
ESC1.writeMicroseconds(1000);
delay(10000);
// Set pwm limits from percentage power limits
maxPWM = maxP * 10 + 1000;
minPWM = minP * 10 + 1000;
IMU.readAcceleration(Ax, Ay, Az); // get new accel data
err = set + Ax; // set initial error (positive bc of IMU orientation)
errLast = err; // initialize last error to current error
}
void loop() {
// calculate PID terms and updated pwm value; save last error
p = kp * err;
i = i + ki * err;
d = kd * (err - errLast);
pwmOut = pwmOut + p + i + d;
errLast = err;
// limit signal output
if (pwmOut > maxPWM) {
pwmOut = maxPWM;
}
if (pwmOut < minPWM) {
pwmOut = minPWM;
}
// write PID output to ESC
ESC1.writeMicroseconds(round(pwmOut));
IMU.readAcceleration(Ax, Ay, Az); // get new accel data
err = set + Ax; // calculate new error
Serial.print(Ax);
Serial.print("\t");
Serial.print(err);
Serial.print("\t");
Serial.print(percentPower);
Serial.print("\t");
Serial.print(pwmOut);
Serial.print("\t");
Serial.println(ESC1.readMicroseconds());
}