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

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

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

这里面有陀螺仪姿态解算、底盘、云台、裁判系统以及任务系统时间统计共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.5pidinit.pid.Ki=M3508_PID_i;                    //            0.05pidinit.pid.Kd=M3508_PID_d;                    //            0.5pidinit.Limiting=M3508_Limiting;               //电机输出限幅pidinit.Reduction_ratio=M3508_Reduction_ratio;//             0PID_Init2(&(PID_Datas.LB),&pidinit);PID_Init2(&(PID_Datas.RB),&pidinit);chassis_data->max_speed=MAX_SPEED;           //哨兵移动速度最大值 2500chassis_data->mid_speed=MID_SPEED;           //1000chassis_data->chassis_mode=chassis_stop;     //初始化模式为停止模式chassis_data->loop_time=set_loop_time;       //循环时间 5000chassis_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:                  //0Chassis_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;elseglobal_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

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

html div初始隐藏点击可见_3种CSS3移动手机隐藏菜单UI界面代码解析/附源码下载...
这是一款效果非常酷的jQuery和CSS3移动手机隐藏菜单UI界面设计.这个UI设计共有三种不同的打开隐藏菜单的效果,分别为滑动显示,Material Design风格效果和展开式效果. 使用方法 H ...
python程序代码解析_Python源码分析3 – 词法分析器PyTokenizer
Introduction 上次我们分析了Python中执行程序可分为5个步骤: Tokenizer进行词法分析,把源程序分解为Token Parser根据Token创建CST CST被转换为AST A ...
linux的内核设计,Linux内核设计的艺术清晰完整版PDF+配套源码
<Linux内核设计的艺术:图解Linux操作系统架构设计与实现原理>内容提要: 关于Linux内核的书已经不计其数,但这本书却是独树一帜的,它的内容代表着Linux内核研究成果的世界顶尖 ...
全民K歌PHP解析源码下载,全民K歌解析html源码
效果图: 全民K歌解析html源码新建一个html文件,复制下方代码粘贴保存上传即可使用:复制全民K歌作者主页地址粘贴即可解析全部歌曲html> 全民K歌获取用户歌曲,全民K歌api,全民K ...
python链表的创建_《大话数据结构》配套源码：链表（Python版）
该书随书源码的语言为C:我参考书中内容和配套源码,写了一套Python格式的配套源码.这套配套源码并非直接翻译C语言的配套源码,而是结合我的理解略作了修改. SinglyLinkedNode 单链表结 ...
Java定时任务(一) Timer及TimerTask的案例解析及源码分析
Java定时任务(一) Timer及TimerTask的案例解析及源码分析一.概述: 定时任务这个概念在Java的学习以及项目的开发中并不陌生,应用场景也是多种多样.比如我们会注意到12306网站 ...
华为OD机试（21-40）老题库解析Java源码系列连载ing
华为OD机试算法题新老题库练习及源码老题库 21.字符串序列判定 22.最长的指定瑕疵度的元音子串 23.处理器问题 24.单向链表中间节点 25.字符串重新排列.字符串重新排序 26.完美走位 2 ...
python电玩城源码_2019最新最全价值2W的微信H5电玩城游戏全套源码+架设教程+配置文档...
2019最新最全价值2W的微信H5电玩城游戏全套源码+架设教程+配置文档由小鱼H5游戏源码精心整理,并分享给大家.喜欢该资源的小伙伴请下载使用,注册.回复.每日签到.点赞送大量积分,满足您免费下载的愿 ...
Jsoup 解析Html源码实例
最近在做数据挖掘的过程中需要对html的源码进行解析,用到了Jsoup这个解析工具,下面写个基本实例来展现它的用法. 需要用到jar包:jsoup-1.7.2.jar,可以到jsoup的官网下载:ht ...

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

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

最新文章

热门文章