哨兵电控代码解析（配套源码（非官方））

发热的32

已于 2022-08-29 10:20:55 修改

阅读量1.1k

点赞数 7

分类专栏：编程哨兵文章标签： c++ stm32 人工智能视觉检测

于 2022-08-29 10:19:46 首次发布

本文链接：https://blog.csdn.net/weixin_57483690/article/details/126575396

版权

编程同时被 2 个专栏收录

1 篇文章 0 订阅

订阅专栏

哨兵

1 篇文章 0 订阅

订阅专栏

首先此文章当是自我记录、总结，也供各位参考，本人学习编程时间不长，若此文章出现错误，欢迎指证。

一、首先从开始任务开始着手

这里面有陀螺仪姿态解算、底盘、云台、裁判系统以及任务系统时间统计共5个任务，本代码只实现哨兵的基本运动主要是底盘以及云台任务。

二、地盘任务，底盘比较简易，只需控制两个3508即可，先从底盘开始。

#include "chassis_task.h"
#include  "gimbal_task.h"
#include "FreeRTOS.h"
#include "pid.h"
#include "task.h"
#include "communicate.h"
#include "detect_task.h"
#include "arm_math.h"

Chassis_Data Chassis_data;

/************* ↓ 初始化底盘 ↓ *************************/
static void Chassis_Init(Chassis_Data * chassis_data)    
{
	
	PID_data pidinit;
	chassis_data = &Chassis_data;                
	chassis_data->RB = GetM3508DataPointer() + 2;
	chassis_data->LB = GetM3508DataPointer() + 3;
	CAN2_Send_Message(45);                         
	
	pidinit.pid.Kp=M3508_PID_p;                    //PID初始化，M3508_PID_p 2.5
	pidinit.pid.Ki=M3508_PID_i;                    //            0.05
	pidinit.pid.Kd=M3508_PID_d;                    //            0.5
	pidinit.Limiting=M3508_Limiting;               //电机输出限幅
	pidinit.Reduction_ratio=M3508_Reduction_ratio;//             0
	
	
	PID_Init2(&(PID_Datas.LB),&pidinit);
	PID_Init2(&(PID_Datas.RB),&pidinit);
	chassis_data->max_speed=MAX_SPEED;           //哨兵移动速度最大值 2500
	chassis_data->mid_speed=MID_SPEED;           //1000
	
	chassis_data->chassis_mode=chassis_stop;     //初始化模式为停止模式
	chassis_data->loop_time=set_loop_time;       //循环时间 5000
	chassis_data->loop_time_counter=0.5*set_loop_time;
	chassis_data->loop_speed=set_loop_speed;     //循环速度 100
	
	
}


/*地盘控制任务*/
void chassis_task(void *pvParameters)
{
	//空闲一段时间
    vTaskDelay(CHASSIS_TASK_INIT_TIME);
	RC_TypeDef *RC_typeDef;                
	RC_typeDef = GetRemoteControlPointer();  //遥控器            
	Chassis_Init(&Chassis_data);   //底盘初始化 
	while(1)
	{
			Chassis_Mode(&Chassis_data);
		setM3508Speed(0,              //can通信发送四个速度数据
							0,
							Chassis_data.set_M3508_value[2],
							Chassis_data.set_M3508_value[3]);
		vTaskDelay(CHASSIS_TASK_TIME); 
		
	}
}

/*模式设置*/
static void Chassis_Mode(Chassis_Data * chassis_data)
{
	switch(chassis_data->chassis_mode){
		case chassis_stop:                  //0
			Chassis_stop_mode(chassis_data);
			break;
		case chassis_loop:                  //1 
			Chassis_Loop_mode(chassis_data);
			break;
	}
}

void Chassis_stop_mode(Chassis_Data * chassis_data){    //停止模式，两速度为0（哨兵移动只用到两个3508）
	
			chassis_data->set_M3508_value[2]=0;
			chassis_data->set_M3508_value[3]=0;
}



