基于百科荣创主车电机驱动板程序 PID控制

姜片123

已于 2023-08-15 13:12:51 修改

阅读量1.2k

点赞数 21

文章标签：驱动开发硬件工程

于 2023-08-15 13:00:15 首次发布

本文链接：https://blog.csdn.net/u012936515/article/details/132295722

版权

适用百科荣创新款主车以及麦克纳姆轮的电机驱动板，老款带电子罗盘的实测不适用

代码以及逻辑上可能会存在一些问题，错误的地方在评论区指出

当时用的370麦轮电机，很难控制，后面指导老师让我们考虑加入pid，原本在主控板的程序里面，效果一般，后面才考虑用hal库重构电机驱动板，但是后面发现效果也没有提升多少，到后面的竞赛平台也就用了这个程序

在看原理图的时候其实发现了一些奇奇怪怪的地方，就比如说前轮可以用定时器捕获功能获取码盘，但是后轮定时器又跟定时器pwm有点冲突，用外部中断的话，有两个轮子的中断线又有冲突，然后电机的pwm又对应互补输出，电机驱动用了TIM8三路互补输出和TIM2两个输出，当时的我stm32也才刚入门，整不明白互补怎么去控制的电机，所以把互补的三路切换到了TIM3的CH1、CH2、CH3上。

码盘用的是TIM1,TIM3,TIM4和TIM5的编码器模式，原电机驱动板只采样后面两个轮子的码盘，这个时候TIM3就已经和输出PWM的TIM3冲突了，TIM3对应的是后面两个轮子的其中一个。想过用外部中断测码盘，但是前面两个电机码盘接的IO的外部中断线又冲突了。
最后是后面两个轮子用定时器TIM1和TIM4来测码盘，前面两个轮子用的是两个外部中断测码盘，又发现前后码盘有个四倍的差距，就直接在中断码盘计数那边改成了+=4和-=4……

带pid的电机驱动板程序其实并没有多麻烦，麻烦在于挖掘CAN的通信

CuneMX的配置

如图：

记得开启CAN中断接收

主要就是一个

CAN中断接收

注：因为用HAL库重构了核心板程序，这里加了一些原厂程序不会有的功能，可能会导致原厂核心板程序搭配这套程序出现问题，原厂应该只会用到0x1e0的接收

u16 Stop_Time=0;
uint8_t TXmessage[8] = {0x00,0x11,0x22,0x33,0x44,0x55,0x66,0x77};
uint8_t RXmessage[8];
uint32_t pTxMailbox = 0;
int Set_MP[4];
u8 Set_MP_Flag = 0;

