-2.png?raw=true)
#ifndef __PID__
#define __PID__
const bool DIRECT = true;
const bool REVERSE = false;
const bool ON = true;
const bool OFF = false;
class PID
{
public:
PID(double*,double*,double*,double,double,double,unsigned long,bool,double,double,double);
void Computing();
void SetTunings(double,double,double);
void SetSampleTime(unsigned long);
void SetOutputLimits(double,double);
void SetOnOff(bool);
void SetDirection(bool);
private:
double Kp,Ki,Kd;
double *input,*output;
double *setpoint;
double integral;
double last_input;
double min,max;
unsigned long last_time;
unsigned long sample_time;
bool on_off;
bool direction;
};
#endif
#if ARDUINO >= 100
#include <Arduino.h>
#else
#include <WProgram.h>
#endif
#include "PID.h"
PID::PID(double *_input,double *_output,double *_setpoint,
double _Kp,double _Ki,double _Kd,
unsigned long _sample_time = 100000,
bool _on_off = OFF,double _direction = DIRECT,
double _min = 0,double _max = 255)
{
input = _input;
output = _output;
setpoint = _setpoint;
sample_time = _sample_time;
on_off = _on_off;
SetOutputLimits(_min,_max);
SetDirection(_direction);
SetTunings(_Kp,_Ki,_Kd);
last_time = (micros() - sample_time);
}
void PID::Computing()
{
if(on_off == OFF){
return ;
}
unsigned long now = micros();
if((now - last_time) >= sample_time){
double error = (*setpoint - *input);
integral += (Ki * error);
if(integral > max){
integral = max;
}
else if(integral < min){
integral = min;
}
double diff_input = (*input - last_input);
*output = (Kp * error) + integral - (Kd * diff_input);
if(*output > max){
*output = max;
}
else if(*output < min){
*output = min;
}
last_input = *input;
last_time = now;
}
}
void PID::SetTunings(double _Kp,double _Ki,double _Kd)
{
if((_Kp < 0) || (_Ki < 0) || (_Kd < 0)){
return ;
}
double SampleTimeInSec = ((double)sample_time) / 1000000.0f;
Kp = _Kp;
Ki = _Ki * SampleTimeInSec;
Kd = _Kd / SampleTimeInSec;
if(direction != REVERSE){
Kp = (0 - Kp);
Ki = (0 - Ki);
Kd = (0 - Kd);
}
}
void PID::SetSampleTime(unsigned long _sample_time)
{
if(_sample_time > 0){
double ratio = ((double)_sample_time / (double)sample_time);
Ki *= ratio;
Kd /= ratio;
sample_time = _sample_time;
}
}
void PID::SetOutputLimits(double _min,double _max)
{
if(_min > _max){
return ;
}
min = _min;
max = _max;
if(on_off == ON){
if(*output > max){
*output = max;
}
else if(*output < min){
*output = min;
}
if(integral > max){
integral = max;
}
else if(integral < min){
integral = min;
}
}
}
void PID::SetOnOff(bool _on_off)
{
bool new_state = (_on_off == ON);
if(new_state == (!on_off)){
integral = *output;
last_input = *input;
if(integral > max){
integral = max;
}
else if(integral < min){
integral = min;
}
}
on_off = new_state;
}
void PID::SetDirection(bool _direction)
{
if((on_off == ON) && (_direction != direction)){
Kp = (0 - Kp);
Ki = (0 - Ki);
Kd = (0 - Kd);
}
direction = _direction;
}
#ifndef __KALMAN_FILTER__
#define __KALMAN_FILTER__
class KalmanFilter
{
public:
KalmanFilter(double r_measure,double q_angle,double q_bias,double arg_angle)
{
Q_angle = q_angle;
Q_bias = q_bias;
R_measure = r_measure;
angle = arg_angle;
bias = 0;
P[0][0] = 0;
P[0][1] = 0;
P[1][0] = 0;
P[1][1] = 0;
}
/* Accelerometer Angle , Gyro Rate , dt */
double Update(double acc,double gyro,double dt)
{
rate = gyro - bias;
angle += rate * dt;
P[0][0] += dt * (dt * P[1][1] - P[0][1] - P[1][0] + Q_angle);
P[0][1] -= dt * P[1][1];
P[1][0] -= dt * P[1][1];
P[1][1] += Q_bias * dt;
angle_diff = acc - angle;
S = P[0][0] + R_measure;
K[0] = P[0][0] / S;
K[1] = P[1][0] / S;
angle += K[0] * angle_diff;
bias += K[1] * angle_diff;
P[0][0] -= K[0] * P[0][0];
P[0][1] -= K[0] * P[0][1];
P[1][0] -= K[1] * P[0][0];
P[1][1] -= K[1] * P[0][1];
return angle;
}
private:
double R_measure;
double Q_angle;
double Q_bias;
double angle;
double bias;
double rate;
double angle_diff;
double S;
double P[2][2];
double K[2];
};
#endif
KalmanFilter KFx(0.03f,0.001f,0.003f,0.0f);
KalmanFilter KFy(0.03f,0.001f,0.003f,0.0f);
unsigned long last_KF_computing_time; //us
void loop()
{
double KF_x_angle = KFx.Update(AccAngleX,GyroRateYaw,
(double)(micros() - last_KF_computing_time) / 1000000.0f);
double KF_y_angle = KFx.Update(AccAngleY,GyroRatePitch,
(double)(micros() - last_KF_computing_time) / 1000000.0f);
last_KF_computing_time = micros();
}
#include <Wire.h>
uint8_t buffer[6];
int xID;
double AccAngleX = 0,AccAngleY = 0;
int GyroLastYaw = 0,GyroLastPitch = 0,GyroLastRoll = 0;
double GyroAngleYaw = 0,GyroAnglePitch = 0,GyroAngleRoll = 0;
double HighPass = 0.93f,LowPass = 0.07f;
double AngleX = 0,AngleY = 0;
unsigned long last_loop_time; //ms
unsigned long last_gyro_computing_time; //us
void setup()
{
Serial.begin(115200);
Wire.begin();
Init6DOF();
last_loop_time = millis();
last_gyro_computing_time = micros();
}
void loop()
{
if(millis() > (last_loop_time + 10)){
IICRead(buffer,0x52,6);
if((buffer[5] & 0x03) == 0x00){
ComputingNunchuk(buffer);
}
else if((buffer[5] & 0x03) == 0x02){
ComputingWMP(buffer);
}
ComplementaryFilter();
IICWrite(0x52,0x00);
last_loop_time = millis();
}
}
void Init6DOF()
{
delay(100);
IICWrite(0x53,0xFE,0x05);
Serial.println("Passthrough Mode Ok!");
delay(100);
IICWrite(0x53,0xF0,0x55);
Serial.println("Init WMP Ok!");
delay(100);
IICWrite(0x52,0xFA);
Serial.println("Set Reading Address 0xFA Ok!");
delay(100);
IICRead(buffer,0x52,6);
xID = buffer[0] + buffer[1] + buffer[2] +
buffer[3] + buffer[4] + buffer[5];
Serial.print("Extension Controller xID = 0x");
Serial.println(xID,HEX);
if(xID == 0xCB){
Serial.println("WMP Connected But Not Avtivared!");
}
if(xID == 0xCE){
Serial.println("WMP Connected And Avtivared!");
}
if(xID == 0x00){
Serial.println("WMP Not Connected!");
}
delay(100);
IICWrite(0x52,0x8);
Serial.println("Set Reading Address 0x08 Ok!");
delay(100);
IICWrite(0x52,0x00);
}
double Map(double value,double Input_Min,double Input_Max,double Output_Min,double Output_Max)
{
double rValue = (value - Input_Min) * (Output_Max - Output_Min) / (Input_Max - Input_Min) + Output_Min;
double rMin,rMax;
if(Output_Min < Output_Max){
rMin = Output_Min;
rMax = Output_Max;
}
else{
rMin = Output_Max;
rMax = Output_Min;
}
if(rValue < rMin){
return rMin;
}
if(rValue > rMax){
return rMax;
}
return rValue;
}
void ComputingNunchuk(uint8_t *buf)
{
int accel_x_axis = (buf[2] << 2) + ((buf[5] >> 3) & 2);
int accel_y_axis = (buf[3] << 2) + ((buf[5] >> 4) & 2);
int accel_z_axis = (buf[4] << 2) + ((buf[5] >> 5) & 6);
int mapX = Map(accel_x_axis,300.0f,700.0f,-90.0f,90.0f);
int mapY = Map(accel_y_axis,300.0f,700.0f,-90.0f,90.0f);
int mapZ = Map(accel_z_axis,360.0f,760.0f,-90.0f,90.0f);
AccAngleX = atan2(mapX,mapZ) / 3.14159 * 180.0f;
AccAngleY = atan2(mapY,mapZ) / 3.14159 * 180.0f;
/*
Serial.print(accel_x_axis);
Serial.print(" ");
Serial.print(accel_y_axis);
Serial.print(" ");
Serial.println(accel_z_axis);
*/
}
void ComputingWMP(uint8_t *buf)
{
int yaw = (((buf[5] & 0xFC) << 6) + buf[0]);
int pitch = (((buf[4] & 0xFC) << 6) + buf[1]);
int roll = (((buf[3] & 0xFC) << 6) + buf[2]);
double GyroDiffYaw = (yaw - GyroLastYaw) / 14.375f;
double GyroDiffPitch = (pitch - GyroLastPitch) / 14.375f;
double GyroDiffRoll = (roll - GyroLastRoll) / 14.375f;
GyroAngleYaw = GyroDiffYaw *
(double)(micros() - last_gyro_computing_time) / 1000000.0f;
GyroAnglePitch = GyroDiffPitch *
(double)(micros() - last_gyro_computing_time) / 1000000.0f;
GyroAngleRoll = GyroDiffRoll *
(double)(micros() - last_gyro_computing_time) / 1000000.0f;
last_gyro_computing_time = micros();
GyroLastYaw = yaw;
GyroLastPitch = pitch;
GyroLastRoll = roll;
/*
Serial.print(yaw);
Serial.print(" ");
Serial.print(pitch);
Serial.print(" ");
Serial.println(roll);
*/
/*
Serial.print(GyroAngleYaw);
Serial.print(" ");
Serial.print(GyroAnglePitch);
Serial.print(" ");
Serial.println(GyroAngleRoll);
*/
}
void ComplementaryFilter()
{
AngleX = (HighPass * (AngleX + GyroAngleYaw)) + (LowPass * AccAngleX);
AngleY = (HighPass * (AngleY + GyroAnglePitch)) + (LowPass * AccAngleY);
Serial.print(AngleX);
Serial.print(" ");
Serial.println(AngleY);
}
void IICWrite(uint8_t address,uint8_t register_address)
{
Wire.beginTransmission(address);
Wire.write(register_address);
Wire.endTransmission();
}
void IICWrite(uint8_t address,uint8_t register_address,uint8_t data)
{
Wire.beginTransmission(address);
Wire.write(register_address);
Wire.write(data);
Wire.endTransmission();
}
void IICRead(uint8_t *buf,uint8_t address,uint8_t length)
{
Wire.requestFrom(address,length);
for(int i = 0;Wire.available();++i){
buf[i] = Wire.read();
}
}
#include <math.h>
class KalmanFilter
{
public:
KalmanFilter(double q,double r);
double Update(double);
double GetK(){return k;}
private:
double k; //kalman gain
double p; //estimation error cvariance
double q; //process noise cvariance
double r; //measurement noise covariance
double x; //value
};
KalmanFilter::KalmanFilter(double q,double r):q(q),r(r),x(0.0)
{
p = sqrt(q * q + r * r);
}
double KalmanFilter::Update(double value)
{
p += q;
k = p / (p + r);
x += k * (value - x);
p *= (1 - k);
return x;
}KalmanFilter kalman(2.0,1.5);
void loop()
{
double Angle = Accelerometer();
double k_value = kalman.Update(Angle);
}