/*************↓ 地盘运动逻辑 ↓****************/
/**运动周期loop_time，counter用于计数，在计数到
loop_time时转换方向（dir）********************/
void Chassis_Loop_mode(Chassis_Data * chassis_data)
	{
	if(chassis_data->loop_time_counter<chassis_data->loop_time)
		{
		if(chassis_data->loop_dir==0)                                //方向0
			{chassis_data->set_M3508_value[2]=chassis_data->loop_speed;
			chassis_data->set_M3508_value[3]=chassis_data->loop_speed;
		}else
			{                                                       //方向1   0的反方向
			chassis_data->set_M3508_value[2]=-chassis_data->loop_speed;
			chassis_data->set_M3508_value[3]=-chassis_data->loop_speed;
		}
		chassis_data->loop_time_counter++;
	 }
	else //方向转换,重置计数
		{ 
		if(chassis_data->loop_dir==1)
			{   
			chassis_data->loop_dir=0;
		}else{
			chassis_data->loop_dir=1;
		}
		chassis_data->loop_time_counter=0;
	  }
}
	


u8 setM3508Speed(int speed1,int speed2,int speed3,int speed4)
 {
	 
	 CAN_Send_Msg(  //四个位置，相应位置对应相应ID发送的数据    can通信参考官方电调使用说明中的协议，此文章中下面已上传 
		CAN2,
		0,                      
		0,
		PIDApplyToM3508Speed(RB_ID,speed3),   //输出LF_ID经过PID计算的输出值
		PIDApplyToM3508Speed(LB_ID,speed4),
		0X200
		);

}


int PIDApplyToM3508Speed(u16 ID,int Speed)                    //返回四个轮子输出值
{
	PID_DATAS *pid;
	pid=&PID_Datas;
	Chassis_Data *data;
	data=&Chassis_data;
	Global_Data *global_data;
	global_data = &Global_data;
	switch(ID)
	{
		case LB_ID:
			return (short)PID_CTRL(Speed,data->LB->rpm_speed,&(pid->LB));//PID计算式，PID在此计算，返回输出值
		case RB_ID:
			return (short)PID_CTRL(Speed,data->RB->rpm_speed,&(pid->RB));
	}
	return 0;
}

底盘任务需仔细看官方的c6020电调can通信协议来正确通信，下面直接附两张通信协议图片就可不用去找了

三、云台任务

云台任务比较复杂，PITCH和YWA轴（两个6020），需要控制射击（两个3508）摩擦轮，一个拨盘用于拨弹（2006电机），视觉传回信息处理等等。下面逐一展开

/*云台任务*/
void gimbal_task(void *pvParameters)
{
	int16_t shoot_out = 0;
    vTaskDelay(GIMBAL_TASK_INIT_TIME);
		Gimbal_Init(&Global_data);
	while(1)
	{		
		Gimbal_Mode(&Global_data);          //云台模式
		shoot_out = Shoot_Task();           //射击任务
		setFRICData(Global_data.FRIC_speed);
		vTaskDelay(GIMBAL_CONTROL_TIME);
	}
}

目前云台写了三个模式，分别是锁定模式，云台巡逻模式以及云台跟随模式。

云台跟随模式（视觉通信）

u8 global_follow_mode(int error_p_angle,int error_y_angle)
{

	Vision_InitTypeDef *vision;
	vision = &Vision;                //取地址才能取出其中的值
	
		Global_Data *global_data;
	global_data = &Global_data;

  /************* ↓ 跟随 ↓ ********************/
	if(vision->yaw.data+Inital_YAW_angle>360)
		global_data->yaw_angle=vision->yaw.data+Inital_YAW_angle-360;
	else if(vision->yaw.data+Inital_YAW_angle<0)
		 global_data->yaw_angle=360+vision->yaw.data+Inital_YAW_angle;
	else
	  global_data->yaw_angle=vision->yaw.data+Inital_YAW_angle;

	if((-vision->pitch.data)+Inital_PITCH_angle>360)
		global_data->pitch_angle=(-vision->pitch.data)+Inital_PITCH_angle-360;
	else if((-vision->pitch.data)+Inital_PITCH_angle<0)
		 global_data->pitch_angle=360+(-vision->pitch.data)+Inital_YAW_angle;	
	else 
	  global_data->pitch_angle=(-vision->pitch.data)+Inital_PITCH_angle;
	
	global_data->pitch_angle=global_data->pitch_angle;
		global_data->yaw_angle=global_data->yaw_angle;
	vTaskDelay(5);
		
  /************* ↓ 限幅 ↓ ********************/	
	
			if(global_data->pitch_angle>M6020_MAX2*0.043945){                 //角度转换   360/8191(6020编码器最.大值)=0.043945（约等于）
		global_data->pitch_angle=M6020_MAX2*0.043945;
	}else if(global_data->pitch_angle<M6020_MIN2*0.043945){
		global_data->pitch_angle=M6020_MIN2*0.043945;
	}
			if(global_data->yaw_angle>M6020_MAX*0.043945){
		global_data->yaw_angle=M6020_MAX*0.043945;
	}else if(global_data->yaw_angle<M6020_MIN*0.043945){
		global_data->yaw_angle=M6020_MIN*0.043945;	
	}
	
setGlobalData(global_data->yaw_angle,global_data->pitch_angle,0,0);		
		
}