void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)//接受邮箱0挂起中断回调函数
{
	if(hcan->Instance==CAN1)
	{
        RxHeader.StdId=0x20;
		HAL_CAN_GetRxMessage(hcan,CAN_FILTER_FIFO0,&RxHeader,RXmessage);//获取数据
        
        
        switch(RxHeader.StdId)
        {
            case 0x1e0:{
                
                motor_set_left_top = RXmessage[0]/4;    //左前轮设定值
                motor_set_right_top = RXmessage[2]/4;   //右前轮设定值
                motor_set_left_aft = RXmessage[1]/4;    //左后轮设定值
                motor_set_right_aft = RXmessage[3]/4;   //右后轮设定值

                if(motor_set_left_top>31)motor_set_left_top-=64;
                if(motor_set_right_top>31)motor_set_right_top-=64;
                if(motor_set_left_aft>31)motor_set_left_aft-=64;
                if(motor_set_right_aft>31)motor_set_right_aft-=64;
                
                memset(RXmessage,0,8);//清空数组
                
            }break;
            case 0x1e1:{  //让电机转动指定个码盘，已废弃
                
                Set_MP[0] = ((RXmessage[1]<<8)+RXmessage[0])/4;
                Set_MP[1] = ((RXmessage[3]<<8)+RXmessage[2])/4;
                Set_MP[2] = ((RXmessage[5]<<8)+RXmessage[4])/4;
                Set_MP[3] = ((RXmessage[7]<<8)+RXmessage[6])/4;
                
                if(Set_MP[0]>8191) Set_MP[0]-=16384;
                if(Set_MP[1]>8191) Set_MP[1]-=16384;
                if(Set_MP[2]>8191) Set_MP[2]-=16384;
                if(Set_MP[3]>8191) Set_MP[3]-=16384;
                
                printf("%d,%d,%d,%d\n\n",Set_MP[0],Set_MP[1],Set_MP[2],Set_MP[3]);
                
                Set_MP_Flag = 1;
                memset(RXmessage,0,8);//清空数组
                
            }break;
            case 0x1e2:{   //停车三秒
                
                if(RXmessage[0]==0x03)
                {
                
                    Stop_Time = RXmessage[1]*72;
                }
                memset(RXmessage,0,8);//清空数组
                
            }break;
        }
        
        
        

        
        
        //printf("0x%X 0x%X 0x%X 0x%X 0x%X 0x%X 0x%X 0x%X\n",RXmessage[0],RXmessage[1],RXmessage[2],RXmessage[3],RXmessage[4],RXmessage[5],RXmessage[6],RXmessage[7]);
    }
	
}

以及在网上找的一个

CAN的发送函数

//can发送一组数据(固定格式:ID为0X12,标准帧,数据帧)	
//len:数据长度(最大为8)				     
//msg:数据指针,最大为8个字节.
//stdid:目标id
//返回值:0,成功;
//		 其他,失败;

//disp地址：0x1e
u8 CAN1_Send_Msg(u8* msg,u8 len,u32 stdid,u32 d)
{	
    u8 i=0;
	uint32_t TxMailbox;
	u8 message[8];
    TxHeader.StdId=stdid;//0X12;        //标准标识符
    TxHeader.ExtId=0;//0x12;        //扩展标识符(29位)
    TxHeader.IDE=CAN_ID_STD;    //使用标准帧
    TxHeader.RTR=CAN_RTR_DATA;  //数据帧
    TxHeader.DLC=len;    
    for(i=0;i<len;i++)
    {
		message[i]=msg[i];
	}
    
    if(HAL_CAN_AddTxMessage(&hcan, &TxHeader, message, &TxMailbox) != HAL_OK)//发送
	{
		return 1;
	}
	while(HAL_CAN_GetTxMailboxesFreeLevel(&hcan) != 3) {}
    return 0;
}

列出一下

CAN ID对应的目标：

0x1e0：电机驱动板的速度

0x1e1：让电机转动指定个码盘 ****

0x1e2：最高权限让电机驱动板停车多少秒 ****

0x3c0：通信显示板Debug打印

0x1F1：通信显示板显示的码盘

0x0E7：主车程序的循迹板数据回传

0x2A0：ZigBee数据

0x280：WiFi数据

printf重定向

原厂程序的printf重定向：

int fputc(int ch,FILE *p)  //printf重定向到通信显示板的debug
{
    delay_ms(1);
    u8 tmp[1];
    sprintf((char*)tmp,"%c",ch);
    Send_Debug_Info((u8*)tmp,1);
    
    USART_SendData(USART1,(u8)ch);	
	while(USART_GetFlagStatus(USART1,USART_FLAG_TXE)==RESET);
    
	return ch;
}

hal库重构后的重定向

int fputc(int ch,FILE *f)
{
    
    CAN1_Send_Msg((uint8_t *)&ch,1,0x3c0,0);
    
	return ch;
}

下面是一部分的代码

设置pwm

