KEA128直立小車源碼

ID:362383 · 發表于 2018-6-30 18:10

kea128的庫

單片機源程序如下:

#include "headfile.h"
/*系統變量初始化部分*/
void PIDStructurePointerInit(PID_Type_Def *PID_Balance, PID_Type_Def *PID_Speed, PID_Type_Def *PID_Path, PID_Type_Def *PID_Diff ){ //PID初始值給定
PID_Balance->Kp = 8;
PID_Balance->Kd = 1.2;
PID_Balance->out = 0;
PID_Speed->Kp = 10;
PID_Speed->Ki = 0.05;
PID_Speed->out = 0;
PID_Path->Kp = 2.0;
PID_Path->Kd = 0.0;
PID_Path->offset = 1.0;
PID_Path->out = 0;
PID_Diff->Kp = 15.0;
PID_Diff->Kd = 2.0;
PID_Diff->Ki = 0.0;
PID_Diff->offset = 0.35;
PID_Diff->out = 0;
for(uint8 n=0; n<10; n++){
PID_Balance->err_k[n] = 0;
PID_Speed->err_k[n] = 0;
PID_Path->err_k[n] = 0;
PID_Diff->err_k[n] = 0;
PID_Speed->out_k[n] = 0;
};
};
void SensorStructurePointerInit(Acc_Type_Def *Acc, Gyro_Type_Def *Gyro, Angle_Type_Def *Angle, Speed_Type_Def *Speed, Inductor_Type_Def *InductorA, Inductor_Type_Def *InductorB){ //結構體指針初始化成員
/*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;
InductorA->LeftFinal=0;
InductorA->RightFinal=0;
InductorA->LeftValue[0]=0;
InductorA->LeftValue[1]=0;
InductorA->LeftValue[2]=0;
InductorB->LeftFinal=0;
InductorB->RightFinal=0;
InductorB->LeftValue[0]=0;
InductorB->LeftValue[1]=0;
InductorB->LeftValue[2]=0;
//角度目標值也就是平衡值沒有轉換成角度值但是是線性關系。
Angle->PiontValue = 2300;
//速度目標值沒有轉換成真實速度與速度線性關系
Speed->PiontSpeed = 2000;
Speed->PiontSpeedMax=5000;
Speed->PiontSpeedMin=1500;
//此處應照車放在賽道上電感在導線正上方最大值電感離導線最遠點為最小值
InductorA->LeftMax = 1200;
InductorA->LeftMin = 125;
InductorA->RightMax = 1200;
InductorA->RightMin = 125;
InductorB->LeftMax = 1200;
InductorB->LeftMin = 125;
InductorB->RightMax = 1200;
InductorB->RightMin = 125;
};
void CarBalanceSystemInit(Gyro_Type_Def *Gyro){//系統初始化
//uart_init(UART1,9600);
gpio_init(H2,GPO,1);
//關于ADC管腳接的東西在headfile.h里有定義
adc_init(ADC0_SE0);
adc_init(ADC0_SE1);
adc_init(ADC0_SE2);
adc_init(ADC0_SE3);
adc_init(ADC0_SE4);
adc_init(ADC0_SE5);
adc_init(ADC0_SE6);
adc_init(ADC0_SE7);
adc_init(ADC0_SE8);
adc_init(ADC0_SE9);
adc_init(ADC0_SE10);
adc_init(ADC0_SE11);
adc_init(ADC0_SE12);
adc_init(ADC0_SE13);
adc_init(ADC0_SE14);
adc_init(ADC0_SE15);
OLED_Init();
OLED_ShowString(0,0,"SysInit Finish!");
OLED_ShowString(0,16,"Car balance2.0V");
OLED_Refresh_Gram();
// 使用數字陀螺儀要初始化IIC
//IIC_init();
//此處陀螺儀中值獲取很重要不過數字陀螺儀默認就是中值無需獲取
// 如果數字陀螺儀中值為0 那么此處直接賦值0即可
Gyro->Z_Offset = adc_once(ADC0_SE2, ADC_12bit);
Gyro->X_Offset = adc_once(ADC0_SE3, ADC_12bit);
systick_delay_ms(10);
//左電機
ftm_pwm_init(ftm2,ftm_ch2,13000,0);
ftm_pwm_init(ftm2,ftm_ch3,13000,0);
//右電機
ftm_pwm_init(ftm2,ftm_ch4,13000,0);
ftm_pwm_init(ftm2,ftm_ch5,13000,0);
systick_delay_ms(10);
//ftm的計數引腳可查看KEA128_port_cfg.h文件內的FTM0_COUNT_PIN與FTM1_COUNT_PIN的宏定義得知
//一個ftm同一時間只能實現一種功能，如果用于測速就不能用于輸出PWM
///////////////////////////////////////////////////////////////////////////////////////////////
ftm_count_init(ftm0); //對E0引腳輸入的脈沖進行計數 E0接編碼器LSB
gpio_init(E1,GPI,0); //用于判斷方向 C5接編碼器DIR
port_pull(E1); //IO上拉
ftm_count_init(ftm1); //對E7引腳輸入的脈沖進行計數 E7接編碼器LSB
gpio_init(H6,GPI,0); //用于判斷方向 H5接編碼器DIR
port_pull(H6); //IO上拉
//////////////////////////////////////////////////////////////////////////////////////////////
//定時器中斷
pit_init(pit0,0x9000);//1.539ms
systick_delay_ms(10);
};
//絕對值函數必須要取地址放入形參
void AbsoluteValue(float *Value, float *Return){
if( (*Value) <0) (*Return) = -1*(*Value);
else *Value = *Return;
}
/*原始數據獲取部分*/
void GetAccelerationValue(Acc_Type_Def *Acc){ //獲取ACC原始數據
Acc->Z_Value[0] = Acc->Z_Value[1];
Acc->Z_Value[1] = Acc->Z_Value[2];
Acc->Z_Value[2] = ACC_Z;//本句是獲取當前加速度上兩句都是上次值和上上次值
//利用多次值進行濾波參數自行調整看效果
Acc->Z_Final = 0.33*Acc->Z_Value[2]+0.33*Acc->Z_Value[1]+0.33*Acc->Z_Value[0];
};
void GetAngleSpeedValue(Gyro_Type_Def *Gyro){ //獲取Gyro原始數據
Gyro->Z_Value[0] = Gyro->Z_Value[1];
Gyro->Z_Value[1] = Gyro->Z_Value[2];
Gyro->Z_Value[2] = GYRO_Z - Gyro->Z_Offset;//如果是數字陀螺儀請給Gyro->Z_Offset賦值在初始化的時候
Gyro->X_Value[0] = Gyro->X_Value[1]; //前前次
Gyro->X_Value[1] = Gyro->X_Value[2]; //前次
Gyro->X_Value[2] = GYRO_X - Gyro->X_Offset; //本次
};
void GetInductorValue(Inductor_Type_Def *InductorA, Inductor_Type_Def *InductorB){ //獲取inductor原始數據
InductorA->LeftValue[0] = InductorA->LeftValue[1];
InductorA->LeftValue[1] = InductorA->LeftValue[2];
InductorA->LeftValue[2] = L1;//本句是獲取當前電感值 L1在haedfile.h文件中有定義
InductorA->RightValue[0] = InductorA->RightValue[1];
InductorA->RightValue[1] = InductorA->RightValue[2];
InductorA->RightValue[2] = L2;//本句是獲取當前電感值 L2在haedfile.h文件中有定義
InductorB->LeftValue[0] = InductorB->LeftValue[1];
InductorB->LeftValue[1] = InductorB->LeftValue[2];
InductorB->LeftValue[2] = L3;//本句是獲取當前電感值 L3在haedfile.h文件中有定義
InductorB->RightValue[0] = InductorB->RightValue[1];
InductorB->RightValue[1] = InductorB->RightValue[2];
InductorB->RightValue[2] = L4;//本句是獲取當前電感值 L4在haedfile.h文件中有定義
};
void GetSpeedValue(Speed_Type_Def *Speed){ //獲取Speed原始數據
for(uint8 n=0; n<15 ; n++){ //循環誤差保存
//存儲前14次的值
Speed->ActualLiftPulse[n] = Speed->ActualLiftPulse[n+1]; //0~14位賦值
if( n==14 ){ //當前值 15次最新值
Speed->ActualLiftPulse[n+1] = ftm_count_get(ftm1)*(-1); //本次 15位賦值
ftm_count_clean(ftm1); //清除ftm1計數值
}//此處一定要修改好根據你們實際的硬件對應好左右輪子因為編碼器是對著安裝的那么
//在車子往前走的時候左右是相反的所以要把其中一個負值編碼器變為正的
//兩個電機存在同樣的問題都是對著安裝的
}
for(uint8 n=0; n<15 ; 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計數值
}
}
if(gpio_get(H6)) //檢測 ftm1的轉向 H6 E1是針對左右編碼器的要弄清楚不要到時候改過以后
//左邊編碼器的方向去控制右邊編碼器的正負號那就完蛋了
Speed->ActualLiftPulse[15] = Speed->ActualLiftPulse[15];
else
Speed->ActualLiftPulse[15] = -Speed->ActualLiftPulse[15];
if(gpio_get(E1)) //檢測 ftm0的轉向
Speed->ActualRightPulse[15] = Speed->ActualRightPulse[15];
else
Speed->ActualRightPulse[15] = -Speed->ActualRightPulse[15]; //這個負號取決于你車向前走時編碼器值是否為正
};
void GetButtonStatus(Button_Type_Def *Button){ //Button原始數據獲取
Button->Up[0] = Button->Up[1];
Button->Up[1] = Button->Up[2];
Button->Up[2] = Button->Up[3];
Button->Up[3] = Button->Up[4];
Button->Up[4] = UP;//第四次是最新的值按鍵值在haedfile.h文件中有定義
Button->Down[0] = Button->Down[1];
Button->Down[1] = Button->Down[2];
Button->Down[2] = Button->Down[3];
Button->Down[3] = Button->Down[4];
Button->Down[4] = DOWN;//第四次是最新的值按鍵值在haedfile.h文件中有定義
Button->Life[0] = Button->Life[1];
Button->Life[1] = Button->Life[2];
Button->Life[2] = Button->Life[3];
Button->Life[3] = Button->Life[4];
Button->Life[4] = LIFE;//第四次是最新的值按鍵值在haedfile.h文件中有定義
Button->Right[0] = Button->Right[1];
Button->Right[1] = Button->Right[2];
Button->Right[2] = Button->Right[3];
Button->Right[3] = Button->Right[4];
Button->Right[4] = RIGHT;//第四次是最新的值按鍵值在haedfile.h文件中有定義
Button->Enter[0] = Button->Enter[1];
Button->Enter[1] = Button->Enter[2];
Button->Enter[2] = Button->Enter[3];
Button->Enter[3] = Button->Enter[4];
Button->Enter[4] = ENTER;//第四次是最新的值按鍵值在haedfile.h文件中有定義
};
void GetSwitchStatus(Switch_Type_Def *Switch){ //開關原始數據獲取
Switch->Bm1[0] = Switch->Bm1[1];
Switch->Bm1[1] = Switch->Bm1[2];
Switch->Bm1[2] = SW1; //撥碼開關第四次是最新的值在haedfile.h文件中有定義
Switch->Bm2[0] = Switch->Bm2[1];
Switch->Bm2[1] = Switch->Bm2[2];
Switch->Bm2[2] = SW2; //撥碼開關第四次是最新的值在haedfile.h文件中有定義
Switch->Bm3[0] = Switch->Bm3[1];
Switch->Bm3[1] = Switch->Bm3[2];
Switch->Bm3[2] = SW3; //撥碼開關第四次是最新的值在haedfile.h文件中有定義
Switch->Bm4[0] = Switch->Bm4[1];
Switch->Bm4[1] = Switch->Bm4[2];
Switch->Bm4[2] = SW4; //撥碼開關第四次是最新的值在haedfile.h文件中有定義
Switch->Bm5[0] = Switch->Bm5[1];
Switch->Bm5[1] = Switch->Bm5[2];
Switch->Bm5[2] = SW5; //撥碼開關第四次是最新的值在haedfile.h文件中有定義
Switch->Bm6[0] = Switch->Bm6[1];
Switch->Bm6[1] = Switch->Bm6[2];
Switch->Bm6[2] = SW6; //撥碼開關第四次是最新的值在haedfile.h文件中有定義
};
/*原始數據處理部分*/
void GyroHighPassFilter(Gyro_Type_Def *Gyro){ //Gyro濾波
float K =0.58;//權值濾波看情況調節
Gyro->Z_Value[2] = K * Gyro->Z_Value[2] + (1-K) * Gyro->Z_Value[1];
Gyro->Z_Final = Gyro->Z_Value[2];
Gyro->X_Value[2] = K * Gyro->X_Value[2] + (1-K) * Gyro->X_Value[1];
Gyro->X_Final = Gyro->X_Value[2];
//權值濾波去除高頻干擾
};
void AngleHardwareFilter(Angle_Type_Def *Angle){ //角度濾波
//本函數是直接采集角度的只有模擬陀螺儀和加速度計進行硬件融合直接解算角度值
Angle->ActualValue[0] = Angle->ActualValue[1];
Angle->ActualValue[1] = Angle->ActualValue[2];
Angle->ActualValue[2] = ANGLE; //此處是獲取最新角度值
//濾波 Angle->FinalValue為最終值
Angle->FinalValue = 0.6*Angle->ActualValue[2] + 0.2*Angle->ActualValue[1] + 0.2*Angle->ActualValue[0];
};
void AngleSoftwareFilter(Acc_Type_Def *Acc, Gyro_Type_Def *Gyro , Angle_Type_Def *Angle){//角度濾波
//本函數是適用于數字陀螺儀通過軟件互補融合濾波解算角度
Acc->Z_Final = 0.9*Acc->Z_Value[2] + 0.05*Acc->Z_Value[1] + 0.05*Acc->Z_Value[0];//權值低通濾波
Gyro->Z_Final = 0.92*Gyro->Z_Value[2] + 0.05*Gyro->Z_Value[1] + 0.03*Gyro->Z_Value[0];//權值低通濾波
//加速度角速度采集
//此處參數需要根據清華方案里的方法去調節參數看波形法
Angle->FinalValue = 0.92*(Angle->FinalValue + 0.05*Gyro->Z_Final) + 0.08*(Acc->Z_Final);//融合濾波
};
void InductorSoftwareFilter(Inductor_Type_Def *InductorA, Inductor_Type_Def *InductorB, Angle_Type_Def *Angle, Switch_Type_Def *Switch){//電感值歸一濾波
/*權值濾波警告無視 */
InductorA->LeftFinal = 0.8*InductorA->LeftValue[2] + 0.15*InductorA->LeftValue[1] + 0.05*InductorA->LeftValue[0];//-InductorA->LeftMin;
InductorA->RightFinal = 0.8*InductorA->RightValue[2] + 0.15*InductorA->RightValue[1] + 0.05*InductorA->RightValue[0];//-InductorA->RightMin;
InductorB->LeftFinal = 0.8*InductorB->LeftValue[2] + 0.15*InductorB->LeftValue[1] + 0.05*InductorB->LeftValue[0];
InductorB->RightFinal = 0.8*InductorB->RightValue[2] + 0.15*InductorB->RightValue[1] + 0.05*InductorB->RightValue[0];
/*數據歸一化到0~1000*/
InductorA->LeftFinal = 1000*( (float)(InductorA->LeftFinal-InductorA->LeftMin)/(InductorA->LeftMax-InductorA->LeftMin) );
InductorA->RightFinal = 1000*( (float)(InductorA->RightFinal-InductorA->RightMin)/(InductorA->RightMax-InductorA->RightMin) );
InductorB->LeftFinal = 1000*( (float)(InductorB->LeftFinal-InductorB->LeftMin)/(InductorB->LeftMax-InductorB->LeftMin) );
InductorB->RightFinal = 1000*( (float)(InductorB->RightFinal-InductorB->RightMin)/(InductorB->RightMax-InductorB->RightMin) );
if(InductorA->LeftFinal>999)InductorA->LeftFinal=999;
if(InductorA->LeftFinal< 0)InductorA->LeftFinal= 0;
if(InductorA->RightFinal>999)InductorA->RightFinal=999;
if(InductorA->RightFinal< 0)InductorA->RightFinal= 0;
if(InductorB->LeftFinal>999)InductorB->LeftFinal=999;
if(InductorB->LeftFinal< 0)InductorB->LeftFinal= 0;
if(InductorB->RightFinal>999)InductorB->RightFinal=999;
if(InductorB->RightFinal< 0)InductorB->RightFinal= 0;
//電感AB A 是靠外側的電感 B是靠內側電感 RL就是左右
InductorA->AndValues = InductorA->LeftFinal+InductorA->RightFinal;//電感和值
InductorA->LeftDerivative = InductorA->LeftValue[2] - InductorA->LeftValue[0]; //左點感變化率
InductorA->RightDerivative= InductorA->RightValue[2] - InductorA->RightValue[0];//右電感變化率
InductorB->LeftDerivative = InductorB->LeftValue[2] - InductorB->LeftValue[0];
};
void SpeedSoftwareFilter(Speed_Type_Def *Speed){ //速度低通濾波
/*警告無視*/
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]);
//濾波
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 ButtonLowPassFilter(Button_Type_Def *Button){ //按鍵低通濾波
Button->Up_Final = Button->Up[0] || Button->Up[1] || Button->Up[2] || Button->Up[3] || Button->Up[4];
//有一個1 最終值就是1 全部為0 最終值才為0
if(Button->Up_Final==1) Button->Up_Final = 0;//取反以1為有效值
else Button->Up_Final = 1;
Button->Down_Final = Button->Down[0] || Button->Down[1] || Button->Down[2] || Button->Down[3] || Button->Down[4];
//有一個1 最終值就是1 全部為0 最終值才為0
if(Button->Down_Final==1) Button->Down_Final = 0;//取反以1為有效值
else Button->Down_Final = 1;
Button->Life_Final = Button->Life[0] || Button->Life[1] || Button->Life[2] || Button->Life[3] || Button->Life[4];
//有一個1 最終值就是1 全部為0 最終值才為0
if(Button->Life_Final==1) Button->Life_Final = 0;//取反以1為有效值
else Button->Life_Final = 1;
Button->Right_Final = Button->Right[0] || Button->Right[1] || Button->Right[2] || Button->Right[3] || Button->Right[4];
//有一個1 最終值就是1 全部為0 最終值才為0
if(Button->Right_Final==1) Button->Right_Final = 0;//取反以1為有效值
else Button->Right_Final = 1;
Button->Enter_Final = Button->Enter[0] || Button->Enter[1] || Button->Enter[2] || Button->Enter[3] || Button->Enter[4];
//有一個1 最終值就是1 全部為0 最終值才為0
if(Button->Enter_Final==1) Button->Enter_Final = 0;//取反以1為有效值
else Button->Enter_Final = 1;
};
void SwitchLowPassFilter(Switch_Type_Def *Switch){ //開關低通濾波
Switch->Bm1_Final = Switch->Bm1[0] || Switch->Bm1[1] || Switch->Bm1[2];
if(Switch->Bm1_Final==1) Switch->Bm1_Final = 0;//取反以1為有效值
else Switch->Bm1_Final = 1;
Switch->Bm2_Final = Switch->Bm2[0] || Switch->Bm2[1] || Switch->Bm2[2];
if(Switch->Bm2_Final==1) Switch->Bm2_Final = 0;//取反以1為有效值
else Switch->Bm2_Final = 1;
Switch->Bm3_Final = Switch->Bm3[0] || Switch->Bm3[1] || Switch->Bm3[2];
if(Switch->Bm3_Final==1) Switch->Bm3_Final = 0;//取反以1為有效值
else Switch->Bm3_Final = 1;
Switch->Bm4_Final = Switch->Bm4[0] || Switch->Bm4[1] || Switch->Bm4[2];
if(Switch->Bm4_Final==1) Switch->Bm4_Final = 0;//取反以1為有效值
else Switch->Bm4_Final = 1;
Switch->Bm5_Final = Switch->Bm5[0] || Switch->Bm5[1] || Switch->Bm5[2];
if(Switch->Bm5_Final==1) Switch->Bm5_Final = 0;//取反以1為有效值
else Switch->Bm5_Final = 1;
Switch->Bm6_Final = Switch->Bm6[0] || Switch->Bm6[1] || Switch->Bm6[2];
if(Switch->Bm6_Final==1) Switch->Bm6_Final = 0;//取反以1為有效值
else Switch->Bm6_Final = 1;
};
void OutOfControlSignal(Speed_Type_Def *Speed, PID_Type_Def *PID){ //速度失控信號檢測
if( (Speed->ActualSpeed[2]-Speed->ActualSpeed[1]) > 400 && Speed->ActualSpeed[2]>10000){
//加速度過大車輪空轉
ftm_pwm_duty(ftm2,ftm_ch2,0);
ftm_pwm_duty(ftm2,ftm_ch3,0);
ftm_pwm_duty(ftm2,ftm_ch4,0);
ftm_pwm_duty(ftm2,ftm_ch5,0);
//
while(1){
// led(LED0,LED_ON); //根據你們的硬件設置報警燈
systick_delay_ms(80);
// led(LED0,LED_OFF);
systick_delay_ms(80);
};
}
};
/*被控制量處理部分*/
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
//一般情況下調直立時先給一個P參數 D給0 就能立起來如果發現不能立瘋轉
//多半是 pid方向相反把(Angle->PiontValue-Angle->FinalValue) 里面調換下位置即可
};
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){ //方向反饋控制
InductorA->RightFinal = PID->offset*InductorA->RightFinal;
InductorA->LeftFinal = (2-PID->offset)*InductorA->LeftFinal;
//調整零偏
//主要是在用 A電感
PID->err = 10.0*(sqrt(1.0*InductorA->RightFinal)-sqrt(1.0*InductorA->LeftFinal) )/ (InductorA->RightFinal+InductorA->LeftFinal);
if( InductorA->AndValues < 25 && Ramp->Flag[0]!=1) {//丟線找回且不在坡道
PID->err += 0.55*PID->err_k[6]; //找回強度
}
else if( InductorA->AndValues < 50 && Ramp->Flag[0]!=1) {//丟線找回
PID->err += 0.50*PID->err_k[6]; //找回強度
}
else if( InductorA->AndValues < 75 && Ramp->Flag[0]!=1) {//丟線找回
PID->err += 0.45*PID->err_k[7]; //找回強度
}
else if( InductorA->AndValues < 100 && Ramp->Flag[0]!=1) {//丟線找回
PID->err += 0.40*PID->err_k[7]; //找回強度
}
else if( InductorA->AndValues < 125 && Ramp->Flag[0]!=1) {//丟線找回
PID->err += 0.35*PID->err_k[8]; //找回強度
}
else if( InductorA->AndValues< 150 && Ramp->Flag[0]!=1) {//丟線找回
PID->err += 0.30*PID->err_k[8]; //找回強度
}
else if( InductorA->AndValues < 175 && Ramp->Flag[0]!=1) {//丟線找回
PID->err += 0.25*PID->err_k[9]; //找回強度9
}
else if( InductorA->AndValues < 200 && Ramp->Flag[0]!=1) {//丟線找回200
PID->err += 0.2*PID->err_k[9]; //找回強度9
}
if(InductorA->AndValues<150) PID->err = 0;
PID->out = PID->Kp * PID->err * Speed->ActualInstantaneous[2] + PID->Kd * Gyro->X_Final;
//轉向PID輸出計算 Gyro->X_Final; 是轉向角速度也是D控制
Speed->PiontDifferential = (int16)(PID->out); //給定差速環目標值
//把轉向環計算出來的輸出值給差速目標環
for(uint8 n=0; n<9 ; n++){ //循環誤差保存
PID->err_k[n] = PID->err_k[n+1]; //0~8位賦值
if( n==8 )
PID->err_k[n+1] = PID->err; //本次 9位賦值
}
};
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);
// 得到差速值是編碼器和轉向角速度共同決定差速反饋值
//Speed->PiontDifferential = 0;
//差速目標- 差速實際值
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++){ //循環誤差保存
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>2000)PID->out=2000;//1800
if(PID->out<-2000)PID->out=-2000;
};
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;
PID->out *= 0.05; //縮小速度環輸出
if(PID->out > 4800) PID->out= 4800; //后退最低角度4200
if(PID->out < -1800) PID->out= -1800;//前進最低角度-1800
for(uint8 n=0; n<9 ; 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疊加反饋控制
float DiffK=1;
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個周期內每4ms周期應給的值
PWM->PiontValue = (int16)(PID_Balance->out)+ (int16)(PID_Speed->out_k[0] + PID_Speed->out_k[9]) ;
//把所有環輸出相加
PWM->Differential = (int16)(PID_Diff->out);
//差速PWM
if(PWM->Differential>0){
PWM->LiftValue = PWM->PiontValue + (DiffK) * PWM->Differential;
PWM->RightValue = PWM->PiontValue - (2-DiffK) * PWM->Differential;
}
if(PWM->Differential<0){ //差速小于零時左邊給目標占空比+ 系數*差速
PWM->LiftValue = PWM->PiontValue + (2-DiffK) * PWM->Differential;
PWM->RightValue = PWM->PiontValue - (DiffK) * PWM->Differential;
}
if(PWM->Differential==0){ //差速=0時左右給一樣的目標占空比
PWM->LiftValue = PWM->PiontValue;
PWM->RightValue = PWM->PiontValue;
}
};
void ButtonFunctionControl(PID_Type_Def *PID_Balance, PID_Type_Def *PID_Speed, PID_Type_Def *PID_Path, PID_Type_Def *PID_Diff, Angle_Type_Def *Angle, Speed_Type_Def *Speed, Inductor_Type_Def *InductorA, Inductor_Type_Def *InductorB, Button_Type_Def *Button, uint8 *Page){ //按鍵功能實現
//OLED 按鍵調節PID參數用的
if(Button->Life_Final==1)if(*Page>0)(*Page)--;
if(Button->Right_Final==1)if(*Page<135)(*Page)++;
//page0
if(*Page<=5){ //UP kp
if(Button->Up_Final==1 ){
if(PID_Balance->Kp<30) PID_Balance->Kp = PID_Balance->Kp+0.01;
}
if(Button->Down_Final==1 ){
if(PID_Balance->Kp>0) PID_Balance->Kp = PID_Balance->Kp-0.01;
}
}
if(*Page<=10 && *Page>5){ //UP kd
if(Button->Up_Final==1){
if(PID_Balance->Kd<30) PID_Balance->Kd = PID_Balance->Kd+0.01;
}
if(Button->Down_Final==1){
if(PID_Balance->Kd>0) PID_Balance->Kd = PID_Balance->Kd-0.01;
}
}
if(*Page<=15 && *Page>10){ // angle piont
if(Button->Up_Final==1){
if(Angle->PiontValue<2400) Angle->PiontValue = Angle->PiontValue+1;
}
if(Button->Down_Final==1){
if(Angle->PiontValue>1600) Angle->PiontValue = Angle->PiontValue-1;
}
}
if(*Page<=20 && *Page>15){ //speed kp
if(Button->Up_Final==1){
if(PID_Speed->Kp<30)PID_Speed->Kp = PID_Speed->Kp+0.1;
}
if(Button->Down_Final==1){
if(PID_Speed->Kp>0)PID_Speed->Kp = PID_Speed->Kp-0.1;
}
}
if(*Page<=25 && *Page>20){ //speed ki
if(Button->Up_Final==1){
if(PID_Speed->Ki<10)PID_Speed->Ki = PID_Speed->Ki+0.001;
}
if(Button->Down_Final==1){
if(PID_Speed->Ki>0)PID_Speed->Ki = PID_Speed->Ki-0.001;
}
}
if(*Page<=30 && *Page>25){ //speeed piont
if(Button->Up_Final==1){
if(Speed->PiontSpeed<10000)Speed->PiontSpeed = Speed->PiontSpeed+10;
}
if(Button->Down_Final==1){
if(Speed->PiontSpeed>0)Speed->PiontSpeed = Speed->PiontSpeed-10;
}
}
//page1
if(*Page<=35 && *Page>30){ //path kp
if(Button->Up_Final==1){
if(PID_Path->Kp<30)PID_Path->Kp = PID_Path->Kp+0.01;
}
if(Button->Down_Final==1 ){
if(PID_Path->Kp>0)PID_Path->Kp = PID_Path->Kp-0.01;
}
}
if(*Page<=40 && *Page>35){ //path kd
if(Button->Up_Final==1){
if(PID_Path->Kd<30)PID_Path->Kd = PID_Path->Kd+0.01;
}
if(Button->Down_Final==1){
if(PID_Path->Kd>0)PID_Path->Kd = PID_Path->Kd-0.01;
}
}
if(*Page<=45 && *Page>40){ //path piont
if(Button->Up_Final==1){
if(PID_Path->offset<2)PID_Path->offset = PID_Path->offset+0.001;
}
if(Button->Down_Final==1){
if(PID_Path->offset>0)PID_Path->offset = PID_Path->offset-0.001;
}
}
if(*Page<=50 && *Page>45){ //diff kp
if(Button->Up_Final==1){
if(PID_Diff->Kp<30)PID_Diff->Kp = PID_Diff->Kp+0.01;
}
if(Button->Down_Final==1 ){
if(PID_Diff->Kp>0)PID_Diff->Kp = PID_Diff->Kp-0.01;
}
}
if(*Page<=55 && *Page>50){ //diff kd
if(Button->Up_Final==1){
if(PID_Diff->Kd<30)PID_Diff->Kd = PID_Diff->Kd+0.01;
}
if(Button->Down_Final==1 ){
if(PID_Diff->Kd>0)PID_Diff->Kd = PID_Diff->Kd-0.01;
}
}
if(*Page<=60 && *Page>55){ //diff kd
if(Button->Up_Final==1){
if(PID_Diff->Ki<30)PID_Diff->Ki = PID_Diff->Ki+0.01;
}
if(Button->Down_Final==1 ){
if(PID_Diff->Ki>0)PID_Diff->Ki = PID_Diff->Ki-0.01;
}
}
if(*Page<=65 && *Page>60){ //diff offset
if(Button->Up_Final==1){
if(PID_Diff->offset<1)PID_Diff->offset = PID_Diff->offset+0.001;
}
if(Button->Down_Final==1 ){
if(PID_Diff->offset>0)PID_Diff->offset = PID_Diff->offset-0.001;
}
}
//page2
if(*Page<=70 && *Page>65){ //左電感最大值
if(Button->Up_Final==1 && InductorA->LeftMax < 4000)InductorA->LeftMax ++;
if(Button->Down_Final==1 && InductorA->LeftMax > 100)InductorA->LeftMax --;
}
if(*Page<=75 && *Page>70){ //左電感最小值
if(Button->Up_Final==1 && InductorA->LeftMin < 4000)InductorA->LeftMin ++;
if(Button->Down_Final==1 && InductorA->LeftMin > 100)InductorA->LeftMin --;
}
if(*Page<=80 && *Page>75){ //右電感最大值
if(Button->Up_Final==1 && InductorA->RightMax < 4000)InductorA->RightMax ++;
if(Button->Down_Final==1 && InductorA->RightMax > 100)InductorA->RightMax --;
}
if(*Page<=85 && *Page>80){ //右電感最小值
if(Button->Up_Final==1 && InductorA->RightMin< 4000)InductorA->RightMin ++;
if(Button->Down_Final==1 && InductorA->RightMin> 100)InductorA->RightMin --;
}
if(*Page<=90 && *Page>85){ //中間電感最大值
if(Button->Up_Final==1 && InductorB->LeftMax< 4000)InductorB->LeftMax ++;
if(Button->Down_Final==1 && InductorB->LeftMax> 100)InductorB->LeftMax --;
}
if(*Page<=95 && *Page>90){//中間電感最小值
if(Button->Up_Final==1 && InductorB->LeftMin< 4000)InductorB->LeftMin ++;
if(Button->Down_Final==1 && InductorB->LeftMin> 100)InductorB->LeftMin --;
}
if(*Page<=100 && *Page>95){//最大速度
if(Button->Up_Final==1 && Speed->PiontSpeedMax<10000)Speed->PiontSpeedMax ++;
if(Button->Down_Final==1 && Speed->PiontSpeedMax >0)Speed->PiontSpeedMax --;
}
if(*Page<=105 && *Page>100){//最小速度
if(Button->Up_Final==1 && Speed->PiontSpeedMin < Speed->PiontSpeedMax)Speed->PiontSpeedMin ++;
if(Button->Down_Final==1 && Speed->PiontSpeedMin > 0)Speed->PiontSpeedMin --;
}
//
// //page3
// if(*Page<=110 && *Page>105){//減速距離
//
// if(Button->Up_Final==1 && Speed->SlowDistance < 6000)Speed->SlowDistance ++;
//
//
// if(Button->Down_Final==1 && Speed->SlowDistance > 0)Speed->SlowDistance --;
// }
//
// if(*Page<=115 && *Page>110){//加速距離
//
// if(Button->Up_Final==1 && Speed->AccDistance < 6000)Speed->AccDistance ++;
//
//
// if(Button->Down_Final==1 && Speed->AccDistance > 0)Speed->AccDistance --;
// }
//
// if(*Page<=120 && *Page>115){//錯誤距離
//
// if(Button->Up_Final==1 && Speed->FaultDistance < 150000)Speed->FaultDistance+=100;
//
//
// if(Button->Down_Final==1 && Speed->FaultDistance > 100)Speed->FaultDistance-=100;
// }
};
/*輸出量處理部分*/
void PrintParameterToOLED(PID_Type_Def *PID_Balance, PID_Type_Def *PID_Speed, PID_Type_Def *PID_Path, PID_Type_Def *PID_Diff, Angle_Type_Def *Angle, Speed_Type_Def *Speed , PWM_Type_Def *PWM, Inductor_Type_Def *InductorA, Inductor_Type_Def *InductorB, Curve_Type_Def *Curve, uint8 Page){ //打印參數到OLED上
uint8 mode[10];
static uint8 Last=0;
if(Page<=30){
if(Page<=5) mode[0] = 0;
else mode[0] = 1;
if(Page<=10 && Page>5) mode[1] =0;
else mode[1] = 1;
if(Page<=15 && Page>10) mode[2] =0;
else mode[2] = 1;
if(Page<=20 && Page>15) mode[3] =0;
else mode[3] = 1;
if(Page<=25 && Page>20) mode[4] =0;
else mode[4] = 1;
if(Page<=30 && Page>25) mode[5] =0;
else mode[5] = 1;
if( Last!=1 ){
OLED_Fill(0,0,127,63,0);
Last = 1;
}
OLED_ShowString(0,0,"UP P D ");
OLED_ShowNum(36,0,(int)(PID_Balance->Kp*10),3,12,mode[0]);
OLED_ShowNum(72,0,(int)(PID_Balance->Kd*10),3,12,mode[1]);
OLED_ShowString(0,12 ,"AnglePiont ");
OLED_ShowNum(96,12,(int)(Angle->PiontValue),4,12,mode[2]);
OLED_ShowString(0,24 ,"Speed P I ");
OLED_ShowNum(56,24,(int)(PID_Speed->Kp*10),3,12,mode[3]);
OLED_ShowNum(96,24,(int)(PID_Speed->Ki*1000),3,12,mode[4]);
OLED_ShowString(0,36 ,"SpeedPiont ");
OLED_ShowNum(96,36,(int)(Speed->PiontSpeed),4,12,mode[5]);
OLED_ShowNum(32,48,(int)(InductorA->LeftFinal),3,12,0);
OLED_ShowNum(56,48,(int)(InductorA->RightFinal),3,12,0);
OLED_ShowNum(86,48,(int)(InductorB->LeftFinal),3,12,0);
}
else if(Page>30 && Page<=65){
if(Page<=35 && Page>30) mode[0] = 0;
else mode[0] = 1;
if(Page<=40 && Page>35) mode[1] =0;
else mode[1] = 1;
if(Page<=45 && Page>40) mode[2] =0;
else mode[2] = 1;
if(Page<=50 && Page>45) mode[3] =0;
else mode[3] = 1;
if(Page<=55 && Page>50) mode[4] =0;
else mode[4] = 1;
if(Page<=60 && Page>55) mode[5] =0;
else mode[5] = 1;
if(Page<=65 && Page>60) mode[6] =0;
else mode[6] = 1;
if( Last!=2 ){
OLED_Fill(0,0,127,63,0);
Last = 2;
}
OLED_ShowString(0,0,"Path P D ");
OLED_ShowNum(48,0,(int)(PID_Path->Kp*10),3,12,mode[0]);
OLED_ShowNum(96,0,(int)(PID_Path->Kd*100),3,12,mode[1]);
OLED_ShowString(0,12 ,"PathPiont ");
OLED_ShowNum(84,12,(int)(PID_Path->offset*100),3,12,mode[2]);
OLED_ShowString(0,24 ,"Diff P D ");
OLED_ShowNum(56,24,(int)(PID_Diff->Kp*10),3,12,mode[3]);
OLED_ShowNum(96,24,(int)(PID_Diff->Kd*10),3,12,mode[4]);
OLED_ShowString(0,36 ,"Diff I S ");
OLED_ShowNum(56,36,(int)(PID_Diff->Ki*10),3,12,mode[5]);
OLED_ShowNum(96,36,(int)(PID_Diff->offset*100),3,12,mode[6]); //差速與陀螺儀融合比率
OLED_ShowNum(32,48,(int)(InductorA->LeftFinal),3,12,0);
OLED_ShowNum(56,48,(int)(InductorA->RightFinal),3,12,0);
OLED_ShowNum(86,48,(int)(InductorB->LeftFinal),3,12,0);
}
else if(Page>65 && Page<=105){
if(Page<=70 && Page>65) mode[0] = 0;
else mode[0] = 1;
if(Page<=75 && Page>70) mode[1] =0;
else mode[1] = 1;
if(Page<=80 && Page>75) mode[2] =0;
else mode[2] = 1;
if(Page<=85 && Page>80) mode[3] =0;
else mode[3] = 1;
if(Page<=90 && Page>85) mode[4] =0;
else mode[4] = 1;
if(Page<=95 && Page>90) mode[5] =0;
else mode[5] = 1;
if(Page<=100 && Page>95) mode[6] =0;
else mode[6] = 1;
if(Page<=105 && Page>100) mode[7] =0;
else mode[7] = 1;
if( Last!=3 ){
OLED_Fill(0,0,127,63,0);
Last = 3;
}
OLED_ShowString(0,0 ,"LAMAX");
OLED_ShowNum(38,0,(int)(InductorA->LeftMax),4,12,mode[0]);
OLED_ShowString(68,0 ,"LAMIN");
OLED_ShowNum(108,0,(int)(InductorA->LeftMin),3,12,mode[1]);
OLED_ShowString(0,12 ,"RAMAX");
OLED_ShowNum(38,12,(int)(InductorA->RightMax),4,12,mode[2]);
OLED_ShowString(68,12 ,"RAMIN");
OLED_ShowNum(108,12,(int)(InductorA->RightMin),3,12,mode[3]);
OLED_ShowString(0,24 ,"ZBMAX");
OLED_ShowNum(38,24,(int)(InductorB->LeftMax),4,12,mode[4]);
OLED_ShowString(68,24 ,"ZBMIN");
OLED_ShowNum(108,24,(int)(InductorB->LeftMin),3,12,mode[5]);
OLED_ShowString(0,36 ,"SpeedMax");
OLED_ShowNum(84,36,(uint32)(Speed->PiontSpeedMax),4,12,mode[6]);
OLED_ShowString(0,48 ,"SpeedMin");
OLED_ShowNum(84,48,(uint32)(Speed->PiontSpeedMin),4,12,mode[7]);
}
OLED_Refresh_Gram();
};
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的時候應該是不轉的
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);
//此處必須先調整好保證 PWM值是 >1000時是前進
//PWM值< 1000 是后退而且兩輪必須一致
//能改硬件就改驅動線和電機引線例如
//給同樣都是 1200 左邊電機慢速往前右邊慢速往后這時候調換右電機線
//如果出現如下情況給1200 理論上是正轉 20%占空比但出現轉速很快說明
//驅動線序不對可以在這里調換通道或者直接改線序
if(PWM->LiftValue>1000){ //邊輪前進 1000~2000
ftm_pwm_duty(ftm2,ftm_ch2,0);
ftm_pwm_duty(ftm2,ftm_ch3,PWM->LiftValue-1000);
}
else if(PWM->LiftValue<1000){ //左輪是后退 0~1000
ftm_pwm_duty(ftm2,ftm_ch2,1000-PWM->LiftValue);
ftm_pwm_duty(ftm2,ftm_ch3,0);
} //如果發現此處給定的是控制右輪轉動則調換下硬件驅動線線序注意是兩根兩根的調換
if(PWM->RightValue>1000){ //右輪是前進 1000~2000
ftm_pwm_duty(ftm2,ftm_ch4,0);
ftm_pwm_duty(ftm2,ftm_ch5,PWM->RightValue-1000);
}
else if(PWM->RightValue<1000){ //右輪是后退 0~1000
ftm_pwm_duty(ftm2,ftm_ch4,1000-PWM->RightValue);
ftm_pwm_duty(ftm2,ftm_ch5,0);
} //如果發現此處給定的是控制左邊輪轉動則調換下硬件驅動線線序注意是兩根兩根的調換
};
uint8 timer3ms = 0, timer4_5ms=0,timer24ms=0 ,timer64ms=0;
int main(void){
//由于本程序閉環較多調試過程比較繁瑣建議一開始的時候屏蔽掉其他環
//只調節最基礎的環節平衡車主要就是對于兩個電機的控制其中有一個函數
//MotorControlFlow函數是直接控制電機的這個函數需要調試好其要求就是
//占空比0~1000退后 1000~2000向前通過調換驅動線和調換電機線或者
//調換程序中 ch2 3 4 5 的 23順序 45順序來達到最終的效果兩個電機
//給1000以上是向前走 1000一下向后走 2000往前最快 0往后最快
//當電機輸出調整好了以后再調節編碼器反饋我們在MotorControlFlow函數里
//直接給 PWM->LiftValue = 1200;PWM->RightValue = 1200; 讓兩個電機固定占空比
//轉動這時候通過IAR在線調試觀察編碼器測速值是否正確是否對應左輪右輪
//(左電機轉動對應左編碼器右電機轉動對應右邊編碼器不能錯！)
//用手阻止一個電機觀察是否是那個電機的編碼器轉速下降確保編碼器的方向一致！
//編碼器和電機調整好以后就方便了很多接下來就是直立前提是陀螺儀參數已經整定好
//我們調試直立的時候要把其他環都關閉方法如下函數PWMFeedbackControl 是把所有環輸出
//整合到電機PWM上的函數其中 PWM->PiontValue = (int16)(PID_Balance->out)+ (int16)(PID_Speed->out_k[0] + PID_Speed->out_k[9]) ;
//這句話是把三個環的輸出疊加在一起我們只需要保留直立環這樣調節直立比較順手
//其中還有一個差速也要給0 對直立就無影響
//當直立調節完畢后速度環和直立的融合是比較難的直立和速度是并環需要精心調試，也可以
//跳過速度環的調試直接進入轉向環與差速環他們之間是串級PID的關系
//如果出現中斷跑飛或者卡住在ADC死循環里說明堆棧溢出調整KEA128.ICF文件的堆棧值擴大不超過12K即可
get_clk(); //獲取時鐘頻率必須執行
uint8 Page=0;
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;
SensorStructurePointerInit( &Acc, &Gyro, &Angle, &Speed, &InductorA, &InductorB );//傳感器參數初始化
PIDStructurePointerInit( &PID_Balance, &PID_Speed, &PID_Path, &PID_Diff );//PID參數初始化
CarBalanceSystemInit( &Gyro ); //系統初始化
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;//差速數目
while(1){
if( timer4_5ms ){
gpio_set(H2,1);//
GetAccelerationValue( &Acc ); //加速度原始數據采集
GetAngleSpeedValue( &Gyro ); //角速度原始數據采集
GetSpeedValue( &Speed ); //獲取原始速度數據
GetInductorValue( &InductorA, &InductorB );//電感電壓原始數據采集
GyroHighPassFilter( &Gyro ); //角速度濾波處理
AngleHardwareFilter( &Angle );//角度濾波處理
SpeedSoftwareFilter( &Speed );//速度遞歸權值濾波
InductorSoftwareFilter( &InductorA, &InductorB, &Angle, &Switch);//電感濾波處理
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輸出控制
gpio_set(H2,0);//
timer4_5ms = 0;//清除時間標志位
}
//車子跑起來以后就不會執行因為開銷比較大影響PID控制周期
if( Speed.ActualSpeed[2]<100 && timer24ms && timer4_5ms != 1 ){
GetButtonStatus( &Button ); //獲取按鍵狀態
ButtonLowPassFilter( &Button );//判斷按鍵狀態
ButtonFunctionControl( &PID_Balance, &PID_Speed, &PID_Path, &PID_Diff, &Angle, &Speed, &InductorA, &InductorB, &Button, &Page );//實現按鍵狀態
GetSwitchStatus( &Switch ); //獲取開關狀態
SwitchLowPassFilter( &Switch );//判斷開關狀態
timer24ms = 0;
};
if(timer64ms && timer4_5ms != 1 ){
if(Speed.ActualSpeed[2]<100)PrintParameterToOLED( &PID_Balance, &PID_Speed, &PID_Path, &PID_Diff, &Angle, &Speed, &PWM, &InductorA, &InductorB, &Curve, Page );
//人機交互界面高速運行不執行
SpeedFeedbackControl( &PID_Speed, &Speed, &Ramp, &Switch ); //速度環反饋控制
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);//清除中斷標志位
}

復制代碼

所有資料51hei提供下載:

KEA128直立.zip (4.71 MB, 下載次數: 37)

ID:105146 · 發表于 2018-7-2 21:02

首先感謝分享！參考一下

ID:105146 · 發表于 2018-7-2 21:29

編譯出錯啊~提示缺少一些文件

帳號		自動登錄	找回密碼
密碼			立即注冊

久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

KEA128直立小車源碼