机器人武术擂台---无差别组(七)台上台下检测及上台算法
做一份笔记,大佬勿喷。
作者:sumjess
注意:本博客是以《2018年华北五省(市、自治区)大学生机器人大赛竞赛规则》为基础而写的
@@@@@@@@@一共写2个版本的底层配置-----STM32、K60@@@@@@@@
特别提醒:K60是基于山外库书写!
@@@@@@@@@STM32的底层配置@@@@@@@@
一、上台机构电机PWM底层配置:
void speed_Init(void) //TIM3_CH1->PC6 CH2->PC7 CH3->PC8 CH4->PC9
{
//**********电机驱动设置************//
TIM_OCInitTypeDef TIM_OCInitStructure;
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure ;
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC,ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); //使能GPIO外设和AFIO复用功能模块时钟
TIM_DeInit(TIM3);//初始化TIM3寄存器
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); //使能GPIO外设和AFIO复用功能模块时钟
TIM_DeInit(TIM2);//初始化TIM3寄存器
GPIO_PinRemapConfig(GPIO_FullRemap_TIM3, ENABLE); //Timer3完全重映射 TIM3_CH1->PC6 CH2->PC7 CH3->PC8 CH4->PC9
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7 | GPIO_Pin_8 | GPIO_Pin_9; //TIM1_CH1 //设置该引脚为复用输出功能,输出TIM3 CH1的PWM脉冲波形 GPIOC.6
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; //复用推挽输出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOC, &GPIO_InitStructure);//初始化GPIO
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2 | GPIO_Pin_3;//TIM1_CH1 //设置该引脚为复用输出功能,输出TIM3 CH1的PWM脉冲波形 GPIOC.6
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; //复用推挽输出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);//初始化GPIO
//**************************************************************************
// 定时器1设置: 72分频,频率10KHz,向上计数,105是为了速度为100时,PWM占空比不会到100,方便电机驱动自举生压。一般PWM频率大于1K,4-10K比较好。
//**************************************************************************
TIM_TimeBaseStructure.TIM_Period = 105-1;
TIM_TimeBaseStructure.TIM_Prescaler = 71;
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
// 主定时器T3为 PWM3 模式
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_Pulse = 0;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
TIM_OC1Init(TIM3, &TIM_OCInitStructure);
TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable);//使能的预装载寄存器
TIM_OCInitStructure.TIM_Pulse = 0;
TIM_OC2Init(TIM3, &TIM_OCInitStructure);
TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable);//使能的预装载寄存器
TIM_OCInitStructure.TIM_Pulse = 0;
TIM_OC3Init(TIM3, &TIM_OCInitStructure);
TIM_OC3PreloadConfig(TIM3, TIM_OCPreload_Enable);//使能的预装载寄存器
TIM_OCInitStructure.TIM_Pulse = 0;
TIM_OC4Init(TIM3, &TIM_OCInitStructure);
TIM_OC4PreloadConfig(TIM3, TIM_OCPreload_Enable);//使能的预装载寄存器
TIM_OCInitStructure.TIM_Pulse = 0;
TIM_OC3Init(TIM2, &TIM_OCInitStructure);
TIM_OC3PreloadConfig(TIM2, TIM_OCPreload_Enable);//使能的预装载寄存器
TIM_OCInitStructure.TIM_Pulse = 0;
TIM_OC4Init(TIM2, &TIM_OCInitStructure);
TIM_OC4PreloadConfig(TIM2, TIM_OCPreload_Enable);//使能的预装载寄存器
TIM_ARRPreloadConfig(TIM3, ENABLE); //使能定时器3
TIM_Cmd(TIM3, ENABLE);
TIM_ARRPreloadConfig(TIM2, ENABLE); //使能定时器3
TIM_Cmd(TIM2, ENABLE);
}
二、上台机构电机算法:
void gotopk(void)
{
TIM_SetCompare1(TIM3,5); //需要根据现实进行更改
TIM_SetCompare2(TIM3,70); //需要根据现实进行更改
TIM_SetCompare4(TIM3,70); //需要根据现实进行更改
TIM_SetCompare3(TIM3,5); //需要根据现实进行更改
delay_ms(2000); //需要根据现实进行更改
TIM_SetCompare4(TIM2,5); //需要根据现实进行更改
TIM_SetCompare3(TIM2,90); //需要根据现实进行更改
delay_ms(3000); //需要根据现实进行更改
}
void gotopb(void)
{
if((gdhou1==1||gdhou2==1)&&(gdqian1==1||gdqian2==1))
gotopk();
}
三、掉落后再次上台程序:
while(1)
{
if(flag==1) //上台标志位
{ gotopk();flag=0;} //上台
if((for_1==0||for_2==0||for_3==0)&&(back_1==0||back_2==0)) //前后都有障碍,掉下擂台
if(down_1==0||down_2==0||down_3==0) { drive_Back_H(); delay_ms(2000); gotopk();} //后面对着外墙,后退,然后开始冲上去
else { drive_around(); drive_Back_H(); delay_ms(2000); gotopk();} //前面对着墙,掉头,后面对着外墙,后退,然后开始冲上去
else algor_SUM_UP(); //未掉下擂台
}