初级会员

- 积分
- 107
- 金钱
- 107
- 注册时间
- 2013-8-3
- 在线时间
- 0 小时
|

楼主 |
发表于 2014-5-14 00:03:00
|
显示全部楼层
#include "motor.h"//电机
// #include "encoder.h"
#include "oled.h"
int Trans =0;
extern u16 engineLoc[2],engineLocDst[2];
extern s16 engineLocErr[2][3];
extern s32 enginePidOp[2],engingSpd[2];
double engineKp=0,engineKi=85,engineKd=0;//12 0.72 0.08
void EngineTim4Init(u16 arr,u16 psc)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef TIM_OCInitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4| RCC_APB2Periph_AFIO, ENABLE); //时钟使能
TIM_TimeBaseStructure.TIM_Period = arr-1; //设置在下一个更新事件装入活动的自动重装载寄存器周期的值 计数到5000为500ms
TIM_TimeBaseStructure.TIM_Prescaler =psc-1; //设置用来作为TIMx时钟频率除数的预分频值 10Khz的计数频率
TIM_TimeBaseStructure.TIM_ClockDivision = 0; //设置时钟分割:TDTS = Tck_tim
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上计数模式
TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure); //根据TIM_TimeBaseInitStruct中指定的参数初始化TIMx的时间基数单位
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2; //选择定时器模式:TIM脉冲宽度调制模式2
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low; //输出极性:TIM输出比较极性高
TIM_OC2Init(TIM4, &TIM_OCInitStructure);//根据T指定的参数初始化外设TIM3 OC2
TIM_OC1Init(TIM4, &TIM_OCInitStructure);//根据T指定的参数初始化外设TIM3 OC2
TIM_OC2PreloadConfig(TIM4, TIM_OCPreload_Enable); //使能TIM4在CCR2上的预装载寄存器
TIM_OC1PreloadConfig(TIM4, TIM_OCPreload_Enable);
TIM_Cmd(TIM4, ENABLE); //使能TIM4
}
void EngineIoInit(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB|RCC_APB2Periph_GPIOA,ENABLE);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8|GPIO_Pin_9;//电机1方向
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7;//PWM两路输出 注意需要设置复用输出类型
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_Init(GPIOB, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1;//电机2方向
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_Init(GPIOA, &GPIO_InitStructure);
}
void Engine1SpeedSet(s32 velo1)
{
if(velo1>0)
{
ENGINE1_DIR1=1;
ENGINE1_DIR2=0;
if(velo1<ENGINE_MAX_SPD) TIM_SetCompare2(TIM4,velo1);
else TIM_SetCompare2(TIM4,ENGINE_MAX_SPD);
}
else if(velo1><0)
{
ENGINE1_DIR1=0;
ENGINE1_DIR2=1;
if(velo1>(-ENGINE_MAX_SPD)) TIM_SetCompare2(TIM4,(Trans-velo1));
else TIM_SetCompare2(TIM4,ENGINE_MAX_SPD);
}
else
{
ENGINE1_DIR1=0;
ENGINE1_DIR2=0;
TIM_SetCompare2(TIM4,0);
}
}
void Engine2SpeedSet(s32 velo2)
{
if(velo2>0)
{
ENGINE2_DIR1=1;
ENGINE2_DIR2=0;
if(velo2<ENGINE_MAX_SPD) TIM_SetCompare1(TIM4,velo2);
else TIM_SetCompare1(TIM4,ENGINE_MAX_SPD);
}
else if(velo2><0)
{
ENGINE2_DIR1=0;
ENGINE2_DIR2=1;
if(velo2>(-ENGINE_MAX_SPD)) TIM_SetCompare1(TIM4,(Trans-velo2));
else TIM_SetCompare1(TIM4,ENGINE_MAX_SPD);
}
else
{
ENGINE2_DIR1=0;
ENGINE2_DIR2=0;
TIM_SetCompare1(TIM4,0);
}
}
void Engine1PositionPID()
{
engineLocErr[0][2]=engineLocErr[0][1];
engineLocErr[0][1]=engineLocErr[0][0];
engineLocErr[0][0]=engineLocDst[0]-engineLoc[0];
if(abs(engineLocErr[0][0])>80)
enginePidOp[0]=engineKp*(engineLocErr[0][0]-engineLocErr[0][1])+engineKi*engineLocErr[0][0]+engineKd*(engineLocErr[0][0]-engineLocErr[0][1]*2+engineLocErr[0][2]);
else enginePidOp[0]=0;
Engine1SpeedSet(enginePidOp[0]);//注意
}
void Engine2PositionPID()
{
engineLocErr[1][2]=engineLocErr[1][1];
engineLocErr[1][1]=engineLocErr[1][0];
engineLocErr[1][0]=engineLocDst[1]-engineLoc[1];
if(abs(engineLocErr[1][0])>80)
enginePidOp[1]=engineKp*(engineLocErr[1][0]-engineLocErr[1][1])+engineKi*engineLocErr[1][0]+engineKd*(engineLocErr[1][0]-engineLocErr[1][1]*2+engineLocErr[1][2]);
else enginePidOp[1]=0;
Engine2SpeedSet(enginePidOp[1]);
} |
|