同步步兵代码，暂时关闭底盘跟随
This commit is contained in:
252
云台/云台-old/CarBody/Gimbal.c
Normal file
252
云台/云台-old/CarBody/Gimbal.c
Normal file
@@ -0,0 +1,252 @@
|
||||
#include "stm32f4xx.h" // Device header
|
||||
#include "stm32f4xx_conf.h"
|
||||
#include "Parameter.h"
|
||||
#include "PID.h"
|
||||
#include "Remote.h"
|
||||
#include "AttitudeAlgorithms.h"
|
||||
#include "M3508.h"
|
||||
#include "M2006.h"
|
||||
#include "GM6020.h"
|
||||
#include "Laser.h"
|
||||
#include "RefereeSystem.h"
|
||||
#include "Visual.h"
|
||||
#include "UART.h"
|
||||
|
||||
#define Gimbal_YawMotor GM6020_1//Yaw轴电机
|
||||
#define Gimbal_PitchMotor GM6020_2//Pitch轴电机
|
||||
#define Gimbal_L_FrictionWheel M3508_1//左摩擦轮
|
||||
#define Gimbal_R_FrictionWheel M3508_2//右摩擦轮
|
||||
|
||||
|
||||
|
||||
uint8_t Gimbal_FrictionWheelFlag;//云台小陀螺标志位,云台开摩擦轮标志位
|
||||
|
||||
PID_PositionInitTypedef Gimbal_YawAnglePositionPID,Gimbal_YawAngleSpeedPID;//Yaw轴GM6020电机PID
|
||||
PID_PositionInitTypedef Gimbal_PitchAnglePositionPID,Gimbal_PitchAngleSpeedPID;//Pitch轴GM6020电机PID
|
||||
PID_PositionInitTypedef Gimbal_L_FrictionWheelPID,Gimbal_R_FrictionWheelPID;//摩擦轮转速PID
|
||||
PID_PositionInitTypedef Gimbal_RammerSpinSpeedPID;//拨弹盘旋转PID
|
||||
|
||||
/*
|
||||
*函数简介:云台初始化
|
||||
*参数说明:无
|
||||
*返回类型:无
|
||||
*备注:无
|
||||
*/
|
||||
void Gimbal_Init(void)
|
||||
{/*
|
||||
PID_PositionStructureInit(&Gimbal_YawAnglePositionPID,0);//Yaw轴陀螺仪闭环
|
||||
PID_PositionSetParameter(&Gimbal_YawAnglePositionPID,140,0,0);
|
||||
PID_PositionSetEkRange(&Gimbal_YawAnglePositionPID,-1,1);
|
||||
PID_PositionSetOUTRange(&Gimbal_YawAnglePositionPID,-200,200);
|
||||
PID_PositionStructureInit(&Gimbal_YawAngleSpeedPID,200);
|
||||
PID_PositionSetParameter(&Gimbal_YawAngleSpeedPID,1000,0,1);
|
||||
PID_PositionSetEkRange(&Gimbal_YawAngleSpeedPID,-1,1);
|
||||
PID_PositionSetOUTRange(&Gimbal_YawAngleSpeedPID,-30000,30000);
|
||||
|
||||
PID_PositionStructureInit(&Gimbal_PitchAnglePositionPID,0);//Pitch轴陀螺仪闭环
|
||||
PID_PositionSetParameter(&Gimbal_PitchAnglePositionPID,6,0,0);
|
||||
PID_PositionSetEkRange(&Gimbal_PitchAnglePositionPID,-1,1);
|
||||
PID_PositionSetOUTRange(&Gimbal_PitchAnglePositionPID,-150,150);
|
||||
PID_PositionStructureInit(&Gimbal_PitchAngleSpeedPID,150);
|
||||
PID_PositionSetParameter(&Gimbal_PitchAngleSpeedPID,200,0,0);
|
||||
PID_PositionSetEkRange(&Gimbal_PitchAngleSpeedPID,-5,5);
|
||||
PID_PositionSetOUTRange(&Gimbal_PitchAngleSpeedPID,-30000,30000);
|
||||
*/
|
||||
PID_PositionStructureInit(&Gimbal_YawAnglePositionPID,0);//Yaw轴陀螺仪闭环
|
||||
PID_PositionSetParameter(&Gimbal_YawAnglePositionPID,10,0,20);
|
||||
PID_PositionSetEkRange(&Gimbal_YawAnglePositionPID,-1,1);
|
||||
PID_PositionSetOUTRange(&Gimbal_YawAnglePositionPID,-350,350);
|
||||
PID_PositionStructureInit(&Gimbal_YawAngleSpeedPID,200);
|
||||
PID_PositionSetParameter(&Gimbal_YawAngleSpeedPID,150,0,10);
|
||||
PID_PositionSetEkRange(&Gimbal_YawAngleSpeedPID,-1,1);
|
||||
PID_PositionSetOUTRange(&Gimbal_YawAngleSpeedPID,-50000,50000);
|
||||
|
||||
PID_PositionStructureInit(&Gimbal_PitchAnglePositionPID,0);//Pitch轴陀螺仪闭环
|
||||
PID_PositionSetParameter(&Gimbal_PitchAnglePositionPID,5,0,0);
|
||||
PID_PositionSetEkRange(&Gimbal_PitchAnglePositionPID,-1,1);
|
||||
PID_PositionSetOUTRange(&Gimbal_PitchAnglePositionPID,-150,150);
|
||||
PID_PositionStructureInit(&Gimbal_PitchAngleSpeedPID,150);
|
||||
PID_PositionSetParameter(&Gimbal_PitchAngleSpeedPID,250,0,0);
|
||||
PID_PositionSetEkRange(&Gimbal_PitchAngleSpeedPID,-1,1);
|
||||
PID_PositionSetOUTRange(&Gimbal_PitchAngleSpeedPID,-30000,30000);
|
||||
|
||||
PID_PositionStructureInit(&Gimbal_L_FrictionWheelPID,0);//左摩擦轮
|
||||
PID_PositionSetParameter(&Gimbal_L_FrictionWheelPID,16,0,30);
|
||||
PID_PositionSetEkRange(&Gimbal_L_FrictionWheelPID,-5,5);
|
||||
PID_PositionSetOUTRange(&Gimbal_L_FrictionWheelPID,-15000,15000);
|
||||
PID_PositionStructureInit(&Gimbal_R_FrictionWheelPID,0);//右摩擦轮
|
||||
PID_PositionSetParameter(&Gimbal_R_FrictionWheelPID,16,0,30);
|
||||
PID_PositionSetEkRange(&Gimbal_R_FrictionWheelPID,-5,5);
|
||||
PID_PositionSetOUTRange(&Gimbal_R_FrictionWheelPID,-15000,15000);
|
||||
|
||||
PID_PositionStructureInit(&Gimbal_RammerSpinSpeedPID,-Gimbal_RammerSpeed);//拨弹盘
|
||||
PID_PositionSetParameter(&Gimbal_RammerSpinSpeedPID,16,0,0);
|
||||
PID_PositionSetEkRange(&Gimbal_RammerSpinSpeedPID,-20,20);
|
||||
PID_PositionSetOUTRange(&Gimbal_RammerSpinSpeedPID,-30000,30000);
|
||||
|
||||
|
||||
|
||||
|
||||
Laser_Init();
|
||||
}
|
||||
|
||||
/*
|
||||
*函数简介:云台PID清理
|
||||
*参数说明:无
|
||||
*返回类型:无
|
||||
*备注:无
|
||||
*/
|
||||
void Gimbal_CleanPID(void)
|
||||
{
|
||||
PID_PositionClean(&Gimbal_YawAnglePositionPID);
|
||||
PID_PositionClean(&Gimbal_YawAngleSpeedPID);
|
||||
PID_PositionClean(&Gimbal_PitchAnglePositionPID);
|
||||
PID_PositionClean(&Gimbal_PitchAngleSpeedPID);
|
||||
PID_PositionClean(&Gimbal_L_FrictionWheelPID);
|
||||
PID_PositionClean(&Gimbal_R_FrictionWheelPID);
|
||||
PID_PositionClean(&Gimbal_RammerSpinSpeedPID);
|
||||
}
|
||||
|
||||
/*
|
||||
*函数简介:云台Pitch轴控制
|
||||
*参数说明:无
|
||||
*返回类型:无
|
||||
*备注:根据拨杆或鼠标获得俯仰角度,映射比例在上方宏定义Gimbal_LeverSpeedMapRate更改
|
||||
*备注:俯仰限幅由结构决定,参数由Parameter.h文件中的Pitch_GM6020PositionLowerLinit和Pitch_GM6020PositionUpperLinit决定
|
||||
*备注:俯仰轴GM6020报文标识符和M2006高位ID一致,故均在拨弹盘控制函数中统一发送控制报文
|
||||
*备注:在此函数中进行了视觉自瞄处理,由于视觉组摆烂,并没有开发出自瞄,也没用进行过联调,故自瞄部分没有拆出去独立函数
|
||||
*/
|
||||
void Gimbal_PitchControl(void)
|
||||
{
|
||||
if(Remote_StartFlag==2)Gimbal_PitchAnglePositionPID.Need_Value=0;//遥控器刚建立连接时,复位Pitch轴角度
|
||||
if(((Remote_RxData.Remote_L_UD>1050 && RefereeSystem_Status==0) || (1024+Remote_RxData.Remote_Mouse_DU*3)<1000) && AttitudeAlgorithms_DegRoll>Pitch_GM6020AngleUpperLinit)
|
||||
Gimbal_PitchAnglePositionPID.Need_Value-=Gimbal_LeverSpeedMapRate/8192.0f*360.0f;//通过遥控器或者鼠标获取俯仰情况
|
||||
else if(((Remote_RxData.Remote_L_UD<1000 && RefereeSystem_Status==0) || (1024+Remote_RxData.Remote_Mouse_DU*3)>1050) && AttitudeAlgorithms_DegRoll<Pitch_GM6020AngleLowerLinit)
|
||||
Gimbal_PitchAnglePositionPID.Need_Value+=Gimbal_LeverSpeedMapRate/8192.0f*360.0f;
|
||||
|
||||
if(Remote_RxData.Remote_Mouse_KeyR==1 && Visual_ReceiveFlag==1)//自瞄,补偿角度
|
||||
{
|
||||
Visual_ReceiveFlag=0;
|
||||
if(GM6020_MotorStatus[Gimbal_PitchMotor-0x205].Position>Pitch_GM6020PositionUpperLinit && Visual_Pitch>0)Gimbal_PitchAnglePositionPID.Need_Value=AttitudeAlgorithms_DegRoll-Visual_Pitch;
|
||||
if(GM6020_MotorStatus[Gimbal_PitchMotor-0x205].Position<Pitch_GM6020PositionLowerLinit && Visual_Pitch<0)Gimbal_PitchAnglePositionPID.Need_Value=AttitudeAlgorithms_DegRoll-Visual_Pitch;
|
||||
Gimbal_YawAnglePositionPID.Need_Value=AttitudeAlgorithms_DegYaw+Visual_Yaw;
|
||||
Remote_RxData.Remote_Mouse_KeyPushR=0;
|
||||
}
|
||||
|
||||
//串级PID闭环Pitch角
|
||||
|
||||
PID_PositionCalc(&Gimbal_PitchAnglePositionPID,AttitudeAlgorithms_DegRoll);
|
||||
Gimbal_PitchAngleSpeedPID.Need_Value=Gimbal_PitchAnglePositionPID.OUT;
|
||||
|
||||
PID_PositionCalc(&Gimbal_PitchAngleSpeedPID,GM6020_MotorStatus[Gimbal_PitchMotor-0x205].Speed);
|
||||
//UART2_Printf("%d\n",AttitudeAlgorithms_DegRoll);
|
||||
//UART2_Printf("%d\n",AttitudeAlgorithms_DegYaw);
|
||||
}
|
||||
|
||||
/*
|
||||
*函数简介:云台PID清理
|
||||
*参数说明:无
|
||||
*返回类型:无
|
||||
*备注:无
|
||||
*/
|
||||
void Debug(void)
|
||||
{
|
||||
//UART2_Printf("%f,%f,%f,%f\n",Gimbal_PitchAngleSpeedPID.Need_Value,Gimbal_PitchAngleSpeedPID.Now_Value,Gimbal_PitchAnglePositionPID.Now_Value,Gimbal_PitchAnglePositionPID.Need_Value);
|
||||
UART2_Printf("%f\n",AttitudeAlgorithms_DegRoll);
|
||||
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
*函数简介:云台Yaw轴控制
|
||||
*参数说明:无
|
||||
*返回类型:无
|
||||
*备注:根据拨杆或鼠标获得偏航角度,映射比例在上方宏定义Gimbal_LeverSpeedMapRate和Gimbal_YawPitchSpeedRate更改
|
||||
*备注:由于云台一直根据陀螺仪角度闭环,不需要考虑小陀螺问题
|
||||
*/
|
||||
void Gimbal_YawControl(void)
|
||||
{
|
||||
if(Remote_StartFlag==2)
|
||||
Gimbal_YawAnglePositionPID.Need_Value=AttitudeAlgorithms_DegYaw;//遥控器刚建立连接时,复位Yaw轴角度
|
||||
if((Remote_RxData.Remote_L_RL>1050 && RefereeSystem_Status==0) || 1024+PC_Spin*PC_Mouse_RLSensitivity>1050)//根据摇杆改变偏航
|
||||
{
|
||||
if(PC_Spin==0)Gimbal_YawAnglePositionPID.Need_Value-=Gimbal_LeverSpeedMapRate*Gimbal_YawPitchSpeedRate*Gimbal_YawPitchSpeedRate*0.0439453125f*((Remote_RxData.Remote_L_RL-1024)/660.0f);
|
||||
else Gimbal_YawAnglePositionPID.Need_Value-=Gimbal_LeverSpeedMapRate*Gimbal_YawPitchSpeedRate*Gimbal_YawPitchSpeedRate*0.0439453125f*(PC_Spin*PC_Mouse_RLSensitivity/660.0f*2);
|
||||
}
|
||||
else if((Remote_RxData.Remote_L_RL<1000 && RefereeSystem_Status==0) || 1024+PC_Spin*PC_Mouse_RLSensitivity<1000)
|
||||
{
|
||||
if(PC_Spin==0)Gimbal_YawAnglePositionPID.Need_Value+=Gimbal_LeverSpeedMapRate*Gimbal_YawPitchSpeedRate*Gimbal_YawPitchSpeedRate*0.0439453125f*((1024-Remote_RxData.Remote_L_RL)/660.0f);
|
||||
else Gimbal_YawAnglePositionPID.Need_Value-=Gimbal_LeverSpeedMapRate*Gimbal_YawPitchSpeedRate*Gimbal_YawPitchSpeedRate*0.0439453125f*(PC_Spin*PC_Mouse_RLSensitivity/660.0f*2);
|
||||
}
|
||||
|
||||
//串级PID闭环Yaw角
|
||||
PID_PositionCalc(&Gimbal_YawAnglePositionPID,AttitudeAlgorithms_DegYaw);
|
||||
Gimbal_YawAngleSpeedPID.Need_Value=Gimbal_YawAnglePositionPID.OUT;
|
||||
PID_PositionCalc(&Gimbal_YawAngleSpeedPID,GM6020_MotorStatus[0].Speed);
|
||||
GM6020_CAN2SetLIDVoltage(Gimbal_YawAngleSpeedPID.OUT,0,0,0);
|
||||
}
|
||||
|
||||
/*
|
||||
*函数简介:摩擦轮控制
|
||||
*参数说明:无
|
||||
*返回类型:无
|
||||
*备注:遥控左拨动开关向上拨(Remote_LS=1)开摩擦轮,摩擦轮打开的同时会打开激光
|
||||
*/
|
||||
void Gimbal_FiringMechanismControl(void)
|
||||
{
|
||||
if(((Remote_RxData.Remote_LS==1 && RefereeSystem_Status==0) || PC_FrictionWheel==1) && RefereeSystem_ShooterStatus==1)//摩擦轮开
|
||||
{
|
||||
Gimbal_L_FrictionWheelPID.Need_Value=-Gimbal_FrictionWheelSpeed;
|
||||
Gimbal_R_FrictionWheelPID.Need_Value=Gimbal_FrictionWheelSpeed;
|
||||
Laser_ON();//开激光
|
||||
Gimbal_FrictionWheelFlag=1;
|
||||
}
|
||||
else//摩擦轮关
|
||||
{
|
||||
Gimbal_L_FrictionWheelPID.Need_Value=Gimbal_R_FrictionWheelPID.Need_Value=0;
|
||||
Laser_OFF();//关激光
|
||||
Gimbal_FrictionWheelFlag=0;
|
||||
}
|
||||
|
||||
PID_PositionCalc(&Gimbal_L_FrictionWheelPID,M3508_MotorStatus[Gimbal_L_FrictionWheel-0x201].RotorSpeed);
|
||||
PID_PositionCalc(&Gimbal_R_FrictionWheelPID,M3508_MotorStatus[Gimbal_R_FrictionWheel-0x201].RotorSpeed);
|
||||
M3508_CANSetLIDCurrent(Gimbal_L_FrictionWheelPID.OUT,Gimbal_R_FrictionWheelPID.OUT,0,0);
|
||||
}
|
||||
|
||||
/*
|
||||
*函数简介:拨弹盘控制
|
||||
*参数说明:无
|
||||
*返回类型:无
|
||||
*备注:俯仰轴GM6020报文标识符和M2006高位ID一致,故均在拨弹盘控制函数中统一发送控制报文
|
||||
*/
|
||||
void Gimbal_Rammer(void)
|
||||
{
|
||||
if(Gimbal_FrictionWheelFlag==1)
|
||||
{
|
||||
if((Remote_RxData.Remote_ThumbWheel<1000 && RefereeSystem_Status==0) || PC_Fire==1)
|
||||
Gimbal_RammerSpinSpeedPID.Need_Value=-Gimbal_RammerSpeed;
|
||||
else if((Remote_RxData.Remote_ThumbWheel>1050 && RefereeSystem_Status==0) || PC_Ejection==1)
|
||||
Gimbal_RammerSpinSpeedPID.Need_Value=Gimbal_RammerSpeed;
|
||||
else
|
||||
Gimbal_RammerSpinSpeedPID.Need_Value=0;
|
||||
}
|
||||
else Gimbal_RammerSpinSpeedPID.Need_Value=0;
|
||||
|
||||
PID_PositionCalc(&Gimbal_RammerSpinSpeedPID,M2006_MotorStatus[6].RotorSpeed);
|
||||
M2006_CANSetHIDCurrent(0,Gimbal_PitchAngleSpeedPID.OUT,Gimbal_RammerSpinSpeedPID.OUT,0);
|
||||
}
|
||||
|
||||
/*
|
||||
*函数简介:云台运动控制
|
||||
*参数说明:无
|
||||
*返回类型:无
|
||||
*备注:无
|
||||
*/
|
||||
void Gimbal_MoveControl(void)
|
||||
{
|
||||
Gimbal_PitchControl();//云台Pitch轴控制
|
||||
Gimbal_YawControl();//云台Yaw轴控制
|
||||
|
||||
Gimbal_FiringMechanismControl();//摩擦轮控制
|
||||
Gimbal_Rammer();//拨弹盘控制
|
||||
}
|
||||
9
云台/云台-old/CarBody/Gimbal.h
Normal file
9
云台/云台-old/CarBody/Gimbal.h
Normal file
@@ -0,0 +1,9 @@
|
||||
#ifndef __GIMBAL_H
|
||||
#define __GIMBAL_H
|
||||
|
||||
void Gimbal_Init(void);//云台初始化
|
||||
void Gimbal_CleanPID(void);//云台PID清理
|
||||
void Gimbal_MoveControl(void);//云台运动控制
|
||||
void Debug(void);//调试函数
|
||||
|
||||
#endif
|
||||
52
云台/云台-old/CarBody/Keyboard.c
Normal file
52
云台/云台-old/CarBody/Keyboard.c
Normal file
@@ -0,0 +1,52 @@
|
||||
#include "stm32f4xx.h" // Device header
|
||||
#include "stm32f4xx_conf.h"
|
||||
#include "UART.h"
|
||||
#include "Remote.h"
|
||||
|
||||
/*
|
||||
*函数简介:键盘初始化
|
||||
*参数说明:无
|
||||
*返回类型:无
|
||||
*备注:初始化UART1(USART6)
|
||||
*/
|
||||
void Keyboard_Init(void)
|
||||
{
|
||||
UART1_Init();
|
||||
}
|
||||
|
||||
/*
|
||||
*函数简介:键盘数据处理
|
||||
*参数说明:接收数据
|
||||
*返回类型:无
|
||||
*备注:无
|
||||
*/
|
||||
void Keyboard_DataProcess(uint8_t *Data)
|
||||
{
|
||||
Remote_RxData.Remote_Mouse_KeyLastR=Remote_RxData.Remote_Mouse_KeyR;//获取上一次五个键的状态
|
||||
Remote_RxData.Remote_KeyLast_Q=Remote_RxData.Remote_Key_Q;
|
||||
Remote_RxData.Remote_KeyLast_E=Remote_RxData.Remote_Key_E;
|
||||
Remote_RxData.Remote_KeyLast_Shift=Remote_RxData.Remote_Key_Shift;
|
||||
Remote_RxData.Remote_KeyLast_Ctrl=Remote_RxData.Remote_Key_Ctrl;
|
||||
|
||||
Remote_RxData.Remote_Mouse_RL=(int16_t)((uint16_t)Data[1]<<8 | Data[0]);
|
||||
Remote_RxData.Remote_Mouse_DU=(int16_t)((uint16_t)Data[3]<<8 | Data[2]);
|
||||
Remote_RxData.Remote_Mouse_Wheel=(int16_t)((uint16_t)Data[5]<<8 | Data[4]);
|
||||
Remote_RxData.Remote_Mouse_KeyL=Data[6];
|
||||
Remote_RxData.Remote_Mouse_KeyR=Data[7];
|
||||
|
||||
Remote_RxData.Remote_Key_W=Data[8] & 0x01;
|
||||
Remote_RxData.Remote_Key_S=(Data[8]>>1) & 0x01;
|
||||
Remote_RxData.Remote_Key_A=(Data[8]>>2) & 0x01;
|
||||
Remote_RxData.Remote_Key_D=(Data[8]>>3) & 0x01;
|
||||
Remote_RxData.Remote_Key_Shift=(Data[8]>>4) & 0x01;
|
||||
Remote_RxData.Remote_Key_Ctrl=(Data[8]>>5) & 0x01;
|
||||
Remote_RxData.Remote_Key_Q=(Data[8]>>6) & 0x01;
|
||||
Remote_RxData.Remote_Key_E=(Data[8]>>7) & 0x01;
|
||||
|
||||
if(Remote_RxData.Remote_KeyLast_Q==0 && Remote_RxData.Remote_Key_Q==1)Remote_RxData.Remote_KeyPush_Q=!Remote_RxData.Remote_KeyPush_Q;//检测是否按下
|
||||
if(Remote_RxData.Remote_KeyLast_E==0 && Remote_RxData.Remote_Key_E==1)Remote_RxData.Remote_KeyPush_E=!Remote_RxData.Remote_KeyPush_E;
|
||||
if(Remote_RxData.Remote_KeyLast_Shift==0 && Remote_RxData.Remote_Key_Shift==1)Remote_RxData.Remote_KeyPush_Shift=!Remote_RxData.Remote_KeyPush_Shift;
|
||||
if(Remote_RxData.Remote_KeyLast_Ctrl==0 && Remote_RxData.Remote_Key_Ctrl==1)Remote_RxData.Remote_KeyPush_Ctrl=!Remote_RxData.Remote_KeyPush_Ctrl;
|
||||
if(Remote_RxData.Remote_Mouse_KeyLastR==0 && Remote_RxData.Remote_Mouse_KeyR==1)Remote_RxData.Remote_Mouse_KeyPushR=1;
|
||||
else Remote_RxData.Remote_Mouse_KeyPushR=0;
|
||||
}
|
||||
7
云台/云台-old/CarBody/Keyboard.h
Normal file
7
云台/云台-old/CarBody/Keyboard.h
Normal file
@@ -0,0 +1,7 @@
|
||||
#ifndef __KEYBOARD_H
|
||||
#define __KEYBOARD_H
|
||||
|
||||
void Keyboard_Init(void);//键盘初始化
|
||||
void Keyboard_DataProcess(uint8_t *Data);//键盘数据处理
|
||||
|
||||
#endif
|
||||
179
云台/云台-old/CarBody/RefereeSystem.c
Normal file
179
云台/云台-old/CarBody/RefereeSystem.c
Normal file
@@ -0,0 +1,179 @@
|
||||
#include "stm32f4xx.h" // Device header
|
||||
#include "stm32f4xx_conf.h"
|
||||
#include "RefereeSystem_CRCTable.h"
|
||||
#include "Keyboard.h"
|
||||
#include "Warming.h"
|
||||
#include "Remote.h"
|
||||
|
||||
/****************************************************************************************************
|
||||
|
||||
此处裁判系统只接收0x0201命令,获取机器人性能体系数据,主要获取发射机构是否上电
|
||||
帧格式:
|
||||
0xA5 0x0D 0x00 包序号 帧头CRC8校验 0x01 0x02 13ByteData 整包CRC16校验
|
||||
|___________________________________| |_______| |
|
||||
帧头 命令码 数据
|
||||
|
||||
****************************************************************************************************/
|
||||
|
||||
/*接收数据缓冲区数组元素数=命令码对应数据段长度+9*/
|
||||
uint8_t RefereeSystem_RxHEXPacket[21]={0xA5,0x0C,0x00,0x00,0x00,0x04,0x03,0};//裁判系统0x0304命令码接收数据缓冲区
|
||||
|
||||
uint8_t RefereeSystem_ShooterStatus;//发射机构状态,0-发射机构未上电,1-发射机构上电
|
||||
uint8_t RefereeSystem_ShooterOpenFlag;//发射机构上电标志位,1-发射机构正在上电,0-其他
|
||||
uint16_t RefereeSystem_ShooterOpenCounter;//发射机构上电读秒等待设备启动
|
||||
uint8_t RefereeSystem_Status=0;//图传链路连接状态,0-图传链路未连接,1-图传链路连接
|
||||
|
||||
/*
|
||||
*函数简介:裁判系统CRC8查表计算
|
||||
*参数说明:校验数据
|
||||
*参数说明:数据长度
|
||||
*参数说明:CRC初始值(默认给参数0xFF)
|
||||
*返回类型:无
|
||||
*备注:表格位于Referee System_CRCTable.h文件中CRC8_Table数组
|
||||
*/
|
||||
uint8_t RefereeSystem_GetCRC8CheckSum(uint8_t *Data,uint16_t Length,uint8_t Initial)
|
||||
{
|
||||
uint8_t Minuend;
|
||||
while(Length--)
|
||||
{
|
||||
Minuend=Initial^(*Data);
|
||||
Data++;
|
||||
Initial=CRC8_Table[Minuend];
|
||||
}
|
||||
return Initial;
|
||||
}
|
||||
|
||||
/*
|
||||
*函数简介:裁判系统CRC8校验
|
||||
*参数说明:校验数据(含尾端CRC校验码)
|
||||
*参数说明:数据长度
|
||||
*返回类型:校验正确返回1,否则返回0
|
||||
*备注:无
|
||||
*/
|
||||
uint8_t RefereeSystem_VerifyCRC8CheckSum(uint8_t *Data,uint16_t Length)
|
||||
{
|
||||
uint8_t CRC8CheckSum=0;
|
||||
if((Data==0) || (Length<=2))return 0;//特殊情况处理
|
||||
CRC8CheckSum=RefereeSystem_GetCRC8CheckSum(Data,Length-1,CRC8_Initial);//获取CRC8计算值
|
||||
return CRC8CheckSum==Data[Length-1];//测量值与计算值相比较
|
||||
}
|
||||
|
||||
/*
|
||||
*函数简介:裁判系统CRC16查表计算
|
||||
*参数说明:校验数据
|
||||
*参数说明:数据长度
|
||||
*参数说明:CRC初始值(默认给参数0xFFFF)
|
||||
*返回类型:无
|
||||
*备注:表格位于Referee System_CRCTable.h文件中CRC16_Table数组
|
||||
*/
|
||||
uint16_t RefereeSystem_GetCRC16CheckSum(uint8_t *Data,uint32_t Length,uint16_t Initial)
|
||||
{
|
||||
uint8_t Minuend;
|
||||
while(Length--)
|
||||
{
|
||||
Minuend=*Data;
|
||||
Data++;
|
||||
Initial=((uint16_t)(Initial)>>8)^CRC16_Table[((uint16_t)(Initial)^(uint16_t)(Minuend))&0x00FF];
|
||||
}
|
||||
return Initial;
|
||||
}
|
||||
|
||||
/*
|
||||
*函数简介:裁判系统CRC16校验
|
||||
*参数说明:校验数据(含尾端CRC校验码)
|
||||
*参数说明:数据长度
|
||||
*返回类型:校验正确返回1,否则返回0
|
||||
*备注:无
|
||||
*/
|
||||
uint32_t RefereeSystem_VerifyCRC16CheckSum(uint8_t *Data, uint32_t Length)
|
||||
{
|
||||
uint16_t CRC16CheckSum=0;
|
||||
if((Data==0)||(Length<=2))return 0;//特殊情况处理
|
||||
CRC16CheckSum=RefereeSystem_GetCRC16CheckSum(Data,Length-2,CRC16_Initial);//获取CRC16计算值
|
||||
return ((CRC16CheckSum&0xFF)==Data[Length-2]&&((CRC16CheckSum>>8)&0xff)==Data[Length-1]);//测量值与计算值相比较
|
||||
}
|
||||
|
||||
/*
|
||||
*函数简介:裁判系统接收初始化
|
||||
*参数说明:无
|
||||
*返回类型:无
|
||||
*备注:默认使用UART1(USART6),默认Rx引脚PG9
|
||||
*/
|
||||
void RefereeSystem_Init(void)
|
||||
{
|
||||
Keyboard_Init();
|
||||
}
|
||||
|
||||
/*
|
||||
*函数简介:UART1串口中断接收裁判系统数据
|
||||
*参数说明:无
|
||||
*返回类型:无
|
||||
*备注:数据帧格式在最上方注释
|
||||
*/
|
||||
void USART6_IRQHandler(void)
|
||||
{
|
||||
/*有效数据位数=命令码对应数据段长度+2*/
|
||||
#define DataLength 14//裁判系统0x0304命令码有效数据位数
|
||||
|
||||
static int RxHEXState=0;//定义静态变量用于接收模式的选择
|
||||
static int pRxHEXState=0;//定义静态变量用于充当计数器
|
||||
|
||||
uint8_t RefereeSystem_RxData;//裁判系统接收数据
|
||||
|
||||
TIM_SetCounter(TIM7,0);
|
||||
TIM_Cmd(TIM7,DISABLE);//关闭定时器并重置计数值
|
||||
|
||||
if(USART_GetITStatus(USART6,USART_IT_RXNE)==SET)//查询接收中断标志位
|
||||
{
|
||||
USART_ClearITPendingBit(USART6,USART_IT_RXNE);//清除接收中断标志位
|
||||
|
||||
RefereeSystem_RxData=USART_ReceiveData(USART6);//将数据存入缓存区
|
||||
|
||||
/*=====检查帧头=====*/
|
||||
if(RxHEXState==0){if(RefereeSystem_RxData==0xA5)RxHEXState=1;}
|
||||
|
||||
/*=====检查数据段长度,区分命令码=====*/
|
||||
else if(RxHEXState==1){if(RefereeSystem_RxData==0x0C)RxHEXState=2;else RxHEXState=0;}
|
||||
|
||||
/*=====0x0304命令码=====*/
|
||||
/*=====检查帧头其他部分=====*/
|
||||
else if(RxHEXState==2){if(RefereeSystem_RxData==0x00)RxHEXState=3;else RxHEXState=0;}
|
||||
else if(RxHEXState==3){RefereeSystem_RxHEXPacket[3]=RefereeSystem_RxData;RxHEXState=4;}
|
||||
else if(RxHEXState==4)
|
||||
{
|
||||
RefereeSystem_RxHEXPacket[4]=RefereeSystem_RxData;
|
||||
|
||||
if(RefereeSystem_VerifyCRC8CheckSum(RefereeSystem_RxHEXPacket,5)==1)RxHEXState=5;
|
||||
else RxHEXState=0;
|
||||
}
|
||||
|
||||
/*=====检查命令码=====*/
|
||||
else if(RxHEXState==5){if(RefereeSystem_RxData==0x04)RxHEXState=6;else RxHEXState=0;}//接收命令码
|
||||
else if(RxHEXState==6){if(RefereeSystem_RxData==0x03){RxHEXState=7;pRxHEXState=0;}else RxHEXState=0;}
|
||||
|
||||
/*=====接收有效数据=====*/
|
||||
else if(RxHEXState==7)
|
||||
{
|
||||
RefereeSystem_RxHEXPacket[pRxHEXState+7]=RefereeSystem_RxData;//接收数据
|
||||
pRxHEXState++;
|
||||
|
||||
if(pRxHEXState>=DataLength)
|
||||
{
|
||||
if(RefereeSystem_VerifyCRC16CheckSum(RefereeSystem_RxHEXPacket,21)==1)//CRC校验
|
||||
{
|
||||
Keyboard_DataProcess(&(RefereeSystem_RxHEXPacket[7]));//获取键盘数据
|
||||
RefereeSystem_Status=1;
|
||||
}
|
||||
RxHEXState=0;
|
||||
}
|
||||
}
|
||||
|
||||
if(Remote_StartFlag==1)//第一次接收数据
|
||||
{
|
||||
Remote_StartFlag=2;
|
||||
Warming_LEDClean();
|
||||
}
|
||||
Remote_Status=1;//遥控器已连接
|
||||
}
|
||||
TIM_Cmd(TIM7,ENABLE);//开启定时
|
||||
}
|
||||
12
云台/云台-old/CarBody/RefereeSystem.h
Normal file
12
云台/云台-old/CarBody/RefereeSystem.h
Normal file
@@ -0,0 +1,12 @@
|
||||
#ifndef __REFEREESYSTEM_H
|
||||
#define __REFEREESYSTEM_H
|
||||
|
||||
extern uint8_t RefereeSystem_ShooterStatus;//裁判系统接收数据缓冲区
|
||||
|
||||
extern uint8_t RefereeSystem_ShooterOpenFlag;//发射机构上电标志位,1-发射机构正在上电,0-其他
|
||||
extern uint16_t RefereeSystem_ShooterOpenCounter;//发射机构上电读秒等待设备启动
|
||||
extern uint8_t RefereeSystem_Status;//图传链路连接状态
|
||||
|
||||
void RefereeSystem_Init(void);//裁判系统接收初始化
|
||||
|
||||
#endif
|
||||
48
云台/云台-old/CarBody/RefereeSystem_CRCTable.h
Normal file
48
云台/云台-old/CarBody/RefereeSystem_CRCTable.h
Normal file
@@ -0,0 +1,48 @@
|
||||
#ifndef __REFEREESYSTEM_CRCTABLE_H
|
||||
#define __REFEREESYSTEM_CRCTABLE_H
|
||||
|
||||
//crc8 generator polynomial:G(x)=x8+x5+x4+1//CRC8-MAXIM
|
||||
uint8_t CRC8_Initial=0xFF;//CRC8初始值
|
||||
const uint8_t CRC8_Table[256]=
|
||||
{
|
||||
0x00,0x5E,0xBC,0xE2,0x61,0x3F,0xDD,0x83,0xC2,0x9C,0x7E,0x20,0xA3,0xFD,0x1F,0x41,
|
||||
0x9D,0xC3,0x21,0x7F,0xFC,0xA2,0x40,0x1E,0x5F,0x01,0xE3,0xBD,0x3E,0x60,0x82,0xDC,
|
||||
0x23,0x7D,0x9F,0xC1,0x42,0x1C,0xFE,0xA0,0xE1,0xBF,0x5D,0x03,0x80,0xDE,0x3C,0x62,
|
||||
0xBE,0xE0,0x02,0x5C,0xDF,0x81,0x63,0x3D,0x7C,0x22,0xC0,0x9E,0x1D,0x43,0xA1,0xFF,
|
||||
0x46,0x18,0xFA,0xA4,0x27,0x79,0x9B,0xC5,0x84,0xDA,0x38,0x66,0xE5,0xBB,0x59,0x07,
|
||||
0xDB,0x85,0x67,0x39,0xBA,0xE4,0x06,0x58,0x19,0x47,0xA5,0xFB,0x78,0x26,0xC4,0x9A,
|
||||
0x65,0x3B,0xD9,0x87,0x04,0x5A,0xB8,0xE6,0xA7,0xF9,0x1B,0x45,0xC6,0x98,0x7A,0x24,
|
||||
0xF8,0xA6,0x44,0x1A,0x99,0xC7,0x25,0x7B,0x3A,0x64,0x86,0xD8,0x5B,0x05,0xE7,0xB9,
|
||||
0x8C,0xD2,0x30,0x6E,0xED,0xB3,0x51,0x0F,0x4E,0x10,0xF2,0xAC,0x2F,0x71,0x93,0xCD,
|
||||
0x11,0x4F,0xAD,0xF3,0x70,0x2E,0xCC,0x92,0xD3,0x8D,0x6F,0x31,0xB2,0xEC,0x0E,0x50,
|
||||
0xAF,0xF1,0x13,0x4D,0xCE,0x90,0x72,0x2C,0x6D,0x33,0xD1,0x8F,0x0C,0x52,0xB0,0xEE,
|
||||
0x32,0x6C,0x8E,0xD0,0x53,0x0D,0xEF,0xB1,0xF0,0xAE,0x4C,0x12,0x91,0xCF,0x2D,0x73,
|
||||
0xCA,0x94,0x76,0x28,0xAB,0xF5,0x17,0x49,0x08,0x56,0xB4,0xEA,0x69,0x37,0xD5,0x8B,
|
||||
0x57,0x09,0xEB,0xB5,0x36,0x68,0x8A,0xD4,0x95,0xCB,0x29,0x77,0xF4,0xAA,0x48,0x16,
|
||||
0xE9,0xB7,0x55,0x0B,0x88,0xD6,0x34,0x6A,0x2B,0x75,0x97,0xC9,0x4A,0x14,0xF6,0xA8,
|
||||
0x74,0x2A,0xC8,0x96,0x15,0x4B,0xA9,0xF7,0xB6,0xE8,0x0A,0x54,0xD7,0x89,0x6B,0x35
|
||||
};
|
||||
|
||||
//crc16 generator polynomial:G(x)=x16+x12+x5+1//CRC16-CCITT
|
||||
uint16_t CRC16_Initial=0xFFFF;//CRC16初始值
|
||||
const uint16_t CRC16_Table[256]=
|
||||
{
|
||||
0x0000,0x1189,0x2312,0x329B,0x4624,0x57AD,0x6536,0x74BF,0x8C48,0x9DC1,0xAF5A,0xBED3,0xCA6C,0xDBE5,0xE97E,0xF8F7,
|
||||
0x1081,0x0108,0x3393,0x221A,0x56A5,0x472C,0x75B7,0x643E,0x9CC9,0x8D40,0xBFDB,0xAE52,0xDAED,0xCB64,0xF9FF,0xE876,
|
||||
0x2102,0x308B,0x0210,0x1399,0x6726,0x76AF,0x4434,0x55BD,0xAD4A,0xBCC3,0x8E58,0x9FD1,0xEB6E,0xFAE7,0xC87C,0xD9F5,
|
||||
0x3183,0x200A,0x1291,0x0318,0x77A7,0x662E,0x54B5,0x453C,0xBDCB,0xAC42,0x9ED9,0x8F50,0xFBEF,0xEA66,0xD8FD,0xC974,
|
||||
0x4204,0x538D,0x6116,0x709F,0x0420,0x15A9,0x2732,0x36BB,0xCE4C,0xDFC5,0xED5E,0xFCD7,0x8868,0x99E1,0xAB7A,0xBAF3,
|
||||
0x5285,0x430C,0x7197,0x601E,0x14A1,0x0528,0x37B3,0x263A,0xDECD,0xCF44,0xFDDF,0xEC56,0x98E9,0x8960,0xBBFB,0xAA72,
|
||||
0x6306,0x728F,0x4014,0x519D,0x2522,0x34AB,0x0630,0x17B9,0xEF4E,0xFEC7,0xCC5C,0xDDD5,0xA96A,0xB8E3,0x8A78,0x9BF1,
|
||||
0x7387,0x620E,0x5095,0x411C,0x35A3,0x242A,0x16B1,0x0738,0xFFCF,0xEE46,0xDCDD,0xCD54,0xB9EB,0xA862,0x9AF9,0x8B70,
|
||||
0x8408,0x9581,0xA71A,0xB693,0xC22C,0xD3A5,0xE13E,0xF0B7,0x0840,0x19C9,0x2B52,0x3ADB,0x4E64,0x5FED,0x6D76,0x7CFF,
|
||||
0x9489,0x8500,0xB79B,0xA612,0xD2AD,0xC324,0xF1BF,0xE036,0x18C1,0x0948,0x3BD3,0x2A5A,0x5EE5,0x4F6C,0x7DF7,0x6C7E,
|
||||
0xA50A,0xB483,0x8618,0x9791,0xE32E,0xF2A7,0xC03C,0xD1B5,0x2942,0x38CB,0x0A50,0x1BD9,0x6F66,0x7EEF,0x4C74,0x5DFD,
|
||||
0xB58B,0xA402,0x9699,0x8710,0xF3AF,0xE226,0xD0BD,0xC134,0x39C3,0x284A,0x1AD1,0x0B58,0x7FE7,0x6E6E,0x5CF5,0x4D7C,
|
||||
0xC60C,0xD785,0xE51E,0xF497,0x8028,0x91A1,0xA33A,0xB2B3,0x4A44,0x5BCD,0x6956,0x78DF,0x0C60,0x1DE9,0x2F72,0x3EFB,
|
||||
0xD68D,0xC704,0xF59F,0xE416,0x90A9,0x8120,0xB3BB,0xA232,0x5AC5,0x4B4C,0x79D7,0x685E,0x1CE1,0x0D68,0x3FF3,0x2E7A,
|
||||
0xE70E,0xF687,0xC41C,0xD595,0xA12A,0xB0A3,0x8238,0x93B1,0x6B46,0x7ACF,0x4854,0x59DD,0x2D62,0x3CEB,0x0E70,0x1FF9,
|
||||
0xF78F,0xE606,0xD49D,0xC514,0xB1AB,0xA022,0x92B9,0x8330,0x7BC7,0x6A4E,0x58D5,0x495C,0x3DE3,0x2C6A,0x1EF1,0x0F78
|
||||
};
|
||||
|
||||
#endif
|
||||
158
云台/云台-old/CarBody/Visual.c
Normal file
158
云台/云台-old/CarBody/Visual.c
Normal file
@@ -0,0 +1,158 @@
|
||||
#include "stm32f4xx.h" // Device header
|
||||
#include "stm32f4xx_conf.h"
|
||||
#include "Visual.h"
|
||||
#include "UART.h"
|
||||
#include "gimbal.h"
|
||||
#include "AttitudeAlgorithms.h"
|
||||
#include"Delay.h"
|
||||
|
||||
float Visual_Yaw,Visual_Pitch,Visual_Roll,Visual_Delay;//视觉数据偏航角,视觉数据俯仰角
|
||||
uint8_t Visual_RxHEXPacket[4],Visual_ReceiveFlag;//视觉数据接收缓冲区,视觉数据接收完成标志位
|
||||
|
||||
/*
|
||||
*函数简介:视觉初始化
|
||||
*参数说明:无
|
||||
发送至下位机数据
|
||||
|数据 1|数据 2|数据 3|数据 4|数据 5|
|
||||
|----|----|----|----|----|
|
||||
|0x09|0x14|yaw 轴|Pitch 轴|0x18|
|
||||
|
||||
*返回类型:无
|
||||
*备注:初始化UART2(USART1)
|
||||
Freq=Sys_APB1TIM/(PSC+1)/(ARR+1)=84MHz/(PSC+1)/(ARR+1)
|
||||
*/
|
||||
void Visual_Init(void)
|
||||
{/*
|
||||
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM12,ENABLE);//开启时钟
|
||||
|
||||
TIM_InternalClockConfig(TIM12);//选择时基单元的时钟(TIM9)
|
||||
|
||||
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;//配置时基单元（配置参数）
|
||||
TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1;//配置时钟分频为1分频
|
||||
TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up;//配置计数器模式为向上计数
|
||||
TIM_TimeBaseInitStructure.TIM_Period=500-1;//配置自动重装值ARR
|
||||
TIM_TimeBaseInitStructure.TIM_Prescaler=840-1;//配置分频值PSC,定时50ms
|
||||
TIM_TimeBaseInitStructure.TIM_RepetitionCounter=0;//配置重复计数单元的置为0
|
||||
TIM_TimeBaseInit(TIM12,&TIM_TimeBaseInitStructure);//初始化TIM12
|
||||
|
||||
TIM_ClearFlag(TIM12,TIM_FLAG_Update);//清除配置时基单元产生的中断标志位
|
||||
|
||||
TIM_ITConfig(TIM12,TIM_IT_Update,ENABLE);//使能更新中断
|
||||
|
||||
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//选择NVIC分组
|
||||
|
||||
NVIC_InitTypeDef NVIC_InitStructure1;//配置NVIC（配置参数）
|
||||
NVIC_InitStructure1.NVIC_IRQChannel=TIM8_BRK_TIM12_IRQn;//选择中断通道为TIM12
|
||||
NVIC_InitStructure1.NVIC_IRQChannelCmd=ENABLE;//使能中断通道
|
||||
NVIC_InitStructure1.NVIC_IRQChannelPreemptionPriority=3;//TIM12的抢占优先级
|
||||
NVIC_InitStructure1.NVIC_IRQChannelSubPriority=3;//TIM12的响应优先级
|
||||
NVIC_Init(&NVIC_InitStructure1);//初始化NVIC
|
||||
TIM_Cmd(TIM12,ENABLE);//启动定时器
|
||||
*/
|
||||
Visual_SendData();
|
||||
|
||||
}
|
||||
void Visual_SendData(void)
|
||||
{
|
||||
SendData MCUMessage;
|
||||
MCUMessage.curr_yaw=AttitudeAlgorithms_DegYaw;
|
||||
MCUMessage.curr_pitch=AttitudeAlgorithms_DegPitch;
|
||||
MCUMessage.state=(uint8_t)('a');
|
||||
MCUMessage.mark=(uint8_t)(0);
|
||||
MCUMessage.anti_top=(uint8_t)(0);
|
||||
MCUMessage.enemy_color=(uint8_t)(1);
|
||||
MCUMessage.delta_x=(uint8_t)(1);
|
||||
MCUMessage.delta_y=(uint8_t)(1);
|
||||
UART2_SendArray((uint8_t *)&MCUMessage, sizeof(MCUMessage));//UART2_SendArray((uint8_t *)&MCUMessage, sizeof(MCUMessage));
|
||||
/*
|
||||
UART2_SendByte((float)AttitudeAlgorithms_DegYaw);
|
||||
UART2_SendByte((float)AttitudeAlgorithms_DegRoll);
|
||||
//UART2_SendByte((float)AttitudeAlgorithms_DegPitch);
|
||||
|
||||
UART2_SendByte((uint8_t)0);
|
||||
UART2_SendByte((uint8_t)'a');
|
||||
UART2_SendByte((uint8_t)0);
|
||||
|
||||
UART2_SendByte((uint8_t)0);
|
||||
UART2_SendByte((uint8_t)1);
|
||||
UART2_SendByte((uint8_t)1);
|
||||
*/
|
||||
//UART2_Printf("%f %f a 1 1 1 1 1 ",AttitudeAlgorithms_DegYaw,AttitudeAlgorithms_DegRoll);
|
||||
|
||||
UART2_SendByte((uint8_t)'\n');
|
||||
|
||||
|
||||
|
||||
|
||||
//UART2_SendArray((uint8_t *)&MCUMessage, sizeof(MCUMessage));
|
||||
}
|
||||
/* buff[0] = 's';
|
||||
buff[1] = static_cast<char>((x_tmp >> 8) & 0xFF);
|
||||
buff[2] = static_cast<char>((x_tmp >> 0) & 0xFF);
|
||||
buff[3] = static_cast<char>((y_tmp >> 8) & 0xFF);
|
||||
buff[4] = static_cast<char>((y_tmp >> 0) & 0xFF);
|
||||
buff[5] = static_cast<char>((z_tmp >> 8) & 0xFF);
|
||||
buff[6] = static_cast<char>((z_tmp >> 0) & 0xFF);
|
||||
buff[7] = static_cast<char>((shoot_delay >> 8) & 0xFF);
|
||||
buff[8] = static_cast<char>((shoot_delay >> 0) & 0xFF);
|
||||
buff[9] = 'e';
|
||||
*/
|
||||
|
||||
/*
|
||||
*函数简介:UART2串口中断接收视觉数据
|
||||
*参数说明:无
|
||||
*返回类型:无
|
||||
*备注:无
|
||||
*/
|
||||
void USART1_IRQHandler(void)
|
||||
{
|
||||
#define DataLength 8//有效数据位数
|
||||
char Visual_RxHEXPacket[10];
|
||||
|
||||
static uint8_t UART2_RxData;
|
||||
//char Message[10];
|
||||
static int RxHEXState=0;//定义静态变量用于接收模式的选择
|
||||
static int pRxHEXState=0;//定义静态变量用于充当计数器
|
||||
char startflag;//包头1
|
||||
//char endflag;//包头2
|
||||
|
||||
if(USART_GetITStatus(USART1,USART_IT_RXNE)==SET)//查询接收中断标志位
|
||||
{
|
||||
UART2_RxData=USART_ReceiveData(USART1);//将数据存入缓存区
|
||||
startflag=(char)UART2_RxData;
|
||||
|
||||
|
||||
if(RxHEXState==0)//模式0-等待包头1(0x09)
|
||||
{
|
||||
|
||||
if( startflag =='s')//检测包头
|
||||
RxHEXState=1;//转入模式1
|
||||
}
|
||||
else if(RxHEXState==1)//模式1-接收有效数据
|
||||
{
|
||||
pRxHEXState=0;//复位计数器
|
||||
Visual_RxHEXPacket[pRxHEXState]=UART2_RxData;//接收数据
|
||||
pRxHEXState++;
|
||||
|
||||
if(pRxHEXState>=DataLength)
|
||||
RxHEXState=2;//转入模式2
|
||||
}
|
||||
else if(RxHEXState==2)//模式2-等待包尾
|
||||
{
|
||||
if(UART2_RxData=='e')//检测包尾
|
||||
{
|
||||
Visual_Yaw=(float)((uint8_t)Visual_RxHEXPacket[0]<<8 | Visual_RxHEXPacket[1]);//右摇杆右左
|
||||
Visual_Pitch=(float)((uint8_t)Visual_RxHEXPacket[2]<<8 | Visual_RxHEXPacket[3]);//右摇杆上下
|
||||
Visual_Roll=(float)((uint8_t)Visual_RxHEXPacket[4]<<8 | Visual_RxHEXPacket[5]);//左摇杆右左
|
||||
Visual_Delay=(float)((uint8_t)Visual_RxHEXPacket[6]<<8 | Visual_RxHEXPacket[7]);//左摇杆上下
|
||||
//Visual_Yaw=(int16_t)((uint16_t)Visual_RxHEXPacket[0]<<8 | Visual_RxHEXPacket[1])*0.01f;
|
||||
//Visual_Pitch=(int16_t)((uint16_t)Visual_RxHEXPacket[2]<<8 | Visual_RxHEXPacket[3])*0.01f;
|
||||
Visual_ReceiveFlag=1;
|
||||
}
|
||||
RxHEXState=0;//回到模式0
|
||||
}
|
||||
}
|
||||
|
||||
USART_ClearITPendingBit(USART1,USART_IT_RXNE);//清除接收中断标志位
|
||||
Visual_SendData();
|
||||
}
|
||||
32
云台/云台-old/CarBody/Visual.h
Normal file
32
云台/云台-old/CarBody/Visual.h
Normal file
@@ -0,0 +1,32 @@
|
||||
#ifndef __VISUAL_H
|
||||
#define __VISUAL_H
|
||||
|
||||
extern float Visual_Yaw,Visual_Pitch,Visual_Roll,Visual_Delay;//视觉数据偏航角,视觉数据俯仰角
|
||||
extern uint8_t Visual_ReceiveFlag;//视觉数据接收完成标志位
|
||||
///发送数据
|
||||
typedef struct
|
||||
{
|
||||
float curr_yaw; // 当前云台yaw角度
|
||||
float curr_pitch; // 当前云台pitch角
|
||||
uint8_t state; // 当前状态，自瞄-大符-小符
|
||||
uint8_t mark; // 云台角度标记位
|
||||
uint8_t anti_top; // 是否为反陀螺模式
|
||||
uint8_t enemy_color; // 敌方颜色
|
||||
int delta_x; // 能量机关x轴补偿量
|
||||
int delta_y; // 能量机关y轴补偿量
|
||||
}SendData;
|
||||
|
||||
//接受数据
|
||||
typedef struct
|
||||
{
|
||||
char start_flag; // 帧头标志，字符's'
|
||||
int16_t yaw; // float类型的实际角度(以度为单位)/100*(32768-1)
|
||||
int16_t pitch; // float类型的实际角度(以度为单位)/100*(32768-1)
|
||||
uint16_t shoot_delay; // 反陀螺模式下的发射延迟
|
||||
char end_flag; // 帧尾标识，字符'e'
|
||||
}GetData;
|
||||
|
||||
void Visual_Init(void);//视觉初始化
|
||||
void Visual_SendData(void);
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user