Hello Again,
I'm back at work on my balancing robot. I currently have working code that reads the sensors, filters the data, and then prints the correct angle of the robot. Now I am trying to implement a simple PID controller based on the one found here: Prevas Parekring. I have most of it about ready to go, however I can't seem to figure out where I should be pulling the "integrated_error" from, or the algorithm required to retrieve the "last_error". Any thoughts on this matter would be greatly appreciated!
#include <stdint.h>
#define GYROSCOPE_SENSITIVITY 14.375
#define M_PI 3.14159265359
#define dt 0.01 // 10 ms sample rate!
float pitch;
float pitchAcc;
#define Kp 0.5
#define Ki 0
#define Kd 0
int16_t accel_x;
int16_t accel_y;
int16_t accel_z;
int16_t gyro_x;
int16_t gyro_y;
int16_t gyro_z;
int STD_LOOP_TIME = 9; //Length of main loop in milliseconds
int lastLoopTime = STD_LOOP_TIME;
int lastLoopUsefulTime = STD_LOOP_TIME;
unsigned long loopStartTime = 0;
void setup()
{
// Init serial output
Serial.begin(57600);
// Init sensors
delay(50); // Give sensors enough time to start
I2C_Init();
Accel_Init();
Gyro_Init();
}
void loop()
{
Read_Accel();
Serial.print("#A:");
Serial.print(accel_x); Serial.print(",");
Serial.print(accel_y); Serial.print(",");
Serial.print(accel_z); Serial.println();
Read_Gyro();
Serial.print("#G:");
Serial.print(gyro_x); Serial.print(",");
Serial.print(gyro_y); Serial.print(",");
Serial.print(gyro_z); Serial.println();
ComplementaryFilter();
Serial.print("Angle: ");
Serial.print(pitch); Serial.println();
// *********************** loop timing control ***************************
lastLoopUsefulTime = millis()-loopStartTime;
if(lastLoopUsefulTime<STD_LOOP_TIME) delay(STD_LOOP_TIME-lastLoopUsefulTime);
lastLoopTime = millis() - loopStartTime;
loopStartTime = millis();
}
// ****************** I2C code to read the sensors ***********************
#include <Wire.h>
// Sensor I2C addresses
#define ACCEL_ADDRESS ((int) 0x53) // 0x53 = 0xA6 / 2
#define GYRO_ADDRESS ((int) 0x68) // 0x68 = 0xD0 / 2
void I2C_Init()
{
Wire.begin();
}
void Accel_Init()
{
Wire.beginTransmission(ACCEL_ADDRESS);
Wire.write(0x2D); // Power register
Wire.write(0x08); // Measurement mode
Wire.endTransmission();
delay(5);
Wire.beginTransmission(ACCEL_ADDRESS);
Wire.write(0x31); // Data format register
Wire.write(0x08); // Set to full resolution
Wire.endTransmission();
delay(5);
// Adjust the output data rate to 100Hz (50Hz bandwidth)
Wire.beginTransmission(ACCEL_ADDRESS);
Wire.write(0x2C); // Rate
Wire.write(0x0A); // Set to 100Hz, normal operation
Wire.endTransmission();
delay(5);
}
// Reads x, y and z accelerometer registers
void Read_Accel()
{
int i = 0;
byte buff[6];
Wire.beginTransmission(ACCEL_ADDRESS);
Wire.write(0x32); // Send address to read from
Wire.endTransmission();
Wire.beginTransmission(ACCEL_ADDRESS);
Wire.requestFrom(ACCEL_ADDRESS, 6); // Request 6 bytes
while(Wire.available()) // ((Wire.available())&&(i<6))
{
buff[i] = Wire.read(); // Read one byte
i++;
}
Wire.endTransmission();
{
accel_x = ((((int) buff[3]) << 8) | buff[2])-5; // X axis (internal sensor y axis)
accel_y = ((((int) buff[1]) << 8) | buff[0])-3; // Y axis (internal sensor x axis)
accel_z = ((((int) buff[5]) << 8) | buff[4])-1; // Z axis (internal sensor z axis)
}
}
void Gyro_Init()
{
// Power up reset defaults
Wire.beginTransmission(GYRO_ADDRESS);
Wire.write(0x3E);
Wire.write(0x80);
Wire.endTransmission();
delay(5);
// Select full-scale range of the gyro sensors
// Set LP filter bandwidth to 42Hz
Wire.beginTransmission(GYRO_ADDRESS);
Wire.write(0x16);
Wire.write(0x1B); // DLPF_CFG = 3, FS_SEL = 3
Wire.endTransmission();
delay(5);
// Set sample rato to 100Hz
Wire.beginTransmission(GYRO_ADDRESS);
Wire.write(0x15);
Wire.write(0x09); // SMPLRT_DIV = 9 (100Hz)
Wire.endTransmission();
delay(5);
// Set clock to PLL with z gyro reference
Wire.beginTransmission(GYRO_ADDRESS);
Wire.write(0x3E);
Wire.write(0x00);
Wire.endTransmission();
delay(5);
}
// Reads x, y and z gyroscope registers
void Read_Gyro()
{
int i = 0;
byte buff[6];
Wire.beginTransmission(GYRO_ADDRESS);
Wire.write(0x1D); // Sends address to read from
Wire.endTransmission();
Wire.beginTransmission(GYRO_ADDRESS);
Wire.requestFrom(GYRO_ADDRESS, 6); // Request 6 bytes
while(Wire.available()) // ((Wire.available())&&(i<6))
{
buff[i] = Wire.read(); // Read one byte
i++;
}
Wire.endTransmission();
{
gyro_x = ((((int) buff[2]) << 8) | buff[3])-11; // X axis (internal sensor -y axis)
gyro_y = ((((int) buff[0]) << 8) | buff[1])+34; // Y axis (internal sensor -x axis)
gyro_z = ((((int) buff[4]) << 8) | buff[5])+12; // Z axis (internal sensor -z axis)
}
}
// ********************** Complementary Filter ***************************
void ComplementaryFilter()
{
// Integrate the gyroscope data -> int(angularSpeed) = angle
pitch += ((float)gyro_y / GYROSCOPE_SENSITIVITY) * dt; // Angle around the Y-axis
// Compensate for drift with accelerometer data if wrong
// Sensitivity = -2 to 2 G at 16Bit -> 2G = 513 && 0.5G = 128
int16_t forceMagnitudeApprox = abs(accel_x) + abs(accel_y) + abs(accel_z);
if (forceMagnitudeApprox > 128 && forceMagnitudeApprox < 513)
{
// Turning around the X axis results in a vector on the Y-axis
pitchAcc = atan2f((float)accel_x, (float)accel_z) * 180 / M_PI;
pitch = pitch * 0.98 + pitchAcc * 0.02;
}
}
// ************************* PID Controller ******************************
void updatePid() {
float K = 1;
float pTerm = 15;
float iTerm = 0;
float dTerm = 0;
int16_t PID_Return;
int error = 0 - pitch;
pTerm = Kp * error;
integrated_error += error;
iTerm = Ki * constrain(integrated_error, -100, 100);
dTerm = Kd * (error - last_error);
last_error = error;
PID_Return = constrain(K*(pTerm + iTerm + dTerm), -400, 400);
}
Andrew