//设置电机pwm，左前，左后，右前，右后
void set_motor(int lt,int la,int rt,int ra)
{
        
    if(rt>0)
    {
        __HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_3,rt);//右前轮正转
        __HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_4,0);//右前轮反转
    }else if(rt==0){
        __HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_3,1000);//右前轮正转
        __HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_4,1000);//右前轮反转
    }else{
        __HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_3,0);//右前轮正转
        __HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_4,-rt);//右前轮反转
    }
    
    if(ra>0)
    {
        __HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_1,ra);//右后轮正转
        __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_2,0);//右后轮反转
    }else if(ra==0){
        __HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_1,1000);//右后轮正转
        __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_2,1000);//右后轮反转
    }else{
        __HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_1,0);//右后轮正转
        __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_2,-ra);//右后轮反转
    }
    
    
    
    if(lt>0)
    {
        __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_4,lt);//左前轮正转
        __HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_3,0);//左前轮反转
    }else if(lt==0){
        __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_4,1000);//左前轮正转
        __HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_3,1000);//左前轮反转
    }else{
        __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_4,0);//左前轮正转
        __HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_3,-lt);//左前轮反转
    }
    
    if(la>0)
    {
        __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_3,la);//左后轮正转
        __HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_2,0);//左后轮反转
    }else if(la==0){
        __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_3,1000);//左后轮正转
        __HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_2,1000);//左后轮反转 
    }else{
        __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_3,0);//左后轮正转
        __HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_2,-la);//左后轮反转      
    }

}

转速 PID

//电机转速pid

float Kp = 1.2;
float Ki = 0.3;
float Kd = 2;


/*
函数功能：PID左前计算
参    数：test：当前车速，set：设置车速
返 回 值：无
*/
int pid_lt(int test,int set)
{
	static int Bias,Pwm,Old_Last_bias,Last_bias;

	Bias=set-test;             //计算偏差
	Pwm=Kp*(Bias-Last_bias)+Ki*Bias+Kd*(Bias-(2*Last_bias)+Old_Last_bias);  //增量式PI控制器
	Old_Last_bias=Last_bias;	                   //保存上上一次偏差
	Last_bias=Bias;	                   //保存上一次偏差
	
	return Pwm;                        //增量输出
}

/*
函数功能：PID左后计算
参    数：test：当前车速，set：设置车速
返 回 值：无
*/
int pid_la(int test,int set)
{
	static int Bias,Pwm,Old_Last_bias,Last_bias;

	Bias=set-test;             //计算偏差
	Pwm=Kp*(Bias-Last_bias)+Ki*Bias+Kd*(Bias-(2*Last_bias)+Old_Last_bias);  //增量式PI控制器
	Old_Last_bias=Last_bias;	                   //保存上上一次偏差
	Last_bias=Bias;	                   //保存上一次偏差
	
	return Pwm;                        //增量输出
}

/*
函数功能：PID右前计算
参    数：test：当前车速，set：设置车速
返 回 值：无
*/
int pid_rt(int test,int set)
{
	static int Bias,Pwm,Old_Last_bias,Last_bias;

	Bias=set-test;             //计算偏差
	Pwm=Kp*(Bias-Last_bias)+Ki*Bias+Kd*(Bias-(2*Last_bias)+Old_Last_bias);  //增量式PI控制器
	Old_Last_bias=Last_bias;	                   //保存上上一次偏差
	Last_bias=Bias;	                   //保存上一次偏差
	
	return Pwm;                        //增量输出
}

/*
函数功能：PID右后计算
参    数：test：当前车速，set：设置车速
返 回 值：无
*/
int pid_ra(int test,int set)
{
	static int Bias,Pwm,Old_Last_bias,Last_bias;

	Bias=set-test;             //计算偏差
	Pwm=Kp*(Bias-Last_bias)+Ki*Bias+Kd*(Bias-(2*Last_bias)+Old_Last_bias);  //增量式PI控制器
	Old_Last_bias=Last_bias;	                   //保存上上一次偏差
	Last_bias=Bias;	                   //保存上一次偏差
	
	return Pwm;                        //增量输出
}

位置PID

