机器人武术擂台---无差别组（七）台上台下检测及上台算法

做一份笔记，大佬勿喷。

作者：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();    //未掉下擂台
	}