|
|
簡單粗暴，直接上代
#include "headfile.h"
#define PIT_FlAG_CLR(PITn) PIT->CHANNEL[PITn].TFLG |= PIT_TFLG_TIF_MASK //清中斷標志（寫1 清空標志位）
#define zhongzhi -17.4 //小車的目標角度
#define PonitSpeed 130
#define Induct_add 200
#define PWMMAX 1500//1800
#define PWMMIN 500//200
void SensorStructurePointerInit(Acc_Type_Def *Acc, Gyro_Type_Def *Gyro, Angle_Type_Def *Angle, Speed_Type_Def *Speed ){ //結(jié)構(gòu)體指針初始化成員
/*Gyro*/
Gyro->X_Final=0;
Gyro->X_Offset=0;
Gyro->X_Value[0]=0;
Gyro->X_Value[1]=0;
Gyro->X_Value[2]=0;
Gyro->Z_Final=0;
Gyro->Z_Offset=0;
Gyro->Z_Value[0]=0;
Gyro->Z_Value[1]=0;
Gyro->Z_Value[2]=0;
//角度目標值 也就是平衡值 沒有轉(zhuǎn)換成角度值 但是是線性關(guān)系。
Angle->PiontValue = zhongzhi;
//速度目標值 沒有轉(zhuǎn)換成真實速度 與速度線性關(guān)系
Speed->PiontSpeed = PonitSpeed;
Speed->PiontSpeedMax=600;
Speed->PiontSpeedMin=-600;
}
void CarBalanceSystemInit(Gyro_Type_Def *Gyro){//系統(tǒng)初始化
adc_init(ADC0_SE0);
adc_init(ADC0_SE1);
adc_init(ADC0_SE2);
adc_init(ADC0_SE3);
adc_init(ADC0_SE6);
adc_init(ADC0_SE7);
OLED_Init(); //OLED初始化
IIC_init(); // 使用數(shù)字陀螺儀要初始化IIC
InitMPU6050(); //MPU6050的初始化函數(shù)
systick_delay_ms(10);
gpio_init(C7,GPO,1); // 使能FTM
//左電機
ftm_pwm_init(ftm2,ftm_ch0,13000,0);
ftm_pwm_init(ftm2,ftm_ch1,13000,0);
//右電機
ftm_pwm_init(ftm2,ftm_ch2,13000,0);
ftm_pwm_init(ftm2,ftm_ch3,13000,0);
systick_delay_ms(10);
//ftm的計數(shù)引腳可查看KEA128_port_cfg.h文件內(nèi)的FTM0_COUNT_PIN與FTM1_COUNT_PIN的宏定義得知
//一個ftm同一時間只能實現(xiàn)一種功能，如果用于測速就不能用于輸出PWM
///////////////////////////////////////////////////////////////////////////////////////////////
ftm_count_init(ftm0); //對引腳輸入的脈沖進行計數(shù) 接編碼器LSB
gpio_init(D3,GPI,0); //用于判斷方向 接編碼器DIR
port_pull(D3); //IO上拉
ftm_count_init(ftm1); //對引腳輸入的脈沖進行計數(shù) 接編碼器LSB
gpio_init(D4,GPI,0); //用于判斷方向 接編碼器DIR
port_pull(D4); //IO上拉
//////////////////////////////////////////////////////////////////////////////////////////////
//定時器中斷
pit_init(pit0,0x9000);//1.539ms
systick_delay_ms(10);
}
//絕對值函數(shù) 必須要取地址放入形參
void AbsoluteValue(float *Value, float *Return){
if( (*Value) <0) (*Return) = -1*(*Value);
else *Value = *Return;
}
/*原始數(shù)據(jù)獲取部分*/
void GetAccelerationValue(Acc_Type_Def *Acc){ //獲取ACC原始數(shù)據(jù)
Get_AccData();
Acc->Z_Value[0] = Acc->Z_Value[1];
Acc->Z_Value[1] = Acc->Z_Value[2];
Acc->Z_Value[2] = ACC_Z;//本句是獲取當(dāng)前加速度 上兩句都是 上次值和上上次值
}
void GetAngleSpeedValue(Gyro_Type_Def *Gyro){ //獲取Gyro原始數(shù)據(jù)
Get_Gyro();
Gyro->Z_Value[0] = Gyro->Z_Value[1];
Gyro->Z_Value[1] = Gyro->Z_Value[2];
Gyro->Z_Value[2] = GYRO_Z ; //本句是獲取當(dāng)前Gyro上兩句都是 上次值和上上次值
Gyro->X_Value[0] = Gyro->X_Value[1]; //前前次
Gyro->X_Value[1] = Gyro->X_Value[2]; //前次
Gyro->X_Value[2] = GYRO_X ; //本次
}
//FTM0-E0 FTM1-E7 左右重新確認
void GetSpeedValue(Speed_Type_Def *Speed){ //獲取Speed原始數(shù)據(jù)
for(uint8 n=0; n<15 ; n++){ //循環(huán)誤差保存
//存儲前14次的值
Speed->ActualLiftPulse[n] = Speed->ActualLiftPulse[n+1]; //0~14位賦值
if( n==14 ){ //當(dāng)前值 15次 最新值
Speed->ActualLiftPulse[n+1] = ftm_count_get(ftm1)*(-1); //本次 15位賦值
ftm_count_clean(ftm1); //清除ftm1計數(shù)值
}//此處一定要修改好 根據(jù)實際的硬件 對應(yīng)好左右輪子 因為編碼器是對著安裝的 那么
//在車子往前走的時候 左右是相反的 所以要把其中一個負值編碼器變?yōu)檎?br />
//兩個電機存在同樣的問題 都是對著安裝的
}
//同上
for(uint8 n=0; n<15 ; n++){ //循環(huán)保存
Speed->ActualRightPulse[n] = Speed->ActualRightPulse[n+1]; //0~14位賦值
if( n==14 ){
Speed->ActualRightPulse[n+1] = ftm_count_get(ftm0); //本次 15位賦值
ftm_count_clean(ftm0); //清除ftm0計數(shù)值
}
}
if(gpio_get(D4)) //檢測 ftm1的轉(zhuǎn)向 H6 E1是針對左右編碼器的 要弄清楚 不要到時候改過以后
//左邊編碼器的方向 去控制右邊編碼器的 正負號 那就完蛋了
Speed->ActualLiftPulse[15] = -Speed->ActualLiftPulse[15];
else
Speed->ActualLiftPulse[15] = Speed->ActualLiftPulse[15];
if(gpio_get(D3)) //檢測 ftm0的轉(zhuǎn)向
Speed->ActualRightPulse[15] = -Speed->ActualRightPulse[15];
else
Speed->ActualRightPulse[15] = Speed->ActualRightPulse[15]; //這個負號取決于你車向前走時 編碼器值是否為正
}
void AngleSoftwareFilter(Acc_Type_Def *Acc, Gyro_Type_Def *Gyro , Angle_Type_Def *Angle)
{ //本函數(shù)用于數(shù)字陀螺儀 通過軟件互補融合濾波解算角度
//加速度 角速度采集
Acc->Z_Final = 0.9*Acc->Z_Value[2] + 0.05*Acc->Z_Value[1] + 0.05*Acc->Z_Value[0]; //權(quán)值低通濾波
Gyro->Z_Final = 0.9*Gyro->Z_Value[2] + 0.05*Gyro->Z_Value[1] + 0.05*Gyro->Z_Value[0];//權(quán)值低通濾波
Gyro->X_Final = 0.92*Gyro->X_Value[2] + 0.05*Gyro->X_Value[1] + 0.03*Gyro->X_Value[0];//權(quán)值低通濾波
//此處參數(shù)需要根據(jù) 清華方案里的方法去調(diào)節(jié)參數(shù) 看波形法
Angle->FinalValue = 0.98*(Angle->FinalValue -0.05*Gyro->Z_Final) + 0.02*(Acc->Z_Final);//融合濾波
}
//void SpeedSoftwareFilter(Speed_Type_Def *Speed){ //速度低通濾波
//
// /*警告無視*////編碼器收到的數(shù)據(jù) 在文件前面有函數(shù)讀取
// Speed->ActualLiftPulse[15]=0.33*Speed->ActualLiftPulse[15] + 0.33*Speed->ActualLiftPulse[14] + 0.33*Speed->ActualLiftPulse[13];
// Speed->ActualRightPulse[15]=0.33*Speed->ActualRightPulse[15] + 0.33*Speed->ActualRightPulse[14] + 0.33*Speed->ActualRightPulse[13];
// // 6 2 2
//
// //實際速度 = R+L
// Speed->ActualSpeed[0] = Speed->ActualSpeed[1];
// Speed->ActualSpeed[1] = Speed->ActualSpeed[2];
// Speed->ActualSpeed[2] =
// ((Speed->ActualRightPulse[0]+Speed->ActualLiftPulse[0])+
// (Speed->ActualRightPulse[1]+Speed->ActualLiftPulse[1])+
// (Speed->ActualRightPulse[2]+Speed->ActualLiftPulse[2])+
// (Speed->ActualRightPulse[3]+Speed->ActualLiftPulse[3])+
// (Speed->ActualRightPulse[4]+Speed->ActualLiftPulse[4])+
// (Speed->ActualRightPulse[5]+Speed->ActualLiftPulse[5])+
// (Speed->ActualRightPulse[6]+Speed->ActualLiftPulse[6])+
// (Speed->ActualRightPulse[7]+Speed->ActualLiftPulse[7])+
// (Speed->ActualRightPulse[8]+Speed->ActualLiftPulse[8])+
// (Speed->ActualRightPulse[9]+Speed->ActualLiftPulse[9])+
// (Speed->ActualRightPulse[10]+Speed->ActualLiftPulse[10])+
// (Speed->ActualRightPulse[11]+Speed->ActualLiftPulse[11])+
// (Speed->ActualRightPulse[12]+Speed->ActualLiftPulse[12])+
// (Speed->ActualRightPulse[13]+Speed->ActualLiftPulse[13])+
// (Speed->ActualRightPulse[14]+Speed->ActualLiftPulse[14])+
// (Speed->ActualRightPulse[15]+Speed->ActualLiftPulse[15]))/32;
//
// //濾波
// Speed->ActualSpeed[2] = 0.85*Speed->ActualSpeed[2]+0.1*Speed->ActualSpeed[1]+0.05*Speed->ActualSpeed[0];
// //7 2 1
//
// //實際差速 = L-R
// Speed->ActualDifferential[0] = Speed->ActualDifferential[1];
// Speed->ActualDifferential[1] = Speed->ActualDifferential[2];
// Speed->ActualDifferential[2] = Speed->ActualLiftPulse[15]-Speed->ActualRightPulse[15];
// Speed->ActualDifferential[2] = 0.8*Speed->ActualDifferential[2]+0.15*Speed->ActualDifferential[1]+0.05*Speed->ActualDifferential[0];
// //實際差速
// Speed->ActualInstantaneous[0] = Speed->ActualInstantaneous[1];
// Speed->ActualInstantaneous[1] = Speed->ActualInstantaneous[2];
// Speed->ActualInstantaneous[2] = (Speed->ActualLiftPulse[15]+Speed->ActualRightPulse[15] + (Speed->ActualLiftPulse[14]+Speed->ActualRightPulse[14])) ;
// //瞬時速度計算
// Speed->ActualAcceleration = Speed->ActualInstantaneous[2] - Speed->ActualInstantaneous[0];
// //加速度計算
//
//}
void MotorControlFlow(PWM_Type_Def *PWM, Ramp_Type_Def *Ramp)
{
//電機直接控制
//如測試線序 在此處 對*PWM賦值 測試完畢再注釋
//PWM->LiftValue = 1200;//左輪正傳 往前走
////PWM->LiftValue = 1000;
////PWM->LiftValue = 800;
//PWM->RightValue =1200;//右輪正傳 往前走
// PWM->RightValue = 1000; //PWM->RightValue = 800;
//左邊右邊都給 1000的時候 應(yīng)該是不轉(zhuǎn)的
if(PWM->LiftValue>(PWMMAX))PWM->LiftValue = (PWMMAX);
if(PWM->LiftValue<(PWMMIN))PWM->LiftValue = (PWMMIN);
if(PWM->RightValue>(PWMMAX))PWM->RightValue = (PWMMAX);
if(PWM->RightValue<(PWMMIN))PWM->RightValue = (PWMMIN);
//此處必須先調(diào)整好 保證 PWM值是 >1000時 是前進
//PWM值< 1000 是后退 而且 兩輪必須一致
//能改硬件 就改驅(qū)動線 和 電機引線 例如
//給同樣都是 1200 左邊電機慢速往前 右邊慢速往后 這時候 調(diào)換右電機線
//如果出現(xiàn)如下情況 給1200 理論上是 正轉(zhuǎn) 20%占空比 但出現(xiàn)轉(zhuǎn)速很快 說明
//驅(qū)動線序不對 可以在這里調(diào)換 通道 或者 直接改線序
if(PWM->LiftValue>1000){ //左輪前進 1000~2000
ftm_pwm_duty(ftm2,ftm_ch1,PWM->LiftValue-1000);
ftm_pwm_duty(ftm2,ftm_ch0,0);
}
else if(PWM->LiftValue<1000){ //左輪是后退 0~1000
ftm_pwm_duty(ftm2,ftm_ch1,0);
ftm_pwm_duty(ftm2,ftm_ch0,1000-PWM->LiftValue);
} //如果發(fā)現(xiàn) 此處給定的是 控制 右輪轉(zhuǎn)動 則調(diào)換下硬件驅(qū)動線線序 注意 是兩根兩根的調(diào)換
if(PWM->RightValue>1000){ //右輪是前進 1000~2000
ftm_pwm_duty(ftm2,ftm_ch3,PWM->RightValue-1000);
ftm_pwm_duty(ftm2,ftm_ch2,0);
}
else if(PWM->RightValue<1000){ //右輪是后退 0~1000
ftm_pwm_duty(ftm2,ftm_ch3,0);
ftm_pwm_duty(ftm2,ftm_ch2,1000-PWM->RightValue);
} //如果發(fā)現(xiàn) 此處給定的是 控制 左邊輪轉(zhuǎn)動 則調(diào)換下硬件驅(qū)動線線序 注意 是兩根兩根的調(diào)換
}
/*被控制量處理部分*/
void BalanceFeedbackControl(PID_Type_Def *PID, Angle_Type_Def *Angle, Gyro_Type_Def *Gyro, Ramp_Type_Def *Ramp){ //直立反饋控制
PID->err = -(Angle->PiontValue-Angle->FinalValue);//硬件
//偏差 = 角度目標-角度反饋值
PID->out = 1000 + PID->Kp * PID->err + PID->Kd * Gyro->Z_Final;
//1000是為了給一個PWM中點 Gyro->Z_Final就是 直立的角速度 也就是D控制 PD
//一般情況下 調(diào)直立時 先給一個P參數(shù) D給0 就能立起來 如果發(fā)現(xiàn)不能立瘋轉(zhuǎn)
//多半是 pid方向相反 把(Angle->PiontValue-Angle->FinalValue) 里面調(diào)換下位置 即可
};
//牛頓迭代法 開方運算
float newtonSqrt(float a){
int i;
float x;
x=a;
for(i=1;i<=10;i++)
x=(x+a/x)/2;
return x;
}
//void DirectionFeedbackControl(PID_Type_Def *PID, Inductor_Type_Def *InductorA, Inductor_Type_Def *InductorB, Gyro_Type_Def *Gyro, Speed_Type_Def *Speed, Ramp_Type_Def *Ramp){ //方向反饋控制
//
// //調(diào)節(jié)offset 在中間位置的時候讓左邊的電感等于右邊的電感
// InductorB->LeftFinal = PID->offset*InductorB->LeftFinal;
// InductorA->LeftFinal = (2-PID->offset)*InductorA->LeftFinal;
// //調(diào)整零偏
//
// //主要是左右兩個電感
// PID->err = 10*((newtonSqrt(InductorB->LeftFinal)-newtonSqrt(InductorA->LeftFinal ))/ (InductorB->LeftFinal+InductorA->LeftFinal));
// //如果和值小于一個數(shù)值可認為沒有偏差 車子繼續(xù)沿著中間路線行駛
// if(InductorA->AndValues<Induct_add) PID->err = 0;
//
// //*************注意PD的極性也就是正負*******************
// PID->out = PID->Kp * PID->err +PID->Kd * Gyro->X_Final;
// //轉(zhuǎn)向PID輸出計算 Gyro->X_Final; 是轉(zhuǎn)向角速度 也是D控制
// //*************注意PD的極性也就是正負******************
//
// Speed->PiontDifferential = (int16)(PID->out); //給定差速環(huán)目標值
// //把轉(zhuǎn)向環(huán)計算出來的輸出值給差速目標環(huán)
//
//
// for(uint8 n=0; n<9 ; n++){ //循環(huán)誤差保存
//
// PID->err_k[n] = PID->err_k[n+1]; //0~8位賦值
//
// if( n==8 )
// PID->err_k[n+1] = PID->err; //本次 9位賦值
//
// }
//
//};
////差速環(huán)就是角速度和編碼器的值一起控制
//void DifferentialFeedbackControl(PID_Type_Def *PID, Speed_Type_Def *Speed, Gyro_Type_Def *Gyro){//差速反饋控制
//
// //PID->offset只是一個比例分配 PID->offset=2
// int16 SpeedDifferentialValue = PID->offset * Speed->ActualDifferential[2] + (1-PID->offset)*(-0.18*Gyro->X_Final);
// //得到差速值是編碼器和轉(zhuǎn)向角速度共同決定差速反饋值
//
// //差速目標- 差速實際值
// PID->err = ( Speed->PiontDifferential - SpeedDifferentialValue);
//
// //誤差積分
// PID->integral += PID->err;
//
// //積分上下限
// if(PID->integral>50000)PID->integral=50000;
//
// if(PID->integral<-50000)PID->integral=-50000;
//
// if(Speed->ActualSpeed[2]< 50)PID->integral = 0;
// //低速積分清零
//
// PID->out = PID->Kp * PID->err + PID->Kd * ( SpeedDifferentialValue + PID->err_k[0]);// + PID->Ki * PID->integral;
// //PID計算
//
//
//
// for(uint8 n=0; n<9 ; n++){ //循環(huán)誤差保存
// PID->err_k[n] = PID->err_k[n+1]; //0~8位賦值
// if( n==8 )
// PID->err_k[n+1] = PID->err; //本次 9位賦值
// }
//
//
//
// PID->err_k[0] = SpeedDifferentialValue;//借用一下PID->err_k[0]咯 存入上次值 差速
//
// //輸出幅度限制
// if(PID->out>2200)PID->out=2200;//1800
// if(PID->out<-2200)PID->out=-2200;
//
//};
//void SpeedFeedbackControl(PID_Type_Def *PID, Speed_Type_Def *Speed, Ramp_Type_Def *Ramp, Switch_Type_Def *Switch){ //速度反饋控制
//
//
// PID->out_k[9] = PID->out;//保存成上一次輸出值
//
// PID->out_k[0] = 0;
// //誤差=實際速度-目標速度
// PID->err = Speed->ActualSpeed[2] - Speed->PiontSpeed;
// //誤差積分
// PID->integral = PID->err_k[0]+PID->err_k[1]+PID->err_k[2]+PID->err_k[3]+PID->err_k[4]+
//
// PID->err_k[5]+PID->err_k[6]+PID->err_k[7]+PID->err_k[8]+PID->err_k[9]+PID->err;
//
// PID->out = PID->Kp * PID->err ;//+ PID->Ki * PID->integral; //速度環(huán)
//
// PID->out *= 0.02; //縮小速度環(huán)輸出
//
// if(PID->out > 1000) PID->out= 1000 ;
//
// if(PID->out < -1000) PID->out= -1000;
//
//
// for(uint8 n=0; n<9 ; n++){ //循環(huán)誤差保存
//
// PID->err_k[n] = PID->err_k[n+1]; //0~8位賦值
// if( n==8 )
// PID->err_k[n+1] = PID->err; //本次 9位賦值
// }
//
//};
void PWMFeedbackControl(PWM_Type_Def *PWM, PID_Type_Def *PID_Balance, PID_Type_Def *PID_Speed, PID_Type_Def *PID_Path, PID_Type_Def *PID_Diff, Speed_Type_Def *Speed, Switch_Type_Def *Switch){ //PWM疊加反饋控制
PID_Speed->out_k[0] += (PID_Speed->out - PID_Speed->out_k[9]) * 0.0625;//分段成1/16分增量
//PID_Speed->out_k[9]是上次值 ， PID_Speed->out是本次值 ， PID_Speed->out_k[0] 是平滑輸出增量值
//疊加的時候把 平滑輸出增量值 + 上次的值 得到16個周期內(nèi) 每4ms周期應(yīng)給的值
PWM->PiontValue = (int16)(PID_Balance->out)-(int16)(PID_Speed->out_k[0] + PID_Speed->out_k[9]) ; //把所有環(huán)輸出相加
PWM->Differential = (int16)(PID_Diff->out); //差速PWM
PWM->Differential = 0; //差速PWM
PWM->LiftValue = PWM->PiontValue + PWM->Differential;
PWM->RightValue = PWM->PiontValue - PWM->Differential;
};
//uint8 dat;
////車子失控停車
//void OutOfControlSignal(Inductor_Type_Def *InductorA, Inductor_Type_Def *InductorB){
// //藍牙控制
//
// //1的ascall碼是49 收到1停止
// if(dat==49){
//
// ftm_pwm_duty(ftm2,ftm_ch0,0);
// ftm_pwm_duty(ftm2,ftm_ch1,0);
// ftm_pwm_duty(ftm2,ftm_ch2,0);
// ftm_pwm_duty(ftm2,ftm_ch3,0);
//
// while(1)
// { //2的ascall碼是50 收到2啟動
// if(dat==50)break;
// };
//
// dat=0; //清除標志位
//
// }
//
// //保護程序，沒有檢測到電感值就停下來
// if(InductorA->LeftFinal<3&&InductorB->LeftFinal<3)
//{
// ftm_pwm_duty(ftm2,ftm_ch0,0);
// ftm_pwm_duty(ftm2,ftm_ch1,0);
// ftm_pwm_duty(ftm2,ftm_ch2,0);
// ftm_pwm_duty(ftm2,ftm_ch3,0);
//
//
//}
//
//};
/////////////////////////////////////////////////////////////////////////
uint8 timer3ms = 0, timer4_5ms=0,timer24ms=0 ,timer64ms=0;
float OutData[3]; //山外上位機顯示的數(shù)據(jù)
//定義結(jié)構(gòu)體
PID_Type_Def PID_Balance, PID_Speed, PID_Path, PID_Diff;
RUN_Type_Def RUN;
Stop_Type_Def Stop;
Ramp_Type_Def Ramp;
Curve_Type_Def Curve;
Acc_Type_Def Acc;
Gyro_Type_Def Gyro;
Angle_Type_Def Angle;
PWM_Type_Def PWM;
Speed_Type_Def Speed;
Button_Type_Def Button;
Switch_Type_Def Switch;
Inductor_Type_Def InductorA, InductorB;
///////////////////////////////////////////////////////////////////////
int main(void)
{
//由于本程序閉環(huán)較多 調(diào)試過程比較繁瑣 建議一開始的時候 屏蔽掉其他環(huán)
//只調(diào)節(jié)最基礎(chǔ)的環(huán)節(jié) 平衡車主要就是對于兩個電機的控制 其中有一個函數(shù)
//MotorControlFlow函數(shù)是直接控制電機的 這個函數(shù)需要調(diào)試好 其要求就是
//占空比0~1000退后 1000~2000向前 通過調(diào)換驅(qū)動線 和 調(diào)換電機線 或者
//調(diào)換程序中 ch2 3 4 5 的 23順序 45順序來 達到最終的效果 兩個電機
//給1000以上是向前走 1000一下向后走 2000往前最快 0往后最快
//當(dāng)電機輸出調(diào)整好了以后 再調(diào)節(jié)編碼器反饋 我們在MotorControlFlow函數(shù)里
//直接給 PWM->LiftValue = 1200;PWM->RightValue = 1200; 讓兩個電機固定占空比
//轉(zhuǎn)動 這時候 通過IAR在線調(diào)試觀察 編碼器測速值 是否正確 是否對應(yīng)左輪右輪
//(左電機轉(zhuǎn)動對應(yīng)左編碼器 右電機轉(zhuǎn)動對應(yīng)右邊編碼器 不能錯！)
//用手阻止一個電機觀察是否是那個電機的編碼器轉(zhuǎn)速下降 確保 編碼器的方向一致！
//編碼器 和 電機調(diào)整好以后就方便了很多 接下來就是直立 前提是陀螺儀參數(shù)已經(jīng)整定好
//我們調(diào)試直立的時候 要把其他環(huán)都關(guān)閉 方法如下 函數(shù)PWMFeedbackControl 是把所有環(huán)輸出
//整合到電機PWM上的函數(shù) 其中 PWM->PiontValue = (int16)(PID_Balance->out)+ (int16)(PID_Speed->out_k[0] + PID_Speed->out_k[9]) ;
//這句話是把 三個環(huán)的輸出 疊加在一起 我們只需要保留 直立環(huán) 這樣調(diào)節(jié)直立比較順手
//其中還有一個 差速 也要給0 對直立就無影響
//當(dāng)直立調(diào)節(jié)完畢后 速度環(huán)和直立的融合是比較難的 直立和速度是并環(huán) 需要精心調(diào)試，也可以
//跳過速度環(huán)的調(diào)試 直接進入轉(zhuǎn)向環(huán) 與差速環(huán) 他們之間是串級PID的關(guān)系
//如果出現(xiàn)中斷 跑飛 或者 卡住在ADC死循環(huán)里 說明堆棧溢出 調(diào)整KEA128.ICF文件的 堆棧值 擴大 不超過12K即可
get_clk(); //獲取時鐘頻率 必須執(zhí)行
//MPU6050與單片機接線請查看IIC文件最上方注釋
//OLED接線查看OLED文件最上方
IIC_init();
InitMPU6050();
SensorStructurePointerInit( &Acc, &Gyro, &Angle, &Speed);//傳感器參數(shù)初始化
OLED_Init();
CarBalanceSystemInit( &Gyro ); //系統(tǒng).初始化
//在線調(diào)試查看mpu_gyro_x mpu_gyro_y mpu_gyro_z mpu_acc_x mpu_acc_y mpu_acc_z三個變量即可
RUN.Flag[4] = 1;//運行出界標志位
RUN.Flag[5] = 0;//啟動電機標志位
Stop.Flag[0] = 0;//停止線標志位延時啟動標志位
Stop.Flag[1] = 0;//停止標志位
Ramp.Flag[0] = 0;//接近坡道標志位
Ramp.Flag[1] = 0;//上坡道標志位
Curve.Flag[1] = 0;//正差速標志位
Curve.Flag[2] = 0;//反差速標志位
Curve.Flag[3] = 0;//差速數(shù)目
while(1){
// if( timer4_5ms ){
// ftm_pwm_duty(ftm2,ftm_ch3,100);
// ftm_pwm_duty(ftm2,ftm_ch2,0);
GetAccelerationValue( &Acc ); //加速度原始數(shù)據(jù)采集
GetAngleSpeedValue( &Gyro ); //角速度原始數(shù)據(jù)采集
GetSpeedValue(&Speed); //獲取原始數(shù)據(jù)
// GetInductorValue( &InductorA, &InductorB ); //電感電壓原始數(shù)據(jù)采集
AngleSoftwareFilter(&Acc,&Gyro ,&Angle); //軟件角度濾波處理
// SpeedSoftwareFilter( &Speed ); //速度遞歸權(quán)值濾波
// Get_AD_data(&InductorA, &InductorB);
BalanceFeedbackControl( &PID_Balance, &Angle, &Gyro, &Ramp ); //平衡度反饋計算
//DirectionFeedbackControl( &PID_Path, &InductorA, &InductorB, &Gyro, &Speed, &Ramp );//路徑反饋計算
//DifferentialFeedbackControl( &PID_Diff, &Speed, &Gyro); //差速反饋計算
PWMFeedbackControl( &PWM, &PID_Balance, &PID_Speed, &PID_Path, &PID_Diff, &Speed, &Switch );//PWM疊加 直立 方向 速度PWM
MotorControlFlow( &PWM, &Ramp ); //電機PWM輸出控制
// OutOfControlSignal(&InductorA, &InductorB); //失控保護
// OutData[0]=Gyro.X_Final; //山外上位機顯示的數(shù)據(jù)
// OutData[1]=Gyro.Z_Final; //山外上位機顯示的數(shù)據(jù)
// OutData[2]=Angle.FinalValue; //山外上位機顯示的數(shù)據(jù)
// vcan_sendware((uint8 *)OutData,sizeof(OutData)); //山外調(diào)試助手
timer4_5ms = 0; //清除時間標志位
// }
if(timer64ms && timer4_5ms != 1 ){
// 人機交互界面 高速運行不執(zhí)行
// OLED_Print_Num1(7, 6,Angle.FinalValue*100);
// OLED_Print_Num1(7, 4, Speed.ActualLiftPulse[15]);
// OLED_Print_Num1(7, 5, Speed.ActualRightPulse[15]);
// SpeedFeedbackControl( &PID_Speed, &Speed, &Ramp, &Switch ); //速度環(huán)反饋控制
// OLED_Print_Num1(7, 1, InductorA.LeftValue[2]);
// OLED_Print_Num1(7, 2, InductorB.LeftValue[2]);
OLED_Print_Num1(7, 3, InductorA.LeftFinal);
OLED_Print_Num1(7, 4, InductorB.LeftFinal);
timer64ms = 0;
};
}
}
uint8 count = 0;
void PIT_CH0_IRQHandler(void)
{
if( count<84 )
count++;
else count = 0;
// if( count%2==0 ) timer3ms = 1;
if( count%3==0 ) timer4_5ms = 1;
if( count%16==0 ) timer24ms = 1;
if( count%42==0 ) timer64ms=1;
PIT_FlAG_CLR(pit0);//清除中斷標志位
}
|
|
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
