void Angle_Calculate(void)
{
char m[100];
//------加速度--------------------------
//範圍為2g時，換算關係：16384 LSB/g
//角度較小時，x=sinx得到角度（弧度）, deg = rad*180/3.14
//因為x>=sinx,故乘以1.3適當放大
Accel_x = MPU6050_GetData(ACCEL_XOUT_H); //讀取X軸加速度
//sprintf(m, "%f ", Accel_x);
//UART_PutString(UART_BT, m);
Angle_ax = (Accel_x + Accel_x_offset) /16384; //去除零點偏移,計算得到角度（弧度）
Angle_ax = Angle_ax * 1.2 * RAD_TO_DEG; //弧度轉換為度,(180/3.14)
//-------角速度-------------------------
//範圍為2000deg/s時，換算關係：16.4 LSB/(deg/s)
Gyro_y = MPU6050_GetData(GYRO_YOUT_H); //靜止時角速度Y軸輸出為+17左右
//sprintf(m, "%f\r\n", Gyro_y);
//UART_PutString(UART_BT, m);
Gyro_y = -(Gyro_y - Gyro_y_offset)/16.4; //去除零點偏移，計算角速度值,負號為方向處理
//Angle_gy = Angle_gy + Gyro_y*0.01; //角速度積分得到傾斜角度.
//-------卡爾曼濾波融合-----------------------
Kalman_Filter(Angle_ax, Gyro_y); //卡爾曼濾波計算傾角
/*//-------互補濾波-----------------------
//補償原理是取當前傾角和加速度獲得傾角差值進行放大，然後與
//陀螺儀角速度疊加後再積分，從而使傾角最跟蹤為加速度獲得的角度
//0.5為放大倍數，可調節補償度；0.01為系統週期10ms
Angle = Angle + (((Angle_ax-Angle)*0.5 + Gyro_y)*0.01);*/
}
/**************************************************************************
函数功能：获取角度
入口参数：获取角度的算法 1：无 2：卡尔曼 3：互补滤波
返回 值：无
**************************************************************************/
void Get_Angle(u8 way)
{
float Accel_Y,Accel_X,Accel_Z,Gyro_Y,Gyro_Z;
if(way==1) //DMP没有涉及到严格的时序问题，在主函数读取
{
}
else
{
Gyro_Y=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_L); //读取Y轴陀螺仪
Gyro_Z=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_L); //读取Z轴陀螺仪
Accel_X=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_XOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_XOUT_L); //读取X轴加速度记
Accel_Z=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_L); //读取Z轴加速度记
if(Gyro_Y>32768) Gyro_Y-=65536; //数据类型转换 也可通过short强制类型转换
if(Gyro_Z>32768) Gyro_Z-=65536; //数据类型转换
if(Accel_X>32768) Accel_X-=65536; //数据类型转换
if(Accel_Z>32768) Accel_Z-=65536; //数据类型转换
Gyro_Balance=-Gyro_Y; //更新平衡角速度
Accel_Y=atan2(Accel_X,Accel_Z)*180/PI; //计算与地面的夹角
Gyro_Y=Gyro_Y/16.4; //陀螺仪量程转换
if(Way_Angle==2) Kalman_Filter(Accel_Y,-Gyro_Y);//卡尔曼滤波
else if(Way_Angle==3) Yijielvbo(Accel_Y,-Gyro_Y); //互补滤波
Angle_Balance=angle; //更新平衡倾角
Gyro_Turn=Gyro_Z; //更新转向角速度
}
}
void Angle_Calculate(void)
{
//------加速度--------------------------
//範圍為2g時，換算關係：16384 LSB/g
//角度較小時，x=sinx得到角度（弧度）, deg = rad*180/3.14
//因為x>=sinx,故乘以1.3適當放大
Accel_x = MPU6050_GetData(ACCEL_XOUT_H); //讀取X軸加速度
Angle_ax = (Accel_x - 1100) /16384; //去除零點偏移,計算得到角度（弧度）
Angle_ax = Angle_ax * 1.2 * 180/3.14; //弧度轉換為度,
//-------角速度-------------------------
//範圍為2000deg/s時，換算關係：16.4 LSB/(deg/s)
Gyro_y = MPU6050_GetData(GYRO_YOUT_H); //靜止時角速度Y軸輸出為-30左右
Gyro_y = -(Gyro_y + 30)/16.4; //去除零點偏移，計算角速度值,負號為方向處理
//Angle_gy = Angle_gy + Gyro_y*0.01; //角速度積分得到傾斜角度.
//-------卡爾曼濾波融合-----------------------
Kalman_Filter(Angle_ax, Gyro_y); //卡爾曼濾波計算傾角
/*//-------互補濾波-----------------------
//補償原理是取當前傾角和加速度獲得傾角差值進行放大，然後與
//陀螺儀角速度疊加後再積分，從而使傾角最跟蹤為加速度獲得的角度
//0.5為放大倍數，可調節補償度；0.01為系統週期10ms
Angle = Angle + (((Angle_ax-Angle)*0.5 + Gyro_y)*0.01);*/
}
void Angle_Calculate(void)
{
/****************************加速度****************************************/
Angle_ax = (Accel_x - 300 ) /16384; //去除零点偏移,计算得到角度（弧度）
Angle_ax = Angle_ax*1.2*180/3.14; //弧度转换为度,
/****************************角速度****************************************/
Gyro_y = -(Gyro_y - 8)/16.4; //去除零点偏移，计算角速度值
Angle_gy = Angle_gy + Gyro_y*0.009; //角速度积分得到倾斜角度.
/***************************卡尔曼滤波+角度融合*************************************/
Kalman_Filter(Angle_ax,Gyro_y); //+卡尔曼滤波计算倾角
/*******************************互补滤波******************************************/
/*补偿原理是取当前倾角和加速度获
得倾角差值进行放大，然后与陀螺
仪角速度叠加后再积分，从而使倾
角最跟踪为加速度获得的角度0.5
为放大倍数，可调节补偿度;
0.01为系统周期10ms
*/
Angle = Angle + (((Angle_ax-Angle)*0.3 + Gyro_y)*0.009);
}
void CarVoltageGet(void)
{
MovingAverageFilter();
g_GravityAngle =(INT16S)(Asin_to_Angle[(abs_value(Acc_X)*100)/63]*GRAVITY_ANGLE_RATIO);
g_GyroscopeAngleSpeed = g_GyroX*GYROSCOPE_ANGLE_RATIO;
g_CarAngle = g_GyroscopeAngleIntegral;
fDeltaValue = (g_GravityAngle - g_CarAngle)/GRAVITY_ADJUST_TIME_CONSTANT;
//记住INT不要和float乱加会出bug!!!!!!
//积分一定要用float型，否则小于1的数都会被忽视，效果太差
g_GyroscopeAngleIntegral += (g_GyroscopeAngleSpeed +fDeltaValue)/GYROSCOPE_ANGLE_SIGMA_FREQUENCY;
Kalman_Filter(g_CarAngle,g_GyroscopeAngleSpeed);
g_nCarAngle = ((INT16S)(Angle*10) ); //调整车体角度
g_nCarGyroVal = (INT16S)( Angle_dot );
}
int main(void)
{
//original data
int Data[9]={0};
int RealinputACC = 0;
int RealinputGyr = 0;
//variables of flags
int switchNum = 0;
int paraDeter = 0;
//temporary variables for test
int tmp=0;
int getValue=0;
//variables for PID control
int output;
int angleHis1=0,angleHis2=0;
int inputK=0,inputI=0,inputD=0;
int Output=0;
int OutputHis1=2000;
int OutputHis2=2000;
int KpIni=KPINI,KiIni=KIINI,KdIni=KDINI;
float Kp = 1;
float Ki = 0;
float Kd = 0.5;
int AngleCali = 0;
int AngleActual=0;
// Parameters for speed control
int speed = 0;
// PID variables for speed controller
int speedHis1 = 0, speedHis2 = 0;
int speedI = 0, speedD = 0;
int outputSpeed =0;
systemInitial();
// send back comfirm information to prove that wireless has been connected
char haha[5]="Hello";
wireless_string(haha,5);
wireless_char('\n');
clear(DDRB,0);
clear(PORTB,0);
// using wireless to change parameters of PID controller, and if DIP switch 1 is ON, this can be skipped
if (!check(PINB,0))
{
while(1)
{
//m_green(ON);
int success;
success = m_rf_read(buffer, packLength);
if ((success==1)&&(buffer[0]>=45))
{
m_green(ON);
ConvertFinishFlag=1;
}
else{
m_green(OFF);
}
if (ConvertFinishFlag==1)
{
if (switchNum==0)
{
// send back current value of parameter and determine which one should be changed
wireless_string(buffer,packLength);
wireless_char(':');
if ((buffer[0]=='k')&&(buffer[1]=='p'))
{
paraDeter = 1;
wireless_int(KpIni);
}
if ((buffer[0]=='k')&&(buffer[1]=='i'))
{
paraDeter = 2;
wireless_int(KiIni);
}
if ((buffer[0]=='k')&&(buffer[1]=='d'))
{
paraDeter = 3;
wireless_int(KdIni);
}
m_wait(50);
wireless_char('\n');
switchNum++;
}else{
// change the parameter, and if the input is not a value, it will send back "SayAgain" and wait untill the input is a value
getValue = atoi(buffer);
if ((getValue==0)&&(buffer[0]!='0'))
{
char Sorry1[8] = "SayAgain";
wireless_string(Sorry1,8);
m_wait(50);
wireless_char('\n');
m_wait(50);
switchNum=1;
}else{
wireless_string(buffer,packLength);
m_wait(50);
wireless_char('\n');
m_wait(50);
switchNum=0;
switch (paraDeter)
{
case 1:KpIni=getValue;break;
case 2:KiIni=getValue;break;
case 3:KdIni=getValue;break;
}
}
}
// when the input is "start", the robot will start
if ((buffer[0]=='s')&&(buffer[1]=='t')&&(buffer[2]=='a')&&(buffer[3]=='r')&&(buffer[4]=='t'))
{
break;
}
ConvertFinishFlag=0;
memset(buffer,0,packLength);
m_wait(100);
}
}
}
m_green(OFF);
// Initializing IMU
while(!m_imu_init(1,0));
int AngleHis=0;
// calibrate the balance angle value, the code here is trying to find out offset of the angle
for (int numpoint=0;numpoint<499;numpoint++)
{
if(m_imu_raw(Data))
{
RealinputACC = ACCPART*(Data[1]-ACOFFSET);
RealinputGyr = GYRPART*(GYOFFSET-Data[3]);
Kalman_Filter(RealinputACC,RealinputGyr);
}
AngleHis+=angle;
}
AngleCali = AngleHis/500;
// Enable timer interrupt and global interrupt
set(TIMSK3 , OCIE3A);
// Initializing the pin change interrupt which will capture the phase of the signal input of the encoder
set(PCMSK0 , PCINT4);
set(PCICR , PCIE0);
clear(DDRB,4);
clear(DDRB,5);
sei();
// Initializing all the parameters will be used in the PID control of the speed ( or we could say 'displacement')
Kp = (float)KpIni/1000;
Ki = (float)KiIni/1000;
Kd = (float)KdIni/1000;
float Kps, Kds, Kis;
int OutputSpeedActual = 0;
Kps = 4;
Kds = 5;
Kis = 0.5;
while(1)
{
if (Timer3Flag==1) //Here is the control loop, all the data sample process and output should be here
{
cli(); //Try to reduce the possibility of changing the data read from IIC, cause multiple device will use the same line and there also different interrupt in the program
// read data from IMU
if(m_imu_raw(Data))
{
m_red(TOGGLE);
}
// make the input value get rid of the offset
RealinputACC = ACCPART*(Data[1]-ACOFFSET); //The acceleration input without offset
RealinputGyr = GYRPART*(Data[3]-GYOFFSET); //The anglar velocity input without offset
Kalman_Filter(RealinputACC,RealinputGyr); //Using the Kalman Filter to get the reliable output of the angle
// read the changed parameter, because the wireless cannot send float, so change it to float here
Kp = (float)KpIni/1000;
Ki = (float)KiIni/1000;
Kd = (float)KdIni/1000;
// calibrate the angle value by using the balance offset of the angle
AngleActual = angle-AngleCali;
inputK = AngleActual-angleHis1;
inputI +=AngleActual;
// PID controller, which you will find that Ki always be zero
Output = Kp*AngleActual + Ki*inputI + Kd*inputK;
// dead region, which means the value in this region won't give the wheels a speed, so remove it from output to make the output speed linear with the input angle
if (AngleActual>0)
{
OutputHis1 =1823-Output;
}
if (AngleActual<0)
{
OutputHis1 =2193-Output;
}
if (AngleActual==0)
{
if (angleHis1>0)
{
OutputHis1 =1823;
}
if (angleHis1<0)
{
OutputHis1 =2193;
}
if (angleHis1=0)
{
OutputHis1 =OutputHis1;
}
}
if (AngleActual>0)
{
OutputHis2 =1857-Output;
}
if (AngleActual<0)
{
OutputHis2 =2148-Output;
}
if (AngleActual==0)
{
if (angleHis1>0)
{
OutputHis2 =1857;
}
if (angleHis1<0)
{
OutputHis2 =2148;
}
if (angleHis1=0)
{
OutputHis2 =OutputHis2;
}
}
// Try to check whether the output value in the limitation
if (OutputHis1>4000) //Detect the limitation of the output value
{
OutputHis1=4000;
}else{
if (OutputHis1<3)
{
OutputHis1=3;
}
}
if (OutputHis2>4000) //Detect the limitation of the output value
{
OutputHis2=4000;
}else{
if (OutputHis2<3)
{
OutputHis2=3;
}
}
// set the output duty cycle
OCR1B = OutputHis1; // for B6
OCR1C = OutputHis2; // for B7
angleHis2 = angleHis1; //record value of the angle which will be used in PID controller(differential)
angleHis1 = AngleActual;//record value of the angle which will be used in PID controller(differential)
Timer3Flag=0;
}
if (Timer3Flag2==INTERRUPT1S) //this condition used to send the wireless data or usb data, cause the frequency here is below 10Hz
{
cli(); // same concern with above
speed += numPulse; //red the the value of holes (net value, which means one direction is positive and the other is negative, here is just a summation)
speedI += speed;
speedD = speed - speedHis1;
// PID controller of the speed(displacement actually)
outputSpeed = Kps*speed + Kis*speedI + Kds*speedD;
//if (speed>0)
//{
//OutputSpeedActual =1800-outputSpeed;
//}
//if (speed<0)
//{
//OutputSpeedActual =2200-outputSpeed;
//}
//if (speed==0)
//{
//if (speed>0)
//{
//OutputSpeedActual =1800;
//}
//if (speed<0)
//{
//OutputSpeedActual =2200;
//}
//if (speed=0)
//{
//OutputSpeedActual =OutputSpeedActual;
//}
//}
// For stability concern, I decide to limit this value, which means the maximum change of duty cycle is 500/4000 = 12.5%
if (outputSpeed>500)
{
outputSpeed=500;
}
if (outputSpeed<-500)
{
outputSpeed=-500;
}
OCR1B = OutputHis1+outputSpeed;
OCR1C = OutputHis2+outputSpeed;
speedHis2 = speedHis1;
speedHis1 = speed;
// all the wireless receiving and sending codes should be wrote here
wireless_int(RealinputACC); // send back value from IMU of acceleration of Y
wireless_char('\t');
wireless_int(RealinputGyr); // send back value from IMU of angular speed around X
wireless_char('\t');
wireless_int(AngleActual); // send back the current angle value
wireless_char('\n');
m_green(TOGGLE);
Timer3Flag2=0;
// clear the counter
numPulse = 0;
sei();
}
//if (Timer3Flag3==INTERRUPT10MS)
//{
//}
}
}
/*角度计算*/
void Angle_Calcu(void)
{
//范围为2g时，换算关系：16384 LSB/g
//deg = rad*180/3.14
/*利用z轴上的重力加速度计算出，芯片与水平面的夹角*/
Accel_x=ACCE_Data_X(ACCEL_XOUT_H);//x轴
// Accel_y=ACCE_Data_Y(ACCEL_YOUT_H);//y轴
Accel_z=ACCE_Data_Z(ACCEL_ZOUT_H);//z轴
Gyro_y = GYRO_Data_Y(GYRO_YOUT_H);//GetData合成的原始数据
if(Accel_x<32764)
{
x=Accel_x/16384;
}
else
{
x=1-(Accel_x-49152)/16384;
}
// if(Accel_y<32764)
// {
// y=Accel_y/16384;
// }
// else
// {
// y=1-(Accel_y-49152)/16384;
// }
if(Accel_z<32764)
{
z=Accel_z/16384;
}
else
{
z=(Accel_z-49152)/16384;
}
Angle_az=(atan(x/z))*180/3.14;
/*角度的正负号*/
if(Accel_x<32764)
{
Angle_az=+Angle_az;
}
if(Accel_x>32764)
{
Angle_az=-Angle_az;
}
/*角速度*/
if(Gyro_y<32768)//向前运动
{
Gyro_y=-(Gyro_y/16.4);//范围为1000deg/s时，换算关系：16.4 LSB/(deg/s)
}
if(Gyro_y>32768)//向后运动
{
Gyro_y=+(65535-Gyro_y)/16.4;
}
//Angle_gy = Angle_gy + Gyro_y*0.025; //角速度积分得到倾斜角度.越大积分出来的角度越大
//-------卡尔曼滤波融合-----------------------
Kalman_Filter(Angle_az,Gyro_y); //卡尔曼滤波计算倾角
}
int main(void) {
//original data
int Data[9]={0};
int RealinputACC = 0;
int RealinputGyr = 0;
//variables of flags
int switchNum = 0;
int paraDeter = 0;
//temporary variables for test
int tmp=0;
int getValue=0;
//variables for PID control
int output;
int angleHis1=0,angleHis2=0;
int inputK=0,inputI=0,inputD=0;
int Output=0;
int OutputHis1=2000;
int OutputHis2=2000;
int KpIni=KPINI,KiIni=KIINI,KdIni=KDINI;
float Kp = 1;
float Ki = 0;
float Kd = 0.5;
int AngleCali = 0;
int AngleActual=0;
systemInitial();
// send back comfirm information to prove that wireless has been connected
wireless_string(haha,5);
wireless_char('\n');
clear(DDRB,0);
clear(PORTB,0);
// using wireless to change parameters of PID controller, and if DIP switch 1 is ON, this can be skipped
if (!check(PINB,0)) {
transmitEdsCode();
}
m_green(OFF);
// Initializing IMU
while(!m_imu_init(1,0));
int AngleHis=0;
// calibrate the balance angle value, the code here is trying to find out offset of the angle
for (int numpoint=0;numpoint<499;numpoint++)
{
if(m_imu_raw(Data))
{
RealinputACC = ACCPART*(Data[1]-ACOFFSET);
RealinputGyr = GYRPART*(GYOFFSET-Data[3]);
Kalman_Filter(RealinputACC,RealinputGyr);
}
AngleHis+=angle;
}
AngleCali = AngleHis/500;
// Enable timer interrupt and global interrupt
set(TIMSK3 , OCIE3A);
set(PCICR,0);
set(PCMSK0,5);
// set(PCMSK0,4);
sei();
while(1)
{
if (Timer3Flag==1){ //Here is the control loop, all the data sample process and output should be here
cli();
// read data from IMU
if(m_imu_raw(Data)) {
m_red(TOGGLE);
}
// make the input value get rid of the offset
RealinputACC = ACCPART*(Data[1]-ACOFFSET); //The acceleration input without offset
RealinputGyr = GYRPART*(Data[3]-GYOFFSET); //The anglar velocity input without offset
Kalman_Filter(RealinputACC,RealinputGyr); //Using the Kalman Filter to get the reliable output of the angle
// read the changed parameter, because the wireless cannot send float, so change it to float here
Kp = (float)KpIni/1000;
Ki = (float)KiIni/1000;
Kd = (float)KdIni/1000;
// calibrate the angle value by using the balance offset of the angle
AngleActual = angle-AngleCali;
inputK = AngleActual-angleHis1;
inputI +=AngleActual;
// PID controller, which you will find that Ki always be zero
Output = Kp*AngleActual + Ki*inputI + Kd*inputK;
// dead region, which means the value in this region won't give the wheels a speed, so remove it from output to make the output speed linear with the input angle
if (AngleActual>0) {
OutputHis1 =1823-Output;
}
if (AngleActual<0) {
OutputHis1 =2193-Output;
}
if (AngleActual==0) {
if (angleHis1>0) {
OutputHis1 =1823;
}
if (angleHis1<0) {
OutputHis1 =2193;
}
if (angleHis1=0){
OutputHis1 =OutputHis1;
}
}
if (AngleActual>0)
OutputHis2 =1857-Output;
if (AngleActual<0)
OutputHis2 =2148-Output;
if (AngleActual==0) {
if (angleHis1>0)
OutputHis2 =1857;
if (angleHis1<0)
OutputHis2 =2148;
if (angleHis1=0)
OutputHis2 =OutputHis2;
}
// Try to check whether the output value in the limitation
if (OutputHis1>4000) //Detect the limitation of the output value
{
OutputHis1=4000;
}else{
if (OutputHis1<3)
{
OutputHis1=3;
}
}
if (OutputHis2>4000){ //Detect the limitation of the output value
OutputHis2=4000;
}else{
if (OutputHis2<3) {
OutputHis2=3;
}
}
// set the output duty cycle
OCR1B = OutputHis1; // for B6
OCR1C = OutputHis2; // for B7
angleHis2 = angleHis1; //record value of the angle which will be used in D
angleHis1 = AngleActual;//record value of the angle which will be used in D
Timer3Flag=0;
sei();
}
//*****************************************************************
// Because the holes on our encoder are few, so in the 5 ms period it
// probably won't get any data. So my advice is to write the code for
// encoder here, which period is 100ms, then we may get some points,
// or if we like we can use flag3 and set it to 1000ms to get more
// points, 1s is enough short to get precise position
//*****************************************************************
//condition used to send the wireless data or usb data,
// because the frequency here is below 10Hz
if (Timer3Flag2==INTERRUPT1S) {
cli();
tmp = angle;
// send back value from IMU of acceleration of Y
wireless_int(RealinputACC);
wireless_char('\t');
// send back value from IMU of anglar speed around X
wireless_int(RealinputGyr);
wireless_char('\t');
wireless_int(AngleActual); // send back the current angle value
wireless_char('\n');
m_green(TOGGLE);
Timer3Flag2=0;
sei();
}
}
}
