Post
by Youngjae Bae » Wed Jul 08, 2020 6:04 am
Below is a code table of Arduino (due) regarding the 3-axis camera stabilizer.
I recently learned about the strength of Kalman Filter while studying stabilization, and I share it because it is worth referring to.
The gimbal works very well in this camera.
Youngjae
====================================================================================================================
#include <EEPROM.h>
#include <Kalman.h>
#include <MPU9250.h>
//Kalman filter objects created
Kalman kalmanX;
Kalman kalmanY;
Kalman kalmanZ;
// MPU9250 object created.
#define SCL 700000 // Pre-defined serial clock rate
#define SDA Wire // Use the built-in Wire Library for I2C communication
#define MPU9250_ADDRESS 0x68 // Address for using MPU9250 as 5V
MPU9250 IMU(MPU9250_ADDRESS, SDA, SCL);
int status;
double timer;
double pitch, roll, yaw;
float ax, ay, az, accX, accY, accZ;
float gxb, gyb, gzb, gx, gy, gz, mxb, myb, mzb, mxs, mys, mzs;
double magx, magy, magz;
double r2d = (180 / M_PI);
double mx, my, mz;
double freq = 500;
int sineArraySize;
int table[] = {128, 130, 132, 134, 136, 138, 139, 141,
143, 145, 147, 149, 151, 153, 155, 157,
159, 161, 163, 165, 167, 169, 171, 173,
174, 176, 178, 180, 182, 184, 185, 187,
189, 191, 192, 194, 196, 198, 199, 201,
202, 204, 206, 207, 209, 210, 212, 213,
215, 216, 218, 219, 220, 222, 223, 224,
226, 227, 228, 229, 231, 232, 233, 234,
235, 236, 237, 238, 239, 240, 241, 242,
243, 244, 245, 245, 246, 247, 247, 248,
249, 249, 250, 250, 251, 251, 252, 252,
253, 253, 253, 254, 254, 254, 254, 255,
255, 255, 255, 255, 255, 255, 255, 255,
255, 255, 254, 254, 254, 254, 253, 253,
253, 252, 252, 251, 251, 250, 250, 249,
249, 248, 247, 247, 246, 245, 245, 244,
243, 242, 241, 240, 239, 238, 237, 236,
235, 234, 233, 232, 231, 229, 228, 227,
226, 224, 223, 222, 220, 219, 218, 216,
215, 213, 212, 210, 209, 207, 206, 204,
202, 201, 199, 198, 196, 194, 192, 191,
189, 187, 185, 184, 182, 180, 178, 176,
174, 173, 171, 169, 167, 165, 163, 161,
159, 157, 155, 153, 151, 149, 147, 145,
143, 141, 139, 138, 136, 134, 132, 130,
128, 125, 123, 121, 119, 117, 116, 114,
112, 110, 108, 106, 104, 102, 100, 98,
96, 94, 92, 90, 88, 86, 84, 82,
81, 79, 77, 75, 73, 71, 70, 68,
66, 64, 63, 61, 59, 57, 56, 54,
53, 51, 49, 48, 46, 45, 43, 42,
40, 39, 37, 36, 35, 33, 32, 31,
29, 28, 27, 26, 24, 23, 22, 21,
20, 19, 18, 17, 16, 15, 14, 13,
12, 11, 10, 10, 9, 8, 8, 7,
6, 6, 5, 5, 4, 4, 3, 3,
2, 2, 2, 1, 1, 1, 1, 0,
0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1, 1, 1, 1, 2, 2,
2, 3, 3, 4, 4, 5, 5, 6,
6, 7, 8, 8, 9, 10, 10, 11,
12, 13, 14, 15, 16, 17, 18, 19,
20, 21, 22, 23, 24, 26, 27, 28,
29, 31, 32, 33, 35, 36, 37, 39,
40, 42, 43, 45, 46, 48, 49, 51,
53, 54, 56, 57, 59, 61, 63, 64,
66, 68, 70, 71, 73, 75, 77, 79,
81, 82, 84, 86, 88, 90, 92, 94,
96, 98, 100, 102, 104, 106, 108, 110,
112, 114, 116, 117, 119, 121, 123, 125
};
double gyroXangle, gyroYangle; // Angle calculate using the gyro only
double compAngleX, compAngleY; // Calculated angle using a complementary filter
double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter
double compPitch, gyroYaw, compRoll, magYaw, gxang;
//PID Variabler.
float PIDX, PIDY, PIDZ, errorX, errorY, errorZ, prev_errorX, prev_errorY, prev_errorZ;
float pidX_p, pidY_p, pidZ_p = 0;
float pidX_i, pidY_i, pidZ_i = 0;
float pidX_d, pidY_d, pidZ_d = 0;
double kxp = 45, kyp = 35, kzp = 10; //3.55
double kxi = 0.002, kyi = 0.002, kzi = 0.00; //0.003
double kxd = 2, kyd = 1.5, kzd = 10; //2.05,d=5.32 7.5
float desired_angleX = 0, desired_angleY = 0, desired_angleZ = 0;
//Motorstyrning
int motorXDelayActual, motorYDelayActual, motorZDelayActual = 60000;
int mXPin1 = 2, mXPin2 = 3, mXPin3 = 4;
int mYPin1 = 5, mYPin2 = 6, mYPin3 = 7;
int mZPin1 = 8, mZPin2 = 9, mZPin3 = 10;
int stepX, stepY, stepZ = 1;
int EN1 = 11;
int EN2 = 12;
int currentStepXA;
int currentStepXB;
int currentStepXC;
int currentStepYA;
int currentStepYB;
int currentStepYC;
int currentStepZA;
int currentStepZB;
int currentStepZC;
long lastMotorXDelayTime, lastMotorYDelayTime, lastMotorZDelayTime = 0;
double mCounter = micros();
uint8_t EepromBuffer[48];
float axb, axs, ayb, ays, azb, azs;
float hxb, hxs, hyb, hys, hzb, hzs;
void setup() {
analogWriteResolution(8);
// serial to display data
Serial.begin(115200);
while (!Serial) {}
// start communication with IMU
status = IMU.begin();
delay(100);
accX = IMU.getAccelX_mss(), accY = IMU.getAccelY_mss(), accZ = IMU.getAccelZ_mss();
gx = IMU.getGyroX_rads(), gy = IMU.getGyroY_rads(), gz = IMU.getGyroZ_rads();
mx = IMU.getMagX_uT(), my = IMU.getMagY_uT(), mz = IMU.getMagZ_uT();
double roll = (180 / M_PI) * atan(accX / sqrt(sq(accY) + sq(accZ)));
double pitch = (180 / M_PI) * atan(accY / sqrt(sq(accX) + sq(accZ)));
double yaw = atan2(my, mx) * r2d;
// Set starting angle
kalmanX.setAngle(roll);
kalmanY.setAngle(pitch);
kalmanZ.setAngle(yaw);
gyroXangle = roll;
gyroYangle = pitch;
compAngleX = roll;
compAngleY = pitch;
if (status < 0) {
Serial.println("IMU initialization unsuccessful");
Serial.println("Check IMU wiring or try cycling power");
Serial.print("Status: ");
Serial.println(status);
while (1) {}
}
for (size_t i = 0; i < sizeof(EepromBuffer); i++) {
EepromBuffer = EEPROM.read(i);
}
memcpy(&axb, EepromBuffer + 0, 4);
memcpy(&axs, EepromBuffer + 4, 4);
memcpy(&ayb, EepromBuffer + 8, 4);
memcpy(&ays, EepromBuffer + 12, 4);
memcpy(&azb, EepromBuffer + 16, 4);
memcpy(&azs, EepromBuffer + 20, 4);
memcpy(&hxb, EepromBuffer + 24, 4);
memcpy(&hxs, EepromBuffer + 28, 4);
memcpy(&hyb, EepromBuffer + 32, 4);
memcpy(&hys, EepromBuffer + 36, 4);
memcpy(&hzb, EepromBuffer + 40, 4);
memcpy(&hzs, EepromBuffer + 44, 4);
IMU.setAccelCalX(axb, axs);
IMU.setAccelCalY(ayb, ays);
IMU.setAccelCalZ(azb, azs);
IMU.setMagCalX(hxb, hxs);
IMU.setMagCalY(hyb, hys);
IMU.setMagCalZ(hzb, hzs);
//Set the accelorometer range
IMU.setAccelRange(MPU9250::ACCEL_RANGE_2G);
// et the gyroscope range.
IMU.setGyroRange(MPU9250::GYRO_RANGE_250DPS);
// setting DLPF bandwidth to 184 Hz
IMU.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_184HZ);
// setting SRD to 0 for a 100 Hz update rate
IMU.setSrd(0);
pinMode(mXPin1, OUTPUT);
pinMode(mXPin2, OUTPUT);
pinMode(mXPin3, OUTPUT);
pinMode(EN1, OUTPUT);
pinMode(EN2, OUTPUT);
digitalWrite(EN1, HIGH);
digitalWrite(EN2, HIGH);
timer = micros();
analogWriteFrequency(mXPin1, freq);
analogWriteFrequency(mXPin2, freq);
analogWriteFrequency(mXPin3, freq);
analogWriteFrequency(mYPin1, freq);
analogWriteFrequency(mYPin2, freq);
analogWriteFrequency(mYPin3, freq);
analogWriteFrequency(mZPin1, freq);
analogWriteFrequency(mZPin2, freq);
analogWriteFrequency(mZPin3, freq);
sineArraySize = sizeof(table) / sizeof(int);
int phaseShift = sineArraySize / 3;
currentStepXA = 0;
currentStepXB = currentStepXA + phaseShift;
currentStepXC = currentStepXB + phaseShift;
currentStepYA = 0;
currentStepYB = currentStepYA + phaseShift;
currentStepYC = currentStepYB + phaseShift;
currentStepZA = 0;
currentStepZB = currentStepZA + phaseShift;
currentStepZC = currentStepZB + phaseShift;
sineArraySize--;
}
void loop() {
IMU.readSensor(); // read the sensor
double dT = (double)(micros() - timer) / 1000000;
timer = micros();
double accX = IMU.getAccelX_mss();
double accY = IMU.getAccelY_mss();
double accZ = IMU.getAccelZ_mss();
double gyroX = IMU.getGyroX_rads();
double gyroY = IMU.getGyroY_rads();
double gyroZ = IMU.getGyroZ_rads();
mx = IMU.getMagX_uT();
my = IMU.getMagY_uT();
mz = IMU.getMagZ_uT();
//Angle from accelorometer
double roll = (180 / M_PI) * atan(accX / sqrt(sq(accY) + sq(accZ)));
double pitch = (180 / M_PI) * atan(accY / sqrt(sq(accX) + sq(accZ)));
double yaw = atan2(my, mx) * r2d;
// Angle from gyro
double gyroXrate = gyroXrate + (gyroX * RAD_TO_DEG) * dT;
double gyroYrate = gyroYrate + (gyroY * RAD_TO_DEG) * dT;
double gyroZrate = gyroZrate + (gyroZ * RAD_TO_DEG) * dT;
//Angle from kalman
double kalRoll = kalmanX.getAngle(roll, gyroXrate, dT);
double kalPitch = kalmanY.getAngle(pitch, gyroYrate, dT);
double kalYaw = kalmanZ.getAngle(yaw, gyroZrate, dT);
//Angle from comp.
compRoll = (double)0.96 * (compRoll + gyroY * dT) + 0.04 * roll;
compPitch = (double)0.96 * (compPitch + gyroX * dT) + 0.04 * pitch;
gyroYaw = (double)(gyroYaw + gyroZ * dT);
runPIDX(kalRoll, desired_angleX, dT);
runPIDY(kalPitch, desired_angleY, dT);
runPIDZ(gyroYaw, desired_angleZ, dT);
//Run Motors
if ((micros() - lastMotorXDelayTime) > motorXDelayActual) {
runMotorX();
lastMotorXDelayTime = micros();
}
if ((micros() - lastMotorYDelayTime) > motorYDelayActual) {
runMotorY();
lastMotorYDelayTime = micros();
}
if ((micros() - lastMotorZDelayTime) > motorZDelayActual) {
runMotorZ();
lastMotorZDelayTime = micros();
}
}
void calAcc(void) {
Serial.println("Starting Accelerometer Calibration"), IMU.calibrateAccel();
Serial.println("Switch"), delay(5000), IMU.calibrateAccel();
Serial.println("Switch"), delay(5000), IMU.calibrateAccel();
Serial.println("Switch"), delay(5000), IMU.calibrateAccel();
Serial.println("Switch"), delay(5000), IMU.calibrateAccel();
Serial.println("Switch"), delay(5000), IMU.calibrateAccel();
Serial.println("Done");
}
//Run motors
void runMotorX(void) {
currentStepXA = currentStepXA + stepX;
if (currentStepXA > sineArraySize) currentStepXA = 0;
if (currentStepXA < 0) currentStepXA = sineArraySize;
currentStepXB = currentStepXB + stepX;
if (currentStepXB > sineArraySize) currentStepXB = 0;
if (currentStepXB < 0) currentStepXB = sineArraySize;
currentStepXC = currentStepXC + stepX;
if (currentStepXC > sineArraySize) currentStepXC = 0;
if (currentStepXC < 0) currentStepXC = sineArraySize;
analogWrite(mXPin1, table[currentStepXA]);
analogWrite(mXPin2, table[currentStepXB]);
analogWrite(mXPin3, table[currentStepXC]);
}
void runMotorY(void) {
currentStepYA = currentStepYA + stepY;
if (currentStepYA > sineArraySize) currentStepYA = 0;
if (currentStepYA < 0) currentStepYA = sineArraySize;
currentStepYB = currentStepYB + stepY;
if (currentStepYB > sineArraySize) currentStepYB = 0;
if (currentStepYB < 0) currentStepYB = sineArraySize;
currentStepYC = currentStepYC + stepY;
if (currentStepYC > sineArraySize) currentStepYC = 0;
if (currentStepYC < 0) currentStepYC = sineArraySize;
analogWrite(mYPin1, table[currentStepYA]);
analogWrite(mYPin2, table[currentStepYB]);
analogWrite(mYPin3, table[currentStepYC]);
}
void runMotorZ(void) {
currentStepZA = currentStepZA + stepZ;
if (currentStepZA > sineArraySize) currentStepZA = 0;
if (currentStepZA < 0) currentStepZA = sineArraySize;
currentStepZB = currentStepZB + stepZ;
if (currentStepZB > sineArraySize) currentStepZB = 0;
if (currentStepZB < 0) currentStepZB = sineArraySize;
currentStepZC = currentStepZC + stepZ;
if (currentStepZC > sineArraySize) currentStepZC = 0;
if (currentStepZC < 0) currentStepZC = sineArraySize;
analogWrite(mZPin1, table[currentStepZA]);
analogWrite(mZPin2, table[currentStepZB]);
analogWrite(mZPin3, table[currentStepZC]);
}
//Run PIDs
void runPIDX(double kalRoll, double desired_angleX, double dT) {
errorX = kalRoll - desired_angleX; pidX_p = kxp * errorX;
if (errorX < 5 && errorX > -5) {
pidX_i = pidX_i + (kxi * errorX);
} else {
pidX_i = 0;
}
pidX_d = kxd * ((errorX - prev_errorX) / dT);
PIDX = pidX_p + pidX_i + pidX_d;
prev_errorX = errorX;
if (errorX > 0) {
stepX = -1;
} else {
stepX = 1;
}
if (PIDX < 5 && PIDX > -5) {
motorXDelayActual = 100000;
} else {
motorXDelayActual = abs(motorXDelayActual - abs(PIDX));
}
if (motorXDelayActual < 1000) {
motorXDelayActual = 1000;
}
}
void runPIDY(double kalPitch, double desired_angleY, double dT) {
errorY = kalPitch - desired_angleY;
pidY_p = kyp * errorY;
if (-5 < errorY && errorY < 5) {
pidY_i = pidY_i + (kyi * errorY);
} else {
pidY_i = 0;
}
pidY_d = kyd * ((errorY - prev_errorY) / dT);
PIDY = pidY_p + pidY_i + pidY_d;
prev_errorY = errorY;
if (errorY > 0) {
stepY = -1;
} else {
stepY = 1;
}
if (PIDY < 5 && PIDY > -5) {
motorYDelayActual = 100000;
} else {
motorYDelayActual = abs(motorYDelayActual - abs(PIDY));
}
if (motorYDelayActual < 1000) {
motorYDelayActual = 1000;
}
}
void runPIDZ(double kalYaw, double desired_angleZ, double dT) {
errorZ = kalYaw - desired_angleZ;
pidZ_p = kzp * errorZ;
if (-5 < errorZ < 5) {
pidZ_i = pidZ_i + (kzi * errorZ);
} else {
pidZ_i = 0;
}
pidZ_d = kzd * ((errorZ - prev_errorZ) / dT);
PIDZ = pidZ_p + pidZ_i + pidZ_d;
prev_errorZ = errorZ;
if (errorZ > 0) {
stepZ = -1;
} else {
stepZ = 1;
}
if (PIDZ < 5 && PIDZ > -5) {
motorZDelayActual = 100000;
} else {
motorZDelayActual = abs(motorZDelayActual - abs(PIDZ));
}
if (motorZDelayActual < 1000) {
motorZDelayActual = 1000;
}
}