//码盘位置pid

float cmd_P = 3;
float cmd_I = 0.0005;
float cmd_D = 12;


/*
函数功能：PID左前计算
参    数：test：当前车速，set：设置车速
返 回 值：无
*/
int wz_pid_lt(int test,int set)
{
	float Kp=cmd_P,Ki=cmd_I,Kd=cmd_D;			//原例程中Kp=20，Ki=30	
	static int Bias,Pwm,Integral_bias,Last_bias;

	Bias=set-test;             //计算偏差
	Integral_bias+=Bias;
	Pwm=Kp*Bias+Ki*Integral_bias+Kd*(Bias-Last_bias);  //位置式PI控制器
	Last_bias=Bias;	                   //保存上一次偏差
	
	return Pwm;                        //增量输出
}

/*
函数功能：PID左后计算
参    数：test：当前车速，set：设置车速
返 回 值：无
*/
int wz_pid_la(int test,int set)
{
	float Kp=cmd_P,Ki=cmd_I,Kd=cmd_D;			//原例程中Kp=20，Ki=30	
	static int Bias,Pwm,Integral_bias,Last_bias;

	Bias=set-test;             //计算偏差
	Integral_bias+=Bias;
	Pwm=Kp*Bias+Ki*Integral_bias+Kd*(Bias-Last_bias);  //位置式PI控制器
	Last_bias=Bias;	                   //保存上一次偏差
	
	return Pwm;                        //增量输出
}

/*
函数功能：PID右前计算
参    数：test：当前车速，set：设置车速
返 回 值：无
*/
int wz_pid_rt(int test,int set)
{
	float Kp=cmd_P,Ki=cmd_I,Kd=cmd_D;			//原例程中Kp=20，Ki=30	
	static int Bias,Pwm,Integral_bias,Last_bias;

	Bias=set-test;             //计算偏差
	Integral_bias+=Bias;
	Pwm=Kp*Bias+Ki*Integral_bias+Kd*(Bias-Last_bias);  //位置式PI控制器
	Last_bias=Bias;	                   //保存上一次偏差
	
	return Pwm;                        //增量输出
}

/*
函数功能：PID右后计算
参    数：test：当前车速，set：设置车速
返 回 值：无
*/
int wz_pid_ra(int test,int set)
{
	float Kp=cmd_P,Ki=cmd_I,Kd=cmd_D;			//原例程中Kp=20，Ki=30	
	static int Bias,Pwm,Integral_bias,Last_bias;

	Bias=set-test;             //计算偏差
	Integral_bias+=Bias;
	Pwm=Kp*Bias+Ki*Integral_bias+Kd*(Bias-Last_bias);  //位置式PI控制器
	Last_bias=Bias;	                   //保存上一次偏差
	
	return Pwm;                        //增量输出
}

定时器中断

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) //外部中断回调函数
{
    if(GPIO_Pin == GPIO_PIN_0)
    {
        if(HAL_GPIO_ReadPin(CODE_D2_GPIO_Port,CODE_D2_Pin)==1)
        {
            code_desk_left_top+=4;
        }else
        {
            code_desk_left_top-=4;
        }
    }

    if(GPIO_Pin == GPIO_PIN_4)
    {
        if(HAL_GPIO_ReadPin(CODE_B2_GPIO_Port,CODE_B2_Pin)==1)
        {
            code_desk_right_top-=4;
        }else
        {
            code_desk_right_top+=4;
        }
    }


}


