自平衡小车程序Word文档格式.docx
- 文档编号:19091747
- 上传时间:2023-01-03
- 格式:DOCX
- 页数:23
- 大小:21.22KB
自平衡小车程序Word文档格式.docx
《自平衡小车程序Word文档格式.docx》由会员分享,可在线阅读,更多相关《自平衡小车程序Word文档格式.docx(23页珍藏版)》请在冰豆网上搜索。
staticfloatbatt_voltage1,ay,ax,in_steer_knob;
staticfloatpitch_rate;
staticfloatinterval;
staticfloattimer0_seconds_conv;
staticfloatp_gain;
staticfloatd_gain;
staticfloatp1_gain;
staticfloatd1_gain;
staticfloatp0_gain;
staticfloatd0_gain;
staticfloattp0,td0,tp1,td1,tp,td;
staticfloatd_step=0.0001;
staticfloatp_step;
staticfloatcor_bat;
staticfloatp_cmd,d_cmd;
//next_cmd;
staticfloatsample_conv=1.0/1024.0/(float)HIST*5.0;
//=3.052exp(-4)
staticfloatsample_conv1=1.0/1024.0/(float)HIST*2.07;
staticfloatsample_conv2=1.0/1024.0/(float)HIST*1.9;
staticfloatsample_conv3=1.0/1024.0/(float)HIST*333.3*2.5;
interruptvoidT2_AD_isr(void);
voidInitEv(void);
voidInitAdc(void);
voidInitGpio(void);
structgyro_filter{
floatangle;
floatay_bias;
//floatax_bias;
floatrate_bias;
floatsteer_knob_bias;
floatrate;
floatsteer_knob;
floatcurr_bias;
floatcurr;
Uint16inited;
};
voidcheck_mode()
{if((!
pitch_rate)&
&
(!
in_steer_knob))mode=0x00;
elsemode=0x01;
}
voiddelay(void)
{Uint16xt;
staticUint16k_temp;
for(xt=0;
xt<
15;
xt++){
k_temp++;
}
voiddelay1(void)
15000;
intadc_collect_samples(Uint16*samples,Uint16*lasts0)
{
unsignedinti,j;
if(*lasts0==totsamples_0){
return0;
//ifnonewadcsamples,returnwith0
*lasts0=totsamples_0;
for(i=0;
i<
6;
i++){
Uint16tot=0;
for(j=0;
j<
HIST;
j++)
tot+=adc_current_samples[i][j];
samples[i]=tot;
//samples[i]=Σadc_current_samples[i][j](j=1,2,...,ADC_HIST)
return1;
Uint16bat_judge(floatx)
{
if(x>
4.30)return0x5;
//25.8
elseif(x>
4.18)return0x4;
//25.1
4.07)return0x3;
//24.4
3.95)return0x2;
//23.7
3.83)return0x1;
//23.0
elsereturn0x0;
voidlpf_update(float*state,floattc,floatinterval,floatinput)
floatfrac=interval/tc;
if(frac>
1.0)frac=1.0;
*state=input*frac+*state*(1.0-frac);
floatfmax(floata,floatb)
if(a>
=b){
returna;
}else{
returnb;
floatfmin(floata,floatb)
if(a<
floatflim(floatx,floatlo,floathi)
if(x>
hi)returnhi;
if(x<
lo)returnlo;
returnx;
voidgyro_init(structgyro_filter*it)
{it->
inited=0x00;
it->
angle=0.0;
ay_bias=0.0;
//it->
ax_bias=0.0;
rate_bias=0.0;
steer_knob_bias=0.0;
rate=0.0;
steer_knob=0.0;
curr_bias=0.0;
curr=0.0;
lpf_angle=0.0;
lpf_angrate=0.0;
lpf_steer_knob=0.0;
voidgyro_sample(structgyro_filter*it,floatin_rate,
floatin_y,floatin_x,floatin_steer,floatcurri,floatinterval)
flim(in_y,-1.0,1.0);
flim(in_x,-1.0,1.0);
if(!
it->
inited){
rate_bias=in_rate;
ay_bias=in_y;
curr_bias=curri;
steer_knob_bias=in_steer;
inited=1;
return;
rate=in_rate-it->
rate_bias;
angle=in_y-it->
ay_bias;
//it->
angle+(in_y-it->
ay_bias)*last_ax-in_x*(last_ay-it->
ay_bias);
curr=curri-it->
curr_bias;
steer_knob=flim(in_steer-it->
steer_knob_bias,-5.0,5.0);
voidset_motor_idle()
GpioDataRegs.GPFDAT.bit.GPIOF12=1;
//SDhigh
GpioDataRegs.GPFDAT.bit.GPIOF13=1;
//OEhigh
stand=0x00;
EvaRegs.T2CON.all=0x1440;
gyro_init(&
pitch_filter);
voidset_motor(floatlevel_left,floatlevel_right)
intleveli_left,leveli_right;
if(fabs(level_left)<
0.002||fabs(level_right)<
0.002)return;
leveli_left=(int16)(level_left*OCRMAX);
leveli_right=(int16)(level_right*OCRMAX);
if(leveli_left<
-(OCRMAX-2))leveli_left=-(OCRMAX-2);
if(leveli_left>
OCRMAX-2)leveli_left=OCRMAX-2;
if(leveli_right<
-(OCRMAX-2))leveli_right=-(OCRMAX-2);
if(leveli_right>
OCRMAX-2)leveli_right=OCRMAX-2;
if(still){
GpioDataRegs.GPFDAT.bit.GPIOF12=0;
//SD=0
//asm("
nop"
);
while(GpioDataRegs.GPFDAT.bit.GPIOF12==1)continue;
GpioDataRegs.GPADAT.bit.GPIOA13=1;
//PORTC|=0x50;
//enableCCW0
GpioDataRegs.GPBDAT.bit.GPIOB13=1;
//delay();
GpioDataRegs.GPADAT.bit.GPIOA14=1;
//PORTC|=0xa0;
//disableCW0
GpioDataRegs.GPBDAT.bit.GPIOB14=1;
//asm("
continue;
still=0x00;
0){
EvaRegs.ACTRA.all=0x00C2;
pwm=-leveli_left;
else{
EvaRegs.ACTRA.all=0x002C;
pwm=leveli_left;
EvbRegs.ACTRB.all=0x002C;
pwm=-leveli_right;
EvbRegs.ACTRB.all=0x00C2;
pwm=leveli_right;
voidmotor_ready(void)
//SDlow
GpioDataRegs.GPFDAT.bit.GPIOF13=0;
//OElow
EvaRegs.T1CON.bit.TENABLE=1;
//启动定时器T1
EvbRegs.T3CON.bit.TENABLE=1;
//启动定时器T3
//16分频,使能定时器操作,连续增模式
voidbalance(void)
{floatsteer_cmd_abs,temp_steer_cmd;
unsignedintT2cnt;
adc_collect_samples(samples,&
last_balance_s0))
return;
{
T2cnt=EvaRegs.T2CNT;
//读取定时器2的值
ticks=OCRMAX*count+T2cnt;
interval=timer0_seconds_conv*(float)ticks;
count=0;
EvaRegs.T2CON.all=0x1400;
//关闭定时器2
EvaRegs.T2CNT=0x0000;
EvaRegs.T2CON.all=0x1440;
//启动定时器2
EvaRegs.GPTCONA.bit.T1TOADC=2;
ay=sample_conv1*samples[4]-sample_conv2*samples[0];
//1.034
pitch_rate=samples[1]*sample_conv3;
in_steer_knob=(samples[2]+samples[3])*sample_conv;
bus_current=samples[4]*sample_conv;
batt_voltage1=samples[5]*sample_conv;
check_mode();
if(!
pitch_filter.inited)
if(batt_voltage1<
3.0){
set_motor_idle();
else
cor_bat=-0.857*batt_voltage1+4.58;
//-0.857=(1-1.3)/(4.18-3.83),4.58=1+0.857*4.18
cor_bat=flim(cor_bat,1.0,1.3);
gyro_sample(&
pitch_filter,pitch_rate,ay,ax,in_steer_knob,bus_current,interval);
EvaRegs.GPTCONA.bit.T1TOADC=0;
if(!
mode){
if(fabs(pitch_filter.angle)<
initial_angle){
GpioDataRegs.GPADAT.bit.GPIOA14=0;
GpioDataRegs.GPBDAT.bit.GPIOB14=0;
GpioDataRegs.GPADAT.bit.GPIOA13=0;
GpioDataRegs.GPBDAT.bit.GPIOB13=0;
still=0x01;
}
lpf_steer_knob=-pitch_filter.steer_knob;
if(stand){
p_gain=fmax(p0_gain,p_gain-p_step);
//0.1
d_gain=fmin(d0_gain,d_gain+d_step);
tp=tp0;
td=td0;
else{p_gain=fmin(p1_gain,p_gain+p_step);
d_gain=fmax(d1_gain,d_gain-d_step);
tp=tp1;
td=td1;
}
lpf_update(&
lpf_angle,tp,interval,-pitch_filter.angle*cor_bat);
//0.6
lpf_angrate,td,interval,-pitch_filter.rate*cor_bat);
//0.5
p_cmd=p_gain*lpf_angle*fabs(lpf_angle);
d_cmd=d_gain*lpf_angrate;
cmd=p_cmd+d_cmd;
cmd=flim(cmd,-hard_speed_lim,hard_speed_lim);
if(lpf_steer_knob>
0.0)steer_cmd=flim(left_steer_cof*lpf_steer_knob*(1-fabs(cmd)),
-steer_lim,steer_lim);
elsesteer_cmd=flim(right_steer_cof*lpf_steer_knob*(1-fabs(cmd)),
steer_cmd_abs=fabs(steer_cmd);
if((lpf_steer_knob>
0.0&
cmd>
0.01)||(lpf_steer_knob<
cmd<
-0.01))
temp_steer_cmd=flim(steer_cmd+0.1*interval,-steer_cmd_abs,steer_cmd_abs);
//?
?
elseif((lpf_steer_knob>
-0.01)||(lpf_steer_knob<
0.01))
temp_steer_cmd=flim(steer_cmd-0.1*interval,-steer_cmd_abs,steer_cmd_abs);
elsetemp_steer_cmd=0.0;
if(cmd>
=0.0){
left_motor_pwm=1.0-cmd*left_cof-temp_steer_cmd;
right_motor_pwm=1.0-cmd*right_cof+temp_steer_cmd;
left_motor_pwm=-(1.0+cmd*left_cof)+temp_steer_cmd;
right_motor_pwm=-(1.0+cmd*right_cof)-temp_steer_cmd;
set_motor(left_motor_pwm,right_motor_pwm);
voidstabilize_analog(void)
Uint16samples[6];
Uint16i;
Uint16last_s0=0;
){
if(adc_collect_samples(samples,&
last_s0))i++;
}
voidstand_test()
if(GpioDataRegs.GPBDAT.bit.GPIOB6&
GpioDataRegs.GPBDAT.bit.GPIOB7)
stand=0x02;
else{
if(GpioDataRegs.GPBDAT.bit.GPIOB6||GpioDataRegs.GPBDAT.bit.GPIOB7)
stand=0x01;
else{
stand=0x00;
voidmain(void)
InitSysCtrl();
EALLOW;
DINT;
IER=0x0000;
//禁止VPU中断并清除所有CPU中断标志位
IFR=0x0000;
InitPieCtrl();
InitPieVectTable();
InitGpio();
InitAdc();
InitEv();
//ThisisneededtowritetoEALLOWprotectedregisters
PieVectTable.T2PINT=&
T2_AD_isr;
EDIS;
//ThisisneededtodisablewritetoEALLOWprotectedregisters
PieCtrlRegs.PIEIER3.bit.INTx1=1;
//T2pint中断
IER|=M_INT1;
//开启中断1
PieCtrlRegs.PIEIER1.bit.INTx7=1;
E
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 平衡 小车 程序