（主要控制代码是global_data->yaw_angle=vision->yaw.data+Inital_YAW_angle;）

此跟随模式难点一在与与视觉信息传输与处理，因为视觉要求PITCH，YAW初始角度为0，但电机的角度很难为0（角度=编码器值*0.043945），故我们发给视觉的角度是要减去初始角度的（如下），所以视觉传回来的角度也应加回初始角度才适应我们的6020电机。难点二是角度转换，视觉角度是180°-负180°，而电机编码器是0-360度，故需要角度转换。

发送视觉数据（主要是发送代码是 Vision_Tx.yaw.data= Vision_Tx.pitch.data=）

	if(global_data->global_mode == global_follow)
	{
		if(global_data->m6020[0]->encoder*0.043945-Inital_YAW_angle>180)
			Vision_Tx.yaw.data=-(360-global_data->m6020[0]->encoder*0.043945-Inital_YAW_angle);
		else if(global_data->m6020[0]->encoder*0.043945-Inital_YAW_angle<-180)
			Vision_Tx.yaw.data=(360+global_data->m6020[0]->encoder*0.043945-Inital_YAW_angle);
		else{
			Vision_Tx.yaw.data=global_data->m6020[0]->encoder*0.043945-Inital_YAW_angle;
		}
	}
	else
	{
			if(global_data->m6020[0]->encoder*0.043945-Inital_YAW_angle>180)
			Vision_Tx.yaw.data=-(360-global_data->m6020[0]->encoder*0.043945-Inital_YAW_angle);
		else if(global_data->m6020[0]->encoder*0.043945-Inital_YAW_angle<-180)
			Vision_Tx.yaw.data=(360+global_data->m6020[0]->encoder*0.043945-Inital_YAW_angle);
		else{
			Vision_Tx.yaw.data=global_data->m6020[0]->encoder*0.043945-Inital_YAW_angle;
		}
	}
	
	Vision_Tx.pitch.data	=-(global_data->m6020[1]->encoder*0.043945-Inital_PITCH_angle);

电控与视觉转换原理（自己的笔记不多赘述），左边电控发送角度给视觉，右边电控接收数据

巡逻模式和锁定模式较简易，可直接从下面链接看代码，在此及不多费章节讲解。

射击也大有篇幅，实现大概作用是视觉一旦识别到有敌对车辆云台就进入跟随模式，摩擦轮打开，拨盘打开，开始射击

  if(vision->dis.data==0)                                      //RC.rc.s[0]==3||RC.rc.s[0]==0
  {
     shoot->shoot_fric_mode=FRIC_SPEED_OFF;  //间接（）
	  shoot->shoot_mode=SHOOT_MODE_OFF;        //直接（摩擦轮动）
		shoot->single_shot_time=0;
    }                                      //√
	else if(vision->dis.data!=0)
	{
	  shoot->shoot_fric_mode=FRIC_SPEED_ON;    //  1 开
	  shoot->shoot_mode=SHOOT_MODE_FRIC_WHEEL; //   √
		shoot->single_shot_time++;
	  }

详细代码在以下链接

最后附上视频链接，哨兵实现的基本功能

视频链接：https://pan.baidu.com/s/1wWpqJfF_l26m6uO6MpnzcQ
提取码：1111

此源码链接：https://pan.baidu.com/s/1sXTnh2KtHmdZGkttjcu7cw
提取码：1111