u16 mp_delay=0;
int code_disk_init[4];

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
  if(htim==(&htim6))//PID定时器6
  {
      static int tmp[4];
      int speed[4];

        code_desk_left_aft-=	(short)__HAL_TIM_GET_COUNTER(&htim4);	  //读取左后轮编码器数据
        code_desk_right_aft+=	(short)__HAL_TIM_GET_COUNTER(&htim1);	  //读取右后轮编码器数据
        __HAL_TIM_GET_COUNTER(&htim4) = 0;							  //计数器值重新置位
        __HAL_TIM_GET_COUNTER(&htim1) = 0;							  //计数器值重新置位

        //做减法计算速度
        speed[0]=tran_reduce(code_desk_left_top,tmp[0]);
        speed[1]=tran_reduce(code_desk_right_top,tmp[1]);
        speed[2]=tran_reduce(code_desk_left_aft,tmp[2]);
        speed[3]=tran_reduce(code_desk_right_aft,tmp[3]);

        //保存老的码盘数值
        tmp[0]=code_desk_left_top;
        tmp[1]=code_desk_right_top;
        tmp[2]=code_desk_left_aft;
        tmp[3]=code_desk_right_aft;
      

        if(Set_MP_Flag) //位置pid模式
        {
            if(mp_delay==0)
            {
                code_disk_init[0] = code_desk_left_top;
                code_disk_init[1] = code_desk_right_top;
                code_disk_init[2] = code_desk_left_aft;
                code_disk_init[3] = code_desk_right_aft;
            }
            
            if(++mp_delay>144)//执行时间
            {
                mp_delay=0;
                Set_MP_Flag=0;
            }
            
            if((abs(code_desk_left_top-(code_disk_init[0]+Set_MP[0]))<20)&&(abs(code_desk_right_top-(code_disk_init[1]+Set_MP[1]))<20)&&(abs(code_desk_left_aft-(code_disk_init[2]+Set_MP[2]))<20)&&(abs(code_desk_right_aft-(code_disk_init[3]+Set_MP[3]))<20))
            {
                mp_delay=0;
                Set_MP_Flag=0;
            }
                
            
            pwm[0]=wz_pid_lt(code_desk_left_top,code_disk_init[0]+Set_MP[0]);
            pwm[1]=wz_pid_rt(code_desk_right_top,code_disk_init[1]+Set_MP[2]);
            pwm[2]=wz_pid_la(code_desk_left_aft,code_disk_init[2]+Set_MP[1]);
            pwm[3]=wz_pid_ra(code_desk_right_aft,code_disk_init[3]+Set_MP[3]);
            
            u8 max_speed = 60;
            //不让pwm超太多
            if(pwm[0]>max_speed)pwm[0]=max_speed;
            if(pwm[1]>max_speed)pwm[1]=max_speed;
            if(pwm[2]>max_speed)pwm[2]=max_speed;
            if(pwm[3]>max_speed)pwm[3]=max_speed;
            if(pwm[0]<-max_speed)pwm[0]=-max_speed;
            if(pwm[1]<-max_speed)pwm[1]=-max_speed;
            if(pwm[2]<-max_speed)pwm[2]=-max_speed;
            if(pwm[3]<-max_speed)pwm[3]=-max_speed;
            
            //printf("%d,%d\n",code_desk_right_aft,code_disk_init[3]+Set_MP[3]);
            
        }else{
            //PID计算
            pwm[0]+=pid_lt(speed[0],motor_set_left_top);
            pwm[1]+=pid_rt(speed[1],motor_set_right_top);
            pwm[2]+=pid_la(speed[2],motor_set_left_aft);
            pwm[3]+=pid_ra(speed[3],motor_set_right_aft);
            
            //清零
            if(motor_set_left_top==0)pwm[0]=0;
            if(motor_set_right_top==0)pwm[1]=0;
            if(motor_set_left_aft==0)pwm[2]=0;
            if(motor_set_right_aft==0)pwm[3]=0;
            
            
            //不让pwm超太多
            if(pwm[0]>100)pwm[0]=100;
            if(pwm[1]>100)pwm[1]=100;
            if(pwm[2]>100)pwm[2]=100;
            if(pwm[3]>100)pwm[3]=100;
            if(pwm[0]<-100)pwm[0]=-100;
            if(pwm[1]<-100)pwm[1]=-100;
            if(pwm[2]<-100)pwm[2]=-100;
            if(pwm[3]<-100)pwm[3]=-100;
            
        }

        

        if(Stop_Time>0)
        {
            Stop_Time--;
            
            pwm[0] = 0;
            pwm[1] = 0;
            pwm[2] = 0;
            pwm[3] = 0;
        }


        //模式切换
        if(mode_flag==0) set_motor(pwm[0]*10,pwm[2]*10,pwm[1]*10,pwm[3]*10);
        if(mode_flag==1) set_motor(motor_set_left_top*10,motor_set_left_aft*10,motor_set_right_top*10,motor_set_right_aft*10);
         
//        printf("set:%3d,%3d,%3d,%3d",motor_set_left_top,motor_set_left_aft,motor_set_right_top,motor_set_right_aft);
//        printf("   pwm:%3d,%3d,%3d,%3d",pwm[0],pwm[1],pwm[2],pwm[3]);
//        printf("   speed %3d,%3d,%3d,%3d",speed[0],speed[1],speed[2],speed[3]);
//        printf("   mp:%3d,%3d,%3d,%3d",code_desk_left_top,code_desk_right_top,code_desk_left_aft,code_desk_right_aft);
//        printf("   vol:%d\n",HAL_ADC_GetValue(&hadc1));

       // printf("%d,%d,%d,%d\n\n",Set_MP[0],Set_MP[1],Set_MP[2],Set_MP[3]);
      
//        //发送给主车以及通信显示板的码盘    
//        u8 dat[]={0x02,((code_desk_left_top*4)&0xFF),(((code_desk_left_top*4) >>8)&0xFF),((code_desk_right_top*4)&0xFF),(((code_desk_right_top*4) >>8)&0xFF),0x00,(motor_volta)&0xFF,(motor_volta>>8)&0xFF};
//        u8 dat2[]={0x03,((code_desk_left_aft*4)&0xFF),(((code_desk_left_aft*4) >>8)&0xFF),((code_desk_right_aft*4)&0xFF),(((code_desk_right_aft*4) >>8)&0xFF),0x00,0x00,0x00};
//        CAN1_Send_Msg(dat,8,0x01E0,0);
//        CAN1_Send_Msg(dat2,8,0x01E1,0);
//        
//        u8 dat2[]={pwm[0],pwm[2],pwm[1],pwm[3],0x00,0x00,0x00,0x00};
//        CAN1_Send_Msg(dat2,8,0x01,0);

		
  }
}

主循环

int main(void)
{
  /* USER CODE BEGIN 1 */
    
  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_ADC1_Init();
  MX_CAN_Init();
  MX_TIM2_Init();
  MX_TIM8_Init();
  MX_USART2_UART_Init();
  MX_TIM3_Init();
  MX_TIM4_Init();
  MX_TIM1_Init();
  MX_TIM6_Init();
  /* USER CODE BEGIN 2 */
  
    HAL_TIM_Base_Start_IT(&htim6);
  

    HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_3);
    HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_4);
    
    HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_2);
    HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_3);
    HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_4);
    
    HAL_TIM_PWM_Start(&htim8,TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim8,TIM_CHANNEL_2);
    HAL_TIM_PWM_Start(&htim8,TIM_CHANNEL_3);

    //电机驱动芯片的睡眠关闭
    HAL_GPIO_WritePin(SLEEP_AB_GPIO_Port,SLEEP_AB_Pin,GPIO_PIN_SET);
    HAL_GPIO_WritePin(SLEEP_CD_GPIO_Port,SLEEP_CD_Pin,GPIO_PIN_SET);


    HAL_TIM_Encoder_Start(&htim4,TIM_CHANNEL_1 | TIM_CHANNEL_2);
    HAL_TIM_Encoder_Start(&htim1,TIM_CHANNEL_1 | TIM_CHANNEL_2);



    HAL_Delay(2000);
    
    
    
    CAN1_Send_Msg("PID mode",8,0x3c0,0);   //上电默认pid模式
    CAN1_Send_Msg("\n",1,0x3c0,0);   //上电默认pid模式
    //printf("PID Mode\n");
    
    HAL_ADCEx_Calibration_Start(&hadc1);    //AD校准
    HAL_ADC_Start(&hadc1);
    
    
      // 读取RCC_CSR寄存器的值
      uint32_t resetFlags = RCC->CSR;
      printf("motor:");
      // 判断重启原因
      if ((resetFlags & RCC_CSR_SFTRSTF) != 0) {
        // 软件复位
        printf("Software Reset\n");
      } else if ((resetFlags & RCC_CSR_PORRSTF) != 0) {
        // 上电复位
        printf("Power-On Reset\n");
      } else if ((resetFlags & RCC_CSR_PINRSTF) != 0) {
        // 外部引脚复位
        printf("Pin Reset\n");
      } else if ((resetFlags & RCC_CSR_IWDGRSTF) != 0) {
        // 独立看门狗复位
        printf("Independent Watchdog Reset\n");
      } else if ((resetFlags & RCC_CSR_WWDGRSTF) != 0) {
        // 窗口看门狗复位
        printf("Window Watchdog Reset\n");
      } else if ((resetFlags & RCC_CSR_LPWRRSTF) != 0) {
        // 低功耗复位
        printf("Low-Power Reset\n");
      } else if ((resetFlags & RCC_CSR_RMVF) != 0) {
        // 电源复位
        printf("Brown-Out Reset\n");
      }
      
      // 清除复位标志
      RCC->CSR |= RCC_CSR_RMVF;

   
    
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
      if(key_scan()==1)
      {
          mode_flag++;
          switch(mode_flag)
          {
              case 0:CAN1_Send_Msg("PID mode\n",8,0x3c0,0);break;//发送到debug
              case 1:CAN1_Send_Msg("Common\n",8,0x3c0,0);break;
              case 2:CAN1_Send_Msg("Debug\n",6,0x3c0,0);set_motor(300,0,0,0);break;
              case 3:set_motor(0,300,0,0);break;
              case 4:set_motor(0,0,300,0);break;
              case 5:set_motor(0,0,0,300);break;
              case 6:set_motor(300,300,300,300);break;
              case 7:set_motor(0,0,0,0);break;             
              case 8:set_motor(-300,0,0,0);break;
              case 9:set_motor(0,-300,0,0);break;
              case 10:set_motor(0,0,-300,0);break;
              case 11:set_motor(0,0,0,-300);break;
              case 12:set_motor(-300,-300,-300,-300);break;
              case 13:set_motor(0,0,0,0);mode_flag=-1;break;
              
          }
         
      }


      HAL_GPIO_TogglePin(LED1_GPIO_Port,LED1_Pin);
      
      
        //发送给主车以及通信显示板的码盘    
        u8 dat[]={0x02,((code_desk_left_top)&0xFF),(((code_desk_left_top) >>8)&0xFF),((code_desk_right_top)&0xFF),(((code_desk_right_top) >>8)&0xFF),0x00,(motor_volta)&0xFF,(motor_volta>>8)&0xFF};
        u8 dat2[]={0x03,((code_desk_left_aft)&0xFF),(((code_desk_left_aft) >>8)&0xFF),((code_desk_right_aft)&0xFF),(((code_desk_right_aft) >>8)&0xFF),0x00,0x00,0x00};
        
        
        CAN1_Send_Msg(dat,8,0x01E0,0);
        
      HAL_Delay(15);
        
        CAN1_Send_Msg(dat2,8,0x01F1,0);
            
            motor_volta = HAL_ADC_GetValue(&hadc1);

      
      
      
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */
}

国赛完成，后面有机会开源hal库的主车程序，以及抛弃arduino后的从车stm32程序和视频循迹