RoboDuoduo/shoot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Compare commits

merge into: RoboDuoduo:main
Branches Tags
RoboDuoduo:main
RoboDuoduo:vesc发射
RoboDuoduo:odrive离合
...
pull from: RoboDuoduo:vesc发射
Branches Tags
RoboDuoduo:vesc发射
RoboDuoduo:odrive离合
RoboDuoduo:main

8 Commits
main ... vesc发射

Author SHA1 Message Date
ws
e297b9960e 能用 2025-04-06 16:15:19 +08:00
ws
ca4d8145e5 1 2025-04-05 16:12:08 +08:00
ws
7fa2501dad 有史 2025-04-05 16:11:38 +08:00
ws
cf66cc255f 视觉测试可以 2025-04-03 15:11:57 +08:00
ws
14cb35351f 暂时重启扳机odrive！！！ 2025-03-31 23:16:05 +08:00
ws
33f343eadb 试了离合，卡不上，不调odrive了 2025-03-16 15:26:21 +08:00
ws
f50817a218 DJI解析 2025-03-15 16:42:56 +08:00
ws
83a9c93e16 离合初试 2025-03-14 23:57:48 +08:00
32 changed files with 663 additions and 379 deletions
Show Stats Expand all files Collapse all files

2
Core/Src/usart.c
View File

@ -102,7 +102,7 @@ void MX_USART6_UART_Init(void)
/* USER CODE END USART6_Init 1 */
huart6.Instance = USART6;
huart6.Init.BaudRate = 115200;
huart6.Init.BaudRate = 4000000;
huart6.Init.WordLength = UART_WORDLENGTH_8B;
huart6.Init.StopBits = UART_STOPBITS_1;
huart6.Init.Parity = UART_PARITY_NONE;

2
MDK-ARM/.vscode/keil-assistant.log vendored
View File

@ -58,3 +58,5 @@
[info] Log at : 2025/3/14|20:12:19|GMT+0800
[info] Log at : 2025/3/16|15:24:31|GMT+0800

4
MDK-ARM/.vscode/settings.json vendored
View File

@ -17,6 +17,8 @@
"shoot.h": "c",
"dji.h": "c",
"calc_lib.h": "c",
"odrive_shoot.h": "c"
"odrive_shoot.h": "c",
"remote_control.h": "c",
"dji_task.h": "c"
}
}

8
MDK-ARM/.vscode/uv4.log vendored
View File

@ -1,10 +1,4 @@
*** Using Compiler 'V5.06 update 7 (build 960)', folder: 'D:\keil\ARM\ARMCC\Bin'
Build target 'mycode1'
compiling read_spi.c...
compiling odrive_shoot.c...
compiling shoot_task.c...
linking...
Program Size: Code=26528 RO-data=1904 RW-data=280 ZI-data=22496
FromELF: creating hex file...
"mycode1\mycode1.axf" - 0 Error(s), 0 Warning(s).
Build Time Elapsed: 00:00:03
Build Time Elapsed: 00:00:00

2
MDK-ARM/.vscode/uv4.log.lock vendored
View File

@ -1 +1 @@
2025/3/14 20:41:26
2025/3/16 13:26:53

19
MDK-ARM/mycode1.uvoptx
View File

@ -208,37 +208,32 @@
<Ww>
<count>11</count>
<WinNumber>1</WinNumber>
<ItemText>angleo,0x0A</ItemText>
<ItemText>encoder,0x0A</ItemText>
</Ww>
<Ww>
<count>12</count>
<WinNumber>1</WinNumber>
<ItemText>shoot</ItemText>
<ItemText>mode,0x0A</ItemText>
</Ww>
<Ww>
<count>13</count>
<WinNumber>1</WinNumber>
<ItemText>multi_turn_angle,0x0A</ItemText>
<ItemText>m</ItemText>
</Ww>
<Ww>
<count>14</count>
<WinNumber>1</WinNumber>
<ItemText>angle_encoder</ItemText>
<ItemText>trigger_motor_data,0x0A</ItemText>
</Ww>
<Ww>
<count>15</count>
<WinNumber>1</WinNumber>
<ItemText>back_angle,0x0A</ItemText>
<ItemText>trigger_angle_o</ItemText>
</Ww>
<Ww>
<count>16</count>
<WinNumber>1</WinNumber>
<ItemText>encoder,0x0A</ItemText>
</Ww>
<Ww>
<count>17</count>
<WinNumber>1</WinNumber>
<ItemText>angles</ItemText>
<ItemText>shoot</ItemText>
</Ww>
</WatchWindow1>
<WatchWindow2>
@ -322,7 +317,7 @@
<Group>
<GroupName>Application/User/Core</GroupName>
<tvExp>1</tvExp>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>

6
User/bsp/can_init.c
View File

@ -18,18 +18,18 @@ void can_filter_init(void)
can_filter_st.FilterMaskIdHigh = 0x0000;
can_filter_st.FilterMaskIdLow = 0x0000;
can_filter_st.FilterBank = 0;
can_filter_st.SlaveStartFilterBank = 14;
can_filter_st.FilterFIFOAssignment = CAN_RX_FIFO0;
HAL_CAN_ConfigFilter(&hcan1, &can_filter_st);
HAL_CAN_Start(&hcan1);
HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING);
can_filter_st.SlaveStartFilterBank = 14;
can_filter_st.FilterBank = 14;
can_filter_st.FilterFIFOAssignment = CAN_RX_FIFO1;
HAL_CAN_ConfigFilter(&hcan2, &can_filter_st);
HAL_CAN_Start(&hcan2);
HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING);
}

2
User/bsp/uart_it.c
View File

@ -31,7 +31,7 @@ UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart) {
case BSP_UART_REF:
return &huart1;
case BSP_UART_AI:
return &huart6;
return &huart1;//NUC使用的是USART1
/*
case BSP_UART_XXX:
return &huartX;

11
User/device/GO_M8010_6_Driver.c
View File

@ -36,7 +36,8 @@ void GO_M8010_init (){
}
//暂存接收数据
static uint8_t Temp_buffer[16];
//static uint8_t Temp_buffer[16];
uint8_t Temp_buffer[16];
//dma传输完成后进入此回调函数
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
@ -45,12 +46,11 @@ void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
if ((Temp_buffer[14] != (crc&0xFF)) || (Temp_buffer[15] != ((crc>>8) & 0xFF)))
{
HAL_GPIO_WritePin(LED_B_GPIO_Port, LED_B_Pin, GPIO_PIN_SET); //蓝色灯亮 // indicate CRC incorrect
HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_G_Pin, GPIO_PIN_RESET); //红色灯灭
return;
}
// CRC pass and start converting data to the motor
HAL_GPIO_WritePin(GPIOH, GPIO_PIN_11, GPIO_PIN_SET); // indicate CRC correct
HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_G_Pin, GPIO_PIN_RESET); //红色灯灭
HAL_GPIO_WritePin(LED_B_GPIO_Port, LED_B_Pin, GPIO_PIN_RESET); //蓝色灯亮 // indicate CRC incorrect
GO_M8010_recv_data(Temp_buffer);
}
@ -92,6 +92,8 @@ void GO_M8010_send_data(UART_HandleTypeDef *huart, int id,int rev,float T,float
return;
}
Pos = Pos * (3.14159f / 180.0f); // 角度转弧度
// assign motor target goal to the buffer
motor->motor_send_data.head[0]=0xFE;
motor->motor_send_data.head[1]=0xEE;
@ -159,6 +161,7 @@ void GO_M8010_recv_data(uint8_t* Temp_buffer)
int32_t Pos = Temp_buffer[10]<<24 | Temp_buffer[9]<<16 | Temp_buffer[8]<<8 | Temp_buffer[7];
motor->Pos = Pos;
motor->Pos = (motor->Pos * 6.28319f )/(32768*6.33f);
motor->Pos = motor->Pos * (180.0f / 3.14159f); // 弧度转角度
int8_t Temp = Temp_buffer[11] & 0xFF;
motor->Temp = Temp;
motor->MError = Temp_buffer[12] & 0x7;

106
User/device/GO_M8010_6_Driver.h
View File

@ -8,104 +8,108 @@ extern "C"{
#include "usart.h"
#include "string.h"
#include <math.h>
#define GO_NUM 2
#define GO_NUM 3
/**
* @brief <EFBFBD><EFBFBD><EFBFBD>ģʽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ
* @brief 电机模式状态信息
*
*/
typedef struct
{
uint8_t id :4; // <EFBFBD><EFBFBD><EFBFBD>ID: 0,1...,14 15<31><35>ʾ<EFBFBD><CABE><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD><D0B5><EFBFBD>㲥<EFBFBD><E3B2A5><EFBFBD><EFBFBD>(<28><>ʱ<EFBFBD>޷<EFBFBD><DEB7><EFBFBD>)
uint8_t status :3; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ: 0.<2E><><EFBFBD><EFBFBD> 1.FOC<4F>ջ<EFBFBD> 2.<2E><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У׼ 3.<2E><><EFBFBD><EFBFBD>
uint8_t none :1; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ
} RIS_Mode_t /*__attribute__((packed))*/; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ 1Byte
uint8_t id :4; // 电机ID: 0,1...,14 15表示广播模式(暂时无法用)
uint8_t status :3; // 工作模式: 0.停止 1.FOC校准 2.编码器校准 3.运行
uint8_t none :1; // 保留位
} RIS_Mode_t /*__attribute__((packed))*/; // 电机模式 1Byte
/**
* @brief <EFBFBD><EFBFBD><EFBFBD>״̬<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ
* @brief 电机状态控制信息
*
*/
typedef struct
{
int16_t tor_des; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ؽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ť<EFBFBD><EFBFBD> unit: N.m (q8)
int16_t spd_des; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ؽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD> unit: rad/s (q7)
int32_t pos_des; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ؽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD> unit: rad (q15)
uint16_t k_pos; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ؽڸն<EFBFBD>ϵ<EFBFBD><EFBFBD> unit: 0.0-1.0 (q15)
uint16_t k_spd; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ؽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><EFBFBD> unit: 0.0-1.0 (q15)
int16_t tor_des; // 电机目标力矩 unit: N.m (q8)
int16_t spd_des; // 电机目标速度 unit: rad/s (q7)
int32_t pos_des; // 电机目标位置 unit: rad (q15)
uint16_t k_pos; // 电机位置环系数 unit: 0.0-1.0 (q15)
uint16_t k_spd; // 电机速度环系数 unit: 0.0-1.0 (q15)
} RIS_Comd_t; // <EFBFBD><EFBFBD><EFBFBD>Ʋ<EFBFBD><EFBFBD><EFBFBD> 12Byte
} RIS_Comd_t; // 控制命令 12Byte
/**
* @brief <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݰ<EFBFBD><EFBFBD><EFBFBD>ʽ
* @brief 电机控制数据包格式
*
*/
typedef struct
{
uint8_t head[2]; // <EFBFBD><EFBFBD>ͷ 2Byte
RIS_Mode_t mode; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ 1Byte
RIS_Comd_t comd; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 12Byte
uint16_t CRC16; // CRC 2Byte
uint8_t head[2]; // 包头 2Byte
RIS_Mode_t mode; // 电机工作模式 1Byte
RIS_Comd_t comd; // 电机控制命令 12Byte
uint16_t CRC16; // CRC校验 2Byte
} ControlData_t; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 17Byte
} ControlData_t; // 电机控制数据包 17Byte
/**
* @brief 电机状态信息结构体
*
*/
typedef struct {
unsigned short id; // <20><><EFBFBD>ID<49><44>0xBB<42><42><EFBFBD><EFBFBD>ȫ<EFBFBD><C8AB><EFBFBD><EFBFBD><EFBFBD> why<68><79><EFBFBD><EFBFBD><EFBFBD><EFBFBD>f<EFBFBD><66><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD><D0B5><EFBFBD><EFBFBD>
unsigned short mode; // 0:<3A><><EFBFBD><EFBFBD>, 5:<3A><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>, 10:<3A>ջ<EFBFBD>FOC<4F><43><EFBFBD><EFBFBD> why<68><79><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1<EFBFBD>ջ<EFBFBD><D5BB><EFBFBD>2У׼<D0A3><D7BC>
uint16_t correct; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD> <20><>1 <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
int MError; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 0.<2E><><EFBFBD><EFBFBD> 1.<2E><><EFBFBD><EFBFBD> 2.<2E><><EFBFBD><EFBFBD> 3.<2E><>ѹ 4.<2E><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
int Temp; // <EFBFBD><EFBFBD>
float tar_pos; // target position
float tar_w; // target speed
float T; // <EFBFBD><EFBFBD>ǰʵ<EFBFBD>ʵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float W; // <EFBFBD><EFBFBD>ǰʵ<EFBFBD>ʵ<EFBFBD><EFBFBD><EFBFBD>ٶȣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>٣<EFBFBD>
float Pos; // <EFBFBD><EFBFBD>ǰ<EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>
int footForce; // They dont even know what 7 is this so we dont update this
uint8_t buffer[17];
uint8_t Rec_buffer[16];
ControlData_t motor_send_data;
}GO_Motorfield;
unsigned short id; // 电机ID, 0xBB表示广播
unsigned short mode; // 工作模式: 0.停止 5.正转 10.FOC控制
uint16_t correct; // 校正状态: 1.校正完成 0.未校正
int MError; // 电机错误: 0.正常 1.过流 2.过载 3.欠压 4.过温
int Temp; // 温度
float tar_pos; // 目标位置
float tar_w; // 目标速度
float T; // 当前实际力矩
float W; // 当前实际速度 (单位: rad/s)
float Pos; // 当前实际位置
int footForce; // 预留字段, 暂未使用
uint8_t buffer[17]; // 数据发送缓冲区
uint8_t Rec_buffer[16]; // 数据接收缓冲区
ControlData_t motor_send_data; // 电机控制数据结构
} GO_Motorfield;
/**
*@brief <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>
*@brief 电机初始化
*/
void GO_M8010_init(void);
/**
*@brief go<EFBFBD><EFBFBD><EFBFBD>Ϳ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
*@input[in] huart <EFBFBD><EFBFBD><EFBFBD>ô<EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><EFBFBD>
*@input[in] id <EFBFBD><EFBFBD><EFBFBD>ص<EFBFBD><EFBFBD>id
*@input[in] rev <EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>֪<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɶ<EFBFBD>õģ<EFBFBD>githubΪ<EFBFBD>ش<EFBFBD>issue
*@input[in] T <EFBFBD><EFBFBD><EFBFBD>أ<EFBFBD><EFBFBD><EFBFBD>λN<EFBFBD><EFBFBD>m
*@input[in] Pos Ŀ<EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD><EFBFBD><EFBFBD>λrad
*@input[in] W Ŀ<EFBFBD><EFBFBD><EFBFBD>ٶȣ<EFBFBD><EFBFBD><EFBFBD>λrad/s
*@input[in] K_P λ<EFBFBD>û<EFBFBD><EFBFBD><EFBFBD>kp
*@input[in] K_W <EFBFBD>ٶȻ<EFBFBD>kp
*@note <EFBFBD><EFBFBD><EFBFBD>ƹ<EFBFBD>ʽ <EFBFBD><EFBFBD> t = T + <EFBFBD><EFBFBD><EFBFBD>趨λ<EFBFBD><EFBFBD> - ʵ<EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD>*K_P + <EFBFBD><EFBFBD><EFBFBD>趨<EFBFBD>ٶ<EFBFBD> - ʵ<EFBFBD><EFBFBD><EFBFBD>ٶȣ<EFBFBD>*K_W
*@brief go发送控制命令
*@input[in] huart 所用串口指针
*@input[in] id 被控电机id
*@input[in] rev 暂时不知道干啥用的，github为回答issue
*@input[in] T 力矩，单位N·m
*@input[in] Pos 目标位置，单位rad
*@input[in] W 目标速度，单位rad/s
*@input[in] K_P 位置环kp
*@input[in] K_W 速度环kp
*@note 控制公式 ： t = T + （设定位置 - 实际位置）*K_P + （设定速度 - 实际速度）*K_W
*/
void GO_M8010_send_data(UART_HandleTypeDef *huart,int id, int rev,float T,float W,float Pos,float K_P,float K_W);
void GO_M8010_send_data(UART_HandleTypeDef *huart,int id, int rev,float T,float Pos,float W,float K_P,float K_W);
/**
*@brief <EFBFBD>û<EFBFBD><EFBFBD>Զ<EFBFBD><EFBFBD>崮<EFBFBD>ڷ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>жϻص<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
*@brief 用户自定义挂载发送完成中断回调函数
*/
void uartTxCB(UART_HandleTypeDef *huart);
//<EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>Ͽ<EFBFBD><EFBFBD><EFBFBD>
// 基础力控函数
void basic_ForceControl (UART_HandleTypeDef *huart, int id, float bias, float length, float mass,
float tar_pos, float tar_w, float K_P,float K_W);
/**
*@brief go<EFBFBD><EFBFBD><EFBFBD>Ϳ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
*@input[in] id <EFBFBD><EFBFBD><EFBFBD>ص<EFBFBD><EFBFBD>id
*@revtal <EFBFBD><EFBFBD><EFBFBD>ص<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><EFBFBD>
*@brief go获取电机指针
*@input[in] id 电机的id
*@revtal 返回电机的指针
*/
GO_Motorfield* getGoPoint(uint8_t id);

200
User/device/djiMotor.c
View File

@ -22,6 +22,7 @@
#include "TopDefine.h"
#include<cmsis_os2.h>
#include "FreeRTOS.h"
#include <math.h>
#include "odrive_can.h"
@ -37,7 +38,7 @@
(ptr)->temperate = (data)[6]; \
(ptr)->ecd - (ptr)->last_ecd > 4096 ? ((ptr)->round_cnt--) : ((ptr)->round_cnt=(ptr)->round_cnt); \
(ptr)->ecd - (ptr)->last_ecd < -4096 ? ((ptr)->round_cnt++) : ((ptr)->round_cnt=(ptr)->round_cnt); \
(ptr)->total_angle = (fp64)((ptr)->round_cnt * 8192 + (ptr)->ecd - (ptr)->offset_ecd ) * MOTOR_ECD_TO_ANGLE; \
(ptr)->total_angle = (fp64)((ptr)->round_cnt * 8192 + (ptr)->ecd - (ptr)->offset_ecd ) * MOTOR_ECD_TO_ANGLE_3508; \
}//ptr指向电机测量数据结构体的指针，data包含电机测量数据的数组.
/*(ptr)->real_angle = (ptr)->real_angle % 360; */
#define get_motor_offset(ptr, data) \
@ -55,6 +56,17 @@
(ptr)->ecd - (ptr)->last_ecd > 4096 ? ((ptr)->round_cnt--) : ((ptr)->round_cnt=(ptr)->round_cnt); \
(ptr)->ecd - (ptr)->last_ecd < -4096 ? ((ptr)->round_cnt++) : ((ptr)->round_cnt=(ptr)->round_cnt); \
(ptr)->total_angle = (fp64)((ptr)->round_cnt * 8192 + (ptr)->ecd - (ptr)->offset_ecd ) * MOTOR_ECD_TO_ANGLE_6020; \
}
#define get_2006_motor_measure(ptr, data) \
{ \
(ptr)->last_ecd = (ptr)->ecd; \
(ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]); \
(ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]); \
(ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]); \
(ptr)->temperate = (data)[6]; \
(ptr)->ecd - (ptr)->last_ecd > 4096 ? ((ptr)->round_cnt--) : ((ptr)->round_cnt=(ptr)->round_cnt); \
(ptr)->ecd - (ptr)->last_ecd < -4096 ? ((ptr)->round_cnt++) : ((ptr)->round_cnt=(ptr)->round_cnt); \
(ptr)->total_angle = (fp64)((ptr)->round_cnt * 8192 + (ptr)->ecd - (ptr)->offset_ecd ) * MOTOR_ECD_TO_ANGLE_2006; \
}
// 解析出初始编码器值
// #define get_5065motor_measure(ptr, data) \
@ -67,7 +79,12 @@
#if DEBUG == 1
//电机回传数据结构体
motor_measure_t motor_chassis[5];
motor_measure_t GM6020_motor[2];
motor_measure_t motor_2006[2];
CAN_MotorFeedback_t motor_5065[2];
#else
static motor_measure_t motor_chassis[5];
#endif
@ -81,6 +98,41 @@ static uint8_t can_send_data_200[8];
CAN_RxHeaderTypeDef dji_rx_header;
uint8_t dji_rx_data[8];
static CAN_TxHeaderTypeDef vesc_tx_message;
static uint8_t vesc_send_data[4];
/**
* @brief 解析vesc电机反馈数据
* @param[in,out] feedback : Vesc电机反馈数据结构体指针
* @param[in] raw : Vesc电机反馈数据的原始数据
* @retval none
*/
static void CAN_VescMotor_Decode_1(CAN_MotorFeedback_t *feedback,
const uint8_t *raw)
{
if (feedback == NULL || raw == NULL) return;
union
{
int x;
uint8_t data[4];
}speed;
speed.data[0]= raw[0];
speed.data[1]= raw[1];
speed.data[2]= raw[2];
speed.data[3]= raw[3];
feedback->rotor_speed = speed.x;
union
{
int16_t y;
uint8_t dat[2];
}current;
current.dat[0]= raw[5];
current.dat[1]= raw[4];
feedback->torque_current =(fp32)current.y/10;
}
/**
* @brief 数据处理函数
* @param[in] none
@ -90,13 +142,10 @@ void djiMotorEncode()
{
switch (dji_rx_header.StdId)
{
case 0x201:
case 0x202:
case 0x203:
case 0x204:
case 0x205:
case 0x206:
case 0x207:
case M3508_1:
case M3508_2:
case M3508_3:
case M3508_4:
{
static uint8_t i = 0;
//get motor id
@ -110,6 +159,36 @@ void djiMotorEncode()
}
break;
}
case M2006_1:
case M2006_2:
{
static uint8_t i = 0;
//get motor id
i = dji_rx_header.StdId - 0x205;
if(motor_2006[i].msg_cnt<=50)
{
motor_2006[i].msg_cnt++;
get_motor_offset(&motor_2006[i], dji_rx_data);
}else{
get_2006_motor_measure(&motor_2006[i], dji_rx_data);
}
break;
}
case GM6020_1:
case GM6020_2:
{
static uint8_t i = 0;
//get motor id
i = dji_rx_header.StdId - 0x207;
if(GM6020_motor[i].msg_cnt<=50)
{
GM6020_motor[i].msg_cnt++;
get_motor_offset(&GM6020_motor[i], dji_rx_data);
}else{
get_6020_motor_measure(&GM6020_motor[i], dji_rx_data);
}
break;
}
default:
@ -118,6 +197,26 @@ void djiMotorEncode()
}
}
}
void vescMotorEncode()
{
switch (dji_rx_header.ExtId) {
case CAN_VESC5065_M1_MSG1:
// 存储消息到对应的电机结构体中
CAN_VescMotor_Decode_1(&motor_5065[0], dji_rx_data);
break;
case CAN_VESC5065_M2_MSG1:
// 存储消息到对应的电机结构体中
CAN_VescMotor_Decode_1(&motor_5065[1], dji_rx_data);
break;
default:
break;
}
}
#if FREERTOS_DJI == 0
/**
* @brief hal库CAN回调函数,接收电机数据
@ -132,7 +231,7 @@ void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
}
#else
static osEventFlagsId_t eventReceive;
static osEventFlagsId_t eventReceive;//事件标志
/**
* @brief 自定义大疆电机回调函数
* @param[in] none
@ -221,7 +320,7 @@ void CAN_cmd_200(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4,
}
/**
* @brief 发送电机控制电流(0x205,0x206,0x207,0x208)
* @brief 发送电机控制电流(0x209,0x20A,0x20B)
* @param[in] motor1: (0x209) 电机控制电流
* @param[in] motor2: (0x20A) 电机控制电流
* @param[in] motor3: (0x20B) 电机控制电流
@ -230,7 +329,7 @@ void CAN_cmd_200(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4,
void CAN_cmd_2FF(int16_t motor1, int16_t motor2, int16_t motor3, CAN_HandleTypeDef*hcan)
{
uint32_t send_mail_box;
tx_meessage_2ff.StdId = 0x1FF;
tx_meessage_2ff.StdId = 0x2FF;
tx_meessage_2ff.IDE = CAN_ID_STD;
tx_meessage_2ff.RTR = CAN_RTR_DATA;
tx_meessage_2ff.DLC = 0x08;
@ -254,4 +353,83 @@ motor_measure_t *get_motor_point(uint8_t i)
{
return &motor_chassis[i];
}
/**
* @brief 返回GM6020电机数据指针
* @param[in] i: 电机编号
* @retval 电机数据指针
*/
motor_measure_t *get_6020_motor_point(uint8_t i)
{
return &GM6020_motor[i];
}
/**
* @brief 返回M2006电机数据指针
* @param[in] i: 电机编号
* @retval 电机数据指针
*/
motor_measure_t *get_2006_motor_point(uint8_t i)
{
return &motor_2006[i];
}
/**
* @brief 限制vesc电机转速
* @param[in/out] rpm: vesce电机转速
* @retval none
*
* 如果rpm的绝对值大于wtrcfg_VESC_COMMAND_ERPM_MAX,
* 则将rpm的值设置为wtrcfg_VESC_COMMAND_ERPM_MAX或-wtrcfg_VESC_COMMAND_ERPM_MAX
*/
void assert_param_rpm(float *rpm){
if( fabsf(*rpm) > wtrcfg_VESC_COMMAND_ERPM_MAX )
*rpm = *rpm > 0 ? wtrcfg_VESC_COMMAND_ERPM_MAX : - wtrcfg_VESC_COMMAND_ERPM_MAX ;
}
/**
* @brief 使vesc电机进入转速模式
* @param[in] controller_id: vesc电机控制器id
* @param[in] RPM: 电机转速
* @retval RPM（1000-50000之间的数）
*/
void CAN_VESC_RPM(uint8_t controller_id, float RPM)
{
uint32_t id;
int32_t data;
uint32_t send_mail_box;
id = controller_id | ((uint32_t)CAN_PACKET_SET_RPM << 8);
assert_param_rpm(&RPM);
data = (int32_t)(RPM);
vesc_tx_message.ExtId = id;
vesc_tx_message.IDE = CAN_ID_EXT;
vesc_tx_message.RTR = CAN_RTR_DATA;
vesc_tx_message.DLC = 0x04;
vesc_send_data[0] = data >> 24 ;
vesc_send_data[1] = data >> 16 ;
vesc_send_data[2] = data >> 8 ;
vesc_send_data[3] = data ;
HAL_CAN_AddTxMessage(&hcan2, &vesc_tx_message, vesc_send_data, &send_mail_box);
}
/**
* @brief 使vesc电机进入制动模式
* @param[in] controller_id: vesc电机控制器id
* @retval none
*/
void CAN_VESC_HEAD(uint8_t controller_id)
{
uint32_t id;
uint32_t send_mail_box;
id = controller_id | ((uint32_t)CAN_PACKET_SET_CURRENT_BRAKE << 8);
vesc_tx_message.ExtId = id;
vesc_tx_message.IDE = CAN_ID_EXT;
vesc_tx_message.RTR = CAN_RTR_DATA;
vesc_tx_message.DLC = 0x04;
HAL_CAN_AddTxMessage(&hcan2, &vesc_tx_message, vesc_send_data, &send_mail_box);
}

61
User/device/djiMotor.h
View File

@ -16,6 +16,18 @@ typedef enum{
M2006 = 2
}motor_type_e;
enum{
M3508_1 = 0x201,
M3508_2 = 0x202,
M3508_3 = 0x203,
M3508_4 = 0x204,
M2006_1 = 0x205,
M2006_2 = 0x206,
GM6020_1 = 0x207,
GM6020_2 = 0x208,
CAN_VESC5065_M1_MSG1 =0x1001, //vesc的数据回传使用了扩展id，[0:7]为驱动器id，[8:15]为帧类型
CAN_VESC5065_M2_MSG1 =0x1002,
};
//rm motor data
typedef struct
@ -45,12 +57,40 @@ typedef struct
} odrive_measure_t;
typedef enum {
CAN_PACKET_SET_DUTY = 0,
CAN_PACKET_SET_CURRENT = 1,
CAN_PACKET_SET_CURRENT_BRAKE =2,
CAN_PACKET_SET_RPM = 3,
CAN_PACKET_SET_POS = 4,
CAN_PACKET_FILL_RX_BUFFER = 5,
CAN_PACKET_FILL_RX_BUFFER_LONG = 6,
CAN_PACKET_PROCESS_RX_BUFFER = 7,
CAN_PACKET_PROCESS_SHORT_BUFFER = 8,
CAN_PACKET_STATUS = 9,
CAN_PACKET_SET_CURRENT_REL = 10,
CAN_PACKET_SET_CURRENT_BRAKE_REL = 11,
CAN_PACKET_SET_CURRENT_HANDBRAKE = 12,
CAN_PACKET_SET_CURRENT_HANDBRAKE_REL = 13
} CAN_PACKET_ID;
//motor calc ecd to angle
#define MOTOR_ECD_TO_ANGLE (360.0 / 8191.0 / (3591.0/187.0))
/* 电机反馈信息 */
typedef struct {
float rotor_ecd;
float rotor_speed;
float torque_current;
float temp;
} CAN_MotorFeedback_t;
#define MOTOR_ECD_TO_ANGLE_3508 (360.0 / 8191.0 / (3591.0/187.0))
#define MOTOR_ECD_TO_ANGLE_2006 (360.0 / 8191.0 / 36)
#define MOTOR_ECD_TO_RAD 0.000766990394f
#define MOTOR_ECD_TO_ANGLE_6020 (360.0 / 8191.0 )
#define wtrcfg_VESC_COMMAND_ERPM_MAX 40000
#define CAN_VESC_CTRL_ID_BASE (0x300)
#if FREERTOS_DJI == 1
/**
@ -101,6 +141,19 @@ extern void CAN_cmd_2FF(int16_t motor1, int16_t motor2, int16_t motor3, CAN_Hand
*/
extern motor_measure_t *get_motor_point(uint8_t i);
/**
* @brief 返回GM6020电机数据指针
* @param[in] i: 电机编号
* @retval 电机数据指针
*/
motor_measure_t *get_6020_motor_point(uint8_t i);
/**
* @brief 返回M2006电机数据指针
* @param[in] i: 电机编号
* @retval 电机数据指针
*/
motor_measure_t *get_2006_motor_point(uint8_t i);
/**
* @brief 数据处理函数
@ -109,6 +162,12 @@ extern motor_measure_t *get_motor_point(uint8_t i);
*/
void djiMotorEncode(void);
void vescMotorEncode(void);
void CAN_VESC_RPM(uint8_t controller_id, float RPM);
void CAN_VESC_HEAD(uint8_t controller_id);
#ifdef __cplusplus
}
#endif

81
User/device/nuc.c
View File

@ -1,6 +1,6 @@
#include "nuc.h"
#include <string.h>
//#include "crc16.h"
#include "crc_ccitt.h"
uint8_t nucbuf[32];
@ -32,59 +32,76 @@ int8_t NUC_Restart(void) {
return DEVICE_OK;
}
bool_t NUC_WaitDmaCplt(void) {
// return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll,500) ==
// SIGNAL_NUC_RAW_REDY);
return 1;
}
int8_t NUC_RawParse(NUC_t *n){
if(n ==NULL) return DEVICE_ERR_NULL;
union
{
int8_t NUC_RawParse(NUC_t *n) {
if (n == NULL) return DEVICE_ERR_NULL;
union {
float x[3];
char data[12];
}instance;//方便在浮点数和字符数组之间进行数据转换
} instance; // 方便在浮点数和字符数组之间进行数据转换
if(nucbuf[0]!=HEAD) goto error; //发送ID不是底盘
else{
n->status_fromnuc =nucbuf[1];
n->ctrl_status =nucbuf[2];
switch (n->status_fromnuc)
{
// 校验数据包头
if (nucbuf[0] != HEAD) {
return DEVICE_ERR;
}
// 提取数据包中的 CRC 值（假设 CRC 值在数据包的最后两个字节）
uint16_t received_crc = (nucbuf[16] << 8) | nucbuf[17]; // 假设 CRC 在第 16 和 17 字节
uint16_t calculated_crc = do_crc_table(nucbuf, 16); // 计算前 16 字节的 CRC
// 校验 CRC
if (received_crc == calculated_crc) {
n->status_fromnuc = nucbuf[1];
n->ctrl_status = nucbuf[2];
switch (n->status_fromnuc) {
case VISION:
/* 协议格式
0xFF HEAD
0x0X 控制帧
0x02 控制帧
0x01 相机帧
x fp32
y fp32
z fp32
0xFE TAIL
*/
if(nucbuf[15]!=TAIL)goto error;
instance.data[3] = nucbuf[6];
instance.data[2] = nucbuf[5];
instance.data[1] = nucbuf[4];
instance.data[0] = nucbuf[3];
n->vision.x= instance.x[0]; //
instance.data[7] = nucbuf[10];
instance.data[6] = nucbuf[9];
instance.data[5] = nucbuf[8];
instance.data[4] = nucbuf[7];
n->vision.y = instance.x[1];//
instance.data[11] = nucbuf[14];
instance.data[10] = nucbuf[13];
instance.data[9] = nucbuf[12];
instance.data[8] = nucbuf[11];
n->vision.z = instance.x[2];//
break;
case PICK:
if (nucbuf[15] != TAIL) goto error;
instance.data[3] = nucbuf[3];
instance.data[2] = nucbuf[4];
instance.data[1] = nucbuf[5];
instance.data[0] = nucbuf[6];
n->vision.x = instance.x[0];
instance.data[7] = nucbuf[7];
instance.data[6] = nucbuf[8];
instance.data[5] = nucbuf[9];
instance.data[4] = nucbuf[10];
n->vision.y = instance.x[1];
instance.data[11] = nucbuf[11];
instance.data[10] = nucbuf[12];
instance.data[9] = nucbuf[13];
instance.data[8] = nucbuf[14];
n->vision.z = instance.x[2];
break;
}
return DEVICE_OK;
}
error:
else
{
return DEVICE_ERR;
}
error:
return DEVICE_ERR;
}

8
User/device/nuc.h
View File

@ -13,7 +13,7 @@
#define ODOM (0x04)
#define PICK (0x06)
#define VISION (0x01)
#define VISION (0x09)
//写结构体存入视觉信息
@ -27,9 +27,9 @@ typedef struct{
struct
{
float x;
float y;
float z;
fp32 x;
fp32 y;
fp32 z;
}vision;
}NUC_t;

2
User/device/remote_control.c
View File

@ -202,6 +202,8 @@ void RC_init(uint8_t *rx1_buf, uint8_t *rx2_buf, uint16_t dma_buf_num)
#else
//DMA双缓冲区+串口空闲中断
// 02 53 44 1x1 3x2
static osEventFlagsId_t eventReceive;
RC_mess_t RC_mess;

86
User/module/ball.c
View File

@ -3,84 +3,82 @@
#include "cmsis_os.h"
#include "FreeRTOS.h"
#include "bsp_delay.h"
#include "go.h"
// PC6 ball_up_Pin读运球 检测到输出0,未检测到输出1
// PE13 down_Pin 下推
// PE14 paw_Pin 爪子
// PI6 ball_in2_Pin 读接球
// PI7 ball_in1_Pin 读接球
// PE11 key_Pin 收网
#define ballcome 1
#define balldown 0
extern GO go;
//光电识别
int ball_in(void)
{
if (HAL_GPIO_ReadPin(ball_up_GPIO_Port, ball_up_Pin) == GPIO_PIN_SET)
{
return ballcome;
}
else
{
return balldown;
}
return (HAL_GPIO_ReadPin(ball_up_GPIO_Port, ball_up_Pin));
}
int pass_ball(void)
{
if (HAL_GPIO_ReadPin(ball_in1_GPIO_Port, ball_in1_Pin) == GPIO_PIN_SET)
{
return ballcome;
}
else
{
return balldown;
}
return (HAL_GPIO_ReadPin(ball_in1_GPIO_Port, ball_in1_Pin));
}
//运球
void handling_ball(int hand, int paw)
void handling_ball(int hand, int angle)
{
paw =ball_in();
if (hand == 1 && paw == 1)
{
//爪子
delay_us(100);
HAL_GPIO_WritePin(paw_GPIO_Port, paw_Pin, GPIO_PIN_RESET);
osDelay(2);
delay_us(100);
//上推
HAL_GPIO_WritePin(down_GPIO_Port, down_Pin, GPIO_PIN_RESET);
hand=0;
}
else
{
//爪子
int ball_state =0;//有球
int paw_state =0;//获取爪子开合状态
if( hand==1)
{
HAL_GPIO_WritePin(paw_GPIO_Port, paw_Pin, GPIO_PIN_SET);
osDelay(2);
delay_us(100);
//下推
HAL_GPIO_WritePin(down_GPIO_Port, down_Pin, GPIO_PIN_SET);
paw_state =1;//爪子开
// osDelay(5);
HAL_GPIO_WritePin(down_GPIO_Port, down_Pin, GPIO_PIN_RESET);//打出
osDelay(500);
angle=80;
GO_TURN_ball(TURN,go,angle);//停止运球
ball_state =1;//无球
osDelay(500);
HAL_GPIO_WritePin(down_GPIO_Port, down_Pin, GPIO_PIN_SET);//收回
ball_state =ball_in();//获取爪子有无球状态
}
else if (hand==0)
{
angle=20;
GO_TURN_ball(TURN,go,angle);//停止运球
}
if (paw_state==1&&ball_state==0)
{
HAL_GPIO_WritePin(paw_GPIO_Port, paw_Pin, GPIO_PIN_RESET);//爪子合
osDelay(500);
ball_state=1;//有球清除标志
paw_state=0;//爪子合清除标志
hand=0;
}
}
//接球
int catch_ball(int inball)
{
inball =pass_ball();
if(inball==1)
{
//接球
HAL_GPIO_WritePin(key_GPIO_Port, key_Pin, GPIO_PIN_SET);
return ballcome;
}
else
{
HAL_GPIO_WritePin(key_GPIO_Port, key_Pin, GPIO_PIN_RESET);
return balldown;
}

2
User/module/ball.h
View File

@ -12,7 +12,7 @@ typedef struct ball
int ball_in(void);
int pass_ball(void);
void handling_ball(int hand, int paw);
void handling_ball(int hand, int angle);
int catch_ball(int inball);

72
User/module/dji.c
View File

@ -15,50 +15,57 @@ pid_type_def speed_pid;//3508速度环pid结构体
pid_type_def angle_pid;//3508位置环pid结构体
//pid
//fp32 M3508_speed_PID[3]={10.0,0.3,0.0}; //P,I,D(速度环)
//fp32 M3508_angle_PID[3]={10.0,0.0,1.5}; //P,I,D(角度环)
fp32 M3508_speed_PID[3]={5.0,0.3,0.0}; //P,I,D(速度环)
fp32 M3508_angle_PID[3]={25.0,0.0,1.5}; //P,I,D(角度环)
fp32 M3508_speed_PID[3]={50.0,0.3,0.0}; //P,I,D(速度环)
fp32 M3508_angle_PID[3]={50.0,0.0,1.5}; //P,I,D(角度环)
//速度环pid参数
fp32 M3508_PID[3]={4.9,0.01,0.0};
int16_t result; //速度环
float angleSet[MOTOR_NUM]; //位置环
//trigger
//编码器数据
const motor_measure_t *trigger_motor_data;//3508电机数据
pid_type_def t_speed_pid;//2006速度环pid结构体
pid_type_def t_angle_pid;//2006位置环pid结构体
fp32 M2006_speed_PID[3]={5.0,0.3,0.0}; //P,I,D(速度环)
fp32 M2006_angle_PID[3]={25.0,0.0,1.5}; //P,I,D(角度环)
fp32 M2006_speed_PID[3]={10.0,0.1,0.0}; //P,I,D(速度环)
fp32 M2006_angle_PID[3]={5.0,0.0,0.05}; //P,I,D(角度环)
//速度环pid参数
fp32 M2006_PID[3]={4.9,0.01,0.0};
int16_t t_result; //速度环 //can最后发送的数据
float t_angleSet[1]; //位置环
//GM6020电机数据
const motor_measure_t *GM6020_motor_data;//3508电机数据
//6020pid
pid_type_def speed_6020_pid;
pid_type_def angle_6020_pid;
const fp32 PID_6020_angle[3] = {50, 0.1, 0};
const fp32 PID_6020_speed[3]={5,0.01,0};
void motor_init(void)
{
// motor_3508_data=get_motor_point(0);
trigger_motor_data=get_motor_point(0);
// PID_init(&speed_pid,PID_POSITION,M3508_speed_PID,3000,1000);
// PID_init(&angle_pid,PID_POSITION,M3508_angle_PID,7000,2000);
trigger_motor_data=get_2006_motor_point(0);
GM6020_motor_data=get_6020_motor_point(0);
// PID_init(&speed_pid,PID_POSITION,M3508_speed_PID,500000,500000);
// PID_init(&angle_pid,PID_POSITION,M3508_angle_PID,500000,100000);
PID_init(&t_speed_pid,PID_POSITION,M2006_speed_PID,500000,500000);
PID_init(&t_angle_pid,PID_POSITION,M2006_angle_PID,500000,100000);
PID_init(&t_speed_pid,PID_POSITION,M2006_speed_PID,1600,600);
PID_init(&t_angle_pid,PID_POSITION,M2006_angle_PID,500,200);
PID_init(&speed_6020_pid,PID_POSITION,PID_6020_speed,16000, 6000);//6020 pid初始化
PID_init(&angle_6020_pid,PID_POSITION,PID_6020_angle,5000, 2000);
t_result=0;
}
float trigger_angle = 0;
void trigger_pos(fp32 angle)
{
@ -67,22 +74,29 @@ void trigger_pos(fp32 angle)
delta[0]=PID_calc(&t_angle_pid,trigger_motor_data->total_angle,angle);
t_result=PID_calc(&t_speed_pid,trigger_motor_data->speed_rpm,delta[0]);
}
void motor_speed(fp32 speed)
//6020控制
void my_GM6020_control(fp32 angle)
{
result=PID_calc(&speed_pid,motor_3508_data->speed_rpm,speed);
}
void motor_pos(fp32 angle)
{
int16_t delta[1];
delta[0]=PID_calc(&angle_pid,motor_3508_data->total_angle,angle);
result=PID_calc(&speed_pid,motor_3508_data->speed_rpm,delta[0]);
fp32 angle_get;
fp32 my_angle;
int32_t M6020_speed;
//6020串级
angle_get= angle;
//外环角度环
my_angle=PID_calc(&angle_6020_pid,GM6020_motor_data->total_angle,angle_get);//角度环
//内环速度环
M6020_speed=PID_calc(&speed_6020_pid,GM6020_motor_data->speed_rpm, my_angle);//速度环
CAN_cmd_1FF(M6020_speed, M6020_speed, M6020_speed, M6020_speed,&hcan1);
osDelay(2);
}

5
User/module/dji.h
View File

@ -15,10 +15,11 @@ typedef enum
}motorput_e;
//float t_angleSet[1]; //位置环
void motor_init(void);
void motor_speed(fp32 speed);
void motor_pos(fp32 angle);
void trigger_pos(fp32 angle);
void my_GM6020_control(fp32 angle);
#endif

29
User/module/go.c
View File

@ -4,7 +4,7 @@
#include "calc_lib.h"
#include "FreeRTOS.h"
#include <cmsis_os2.h>
#define KP 0.12
#define KP 0.9
#define KD 0.008
//可活动角度
#define ANGLE_ALLOW 0.3f
@ -13,7 +13,7 @@ extern RC_mess_t RC_mess;
GO go;
//初始化
void gimbalInit(void)
void GO_Init(void)
{
int i;
GO_M8010_init();
@ -23,11 +23,9 @@ void gimbalInit(void)
go.angleSetgo[i] = 0;
go.offestAngle[i] = 0;
// GO_M8010_send_data(&huart1, i,0,0,0,0,0,0);
GO_M8010_send_data(&huart6, i,0,0,0,0,0,0);
go.offestAngle[i] = go.goData[i]->Pos;
HAL_Delay(100);
// GO_M8010_send_data(&huart1, i,0,0,0,0,0,0);
GO_M8010_send_data(&huart6, i,0,0,0,0,0,0);
go.offestAngle[i] = go.goData[i]->Pos;
HAL_Delay(100);
@ -38,19 +36,30 @@ void gimbalInit(void)
void gimbalFlow(void)
{
//用485模块发送数据有问题
GO_M8010_send_data(&huart1, 0,0,0,go.angleSetgo[0],1,KP,KD);
go.angleSetgo[0]=1;
//串口6发送数据没有问题
GO_M8010_send_data(&huart6, 0,0,0,go.angleSetgo[0],1,KP,KD);
// GO_M8010_send_data(&huart6, 0,0,0,go.angleSetgo[0],1,KP,KD);
GO_M8010_send_data(&huart6, 1,0,0,go.angleSetgo[0],1,KP,KD);
// GO_M8010_send_data(&huart6, 2,0,0,go.angleSetgo[0],1,KP,KD);
osDelay(1);
}
void gimbalZero(void)
{
GO_M8010_send_data(&huart1, 0,0,0,0,0,0,0);
GO_M8010_send_data(&huart6, 0,0,0,0,0,0,0);
osDelay(1);
GO_M8010_send_data(&huart1, 1,0,0,0,0,0,0);
GO_M8010_send_data(&huart6, 1,0,0,0,0,0,0);
}
void GO_TURN_ball(GO_ID_t go_id,GO go_set,int angle)
{
go_set.angleSetgo[go_id] = angle;
go_set.Kp = KP;
go_set.Kd = KD;
GO_M8010_send_data(&huart6, go_id,0,0,go_set.angleSetgo[go_id],1,go_set.Kp,go_set.Kd);
}

9
User/module/go.h
View File

@ -3,6 +3,12 @@
#include "GO_M8010_6_Driver.h"
typedef enum{
GIMBAL = 0,
TURN = 1,
OTHER = 2
}GO_ID_t;
typedef struct
{
/* data */
@ -16,9 +22,10 @@ typedef struct
float allowRange;
}GO;
void gimbalInit(void);
void GO_Init(void);
void gimbalFlow(void);
void gimbalZero(void);
void GO_TURN_ball(GO_ID_t go_id,GO go_set,int angle);
#endif

3
User/module/led.c
View File

@ -5,9 +5,8 @@
void LED_red(void)
{
//GPIO_PIN_12
//HAL_GPIO_TogglePin(ledBlue_GPIO_Port, ledBlue_Pin);
HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_R_Pin, GPIO_PIN_SET);
osDelay(1000);
osDelay(500);
HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_R_Pin, GPIO_PIN_RESET);
}

111
User/module/shoot.c
View File

@ -1,92 +1,51 @@
#include "shoot.h"
#include "remote_control.h"
#include "go.h"
#include "calc_lib.h"
extern RC_mess_t RC_mess;
extern motor_measure_t *trigger_motor_data;//3508电机数据
extern GO go;
#define GO1_SHOOT 0
#define ODRIVE_SHOOT 1
int shoot_flag = 0;
int speed_5065=0;
#define KP 0.12
#define KD 0.008
#if GO1_SHOOT == 1
GO_SHOOT goshoot;
void shooterInit(void)
//sw[1] 306--1694
void control_shoot(void)
{
int i;
GO_M8010_init();
for(i = 0;i < GO_NUM;i ++)
{
goshoot.goData[i] = getGoPoint(i);//获取电机数据指针
goshoot.angleSetgo[i] = 0;
goshoot.offestAngle[i] = 0;
// GO_M8010_send_data(&huart1, i,0,0,0,0,0,0);
GO_M8010_send_data(&huart6, i,0,0,0,0,0,0);
goshoot.offestAngle[i] = goshoot.goData[i]->Pos;
HAL_Delay(100);
// GO_M8010_send_data(&huart1, i,0,0,0,0,0,0);
GO_M8010_send_data(&huart6, i,0,0,0,0,0,0);
goshoot.offestAngle[i] = goshoot.goData[i]->Pos;
HAL_Delay(100);
if(RC_mess.RC_data.sw[1]>800)
{
if(HAL_GPIO_ReadPin(ball_in1_GPIO_Port, ball_in1_Pin) == GPIO_PIN_RESET)
{
CAN_VESC_HEAD(1);
CAN_VESC_HEAD(2);
}
else
{
//speed_5065=5000;
speed_5065=map_fp32(RC_mess.RC_data.ch[2],0,100,0,25000);
CAN_VESC_RPM(1, speed_5065); //发射电机转速
CAN_VESC_RPM(2, speed_5065); //发射电机转速
}
}
if(shoot_flag == 2 || RC_mess.RC_data.sw[5]==1694)//返回//不拨动为306
{
speed_5065=-5000;
CAN_VESC_RPM(1, speed_5065); //发射电机转速
CAN_VESC_RPM(2, speed_5065); //发射电机转速
osDelay(2000);
speed_5065=0;
}
osDelay(1);
}
void shoot_ball(int key)
{
//蓄力
if(trigger_motor_data->real_angle ==60)//扳机已闭合
{
goshoot.angleSetgo[0] = 10;
GO_M8010_send_data(&huart6, 0,0,0,goshoot.angleSetgo[0],1,KP,KD);
}
if (key ==2)//上升准备蓄力
{
goshoot.angleSetgo[0] = 0;
GO_M8010_send_data(&huart6, 0,0,0,goshoot.angleSetgo[0],1,KP,KD);
}
}
#elif ODRIVE_SHOOT == 1
//初始位置在最上面, 有球放过来, 电机转动到最下面等待发射
//发射指令,当到最高点时,扳机离合
//返回扳机,远动到最下面
extern float angle_encoder;
extern volatile int32_t multi_turn_angle ;
extern volatile uint16_t last_angle ;
void shoot_odrive(int angle)
{
// if(ball)
// {
// if(multi_turn_angle==8)
// {
// //di
// odrive_accel_cmd(AXIS0_NODE,200,200);
// odrive_pos_cmd(AXIS0_NODE,0);
// }
// else if(multi_turn_angle==0)
// {
// }
// }
odrive_accel_cmd(AXIS0_NODE,200,200);
osDelay(2);
odrive_pos_cmd(AXIS0_NODE,angle);
}
#endif

5
User/module/shoot.h
View File

@ -20,8 +20,7 @@ typedef struct
float allowRange;
}GO_SHOOT;
void shooterInit(void);
void shoot_ball(int key);
void shoot_odrive(int angle);
void control_shoot(void);
#endif

34
User/task/ball_task.c
View File

@ -5,10 +5,15 @@
#include <cmsis_os2.h>
#include "remote_control.h"
#include "ball.h"
#include "go.h"
extern RC_mess_t RC_mess;
extern GO go;
BASKETBALL basketball;
int bb=0;
int Turn_pos=0;
void Task_Ball(void *argument)
{
(void)argument; /* 未使用argument，消除警告 */
@ -16,11 +21,36 @@ void Task_Ball(void *argument)
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计算 */
basketball.hand=0;
basketball.paw=0;
Turn_pos=20;
GO_TURN_ball(TURN,go,Turn_pos); //发射舵机位置
HAL_GPIO_WritePin(down_GPIO_Port, down_Pin, GPIO_PIN_SET);//收回
while(1)
{
handling_ball(basketball.hand, basketball.paw);
// Turn_pos=20;
if(RC_mess.RC_data.sw[0]==306)//加速
{
basketball.hand=1;
}
else if(RC_mess.RC_data.sw[0]==1694)//减速
{
basketball.hand=0;
}
handling_ball(basketball.hand,Turn_pos);
bb=ball_in();
if(bb==1)
{
// HAL_GPIO_WritePin(key_GPIO_Port, key_Pin, GPIO_PIN_SET);
// HAL_GPIO_WritePin(paw_GPIO_Port, paw_Pin, GPIO_PIN_SET);
// HAL_GPIO_WritePin(down_GPIO_Port, down_Pin, GPIO_PIN_SET);
}
else
{
// HAL_GPIO_WritePin(key_GPIO_Port, key_Pin, GPIO_PIN_RESET);
// HAL_GPIO_WritePin(paw_GPIO_Port, paw_Pin, GPIO_PIN_RESET);
// HAL_GPIO_WritePin(down_GPIO_Port, down_Pin, GPIO_PIN_RESET);
}
tick += delay_tick; /* 计算下一个唤醒时刻 */
osDelayUntil(tick); /* 运行结束，等待下一个周期唤醒 */

2
User/task/can_task.c
View File

@ -25,7 +25,7 @@ void Task_Can(void *argument)
{
waitNewDji();//等待新数据
djiMotorEncode();//数据解析
vescMotorEncode();//数据解析
//将can数据添加到消息队列

42
User/task/dji_task.c
View File

@ -5,22 +5,24 @@
#include <cmsis_os2.h>
#include "remote_control.h"
#include "dji.h"
#include "read_spi.h"
#include "vofa.h"
#include "buzzer.h"
extern RC_mess_t RC_mess;
extern int16_t result;
extern int16_t t_result;
extern motor_measure_t *motor_3508_data;
float vofa[8];
extern motor_measure_t *trigger_motor_data;
Encoder_t encoder;
int shoot_f=0;
int pos2006=0;
/**
* \brief 电机任务
*
* \param argument 未使用
*/
int speed=0;
float angle=0;
float m;
void Task_Motor(void *argument)
{
(void)argument; /* 未使用argument，消除警告 */
@ -28,25 +30,27 @@ void Task_Motor(void *argument)
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CTRL_CHASSIS;
motor_init();
Encoder_Init(&encoder);
uint32_t tick = osKernelGetTickCount();
while(1)
{
//收到消息队列新数据
//电机控制
//motor_speed(speed);
m=angle*(8191/360);
// motor_pos(m);
trigger_pos(m);
//CAN_cmd_200(result[MOTOR_UP],result[MOTOR_UP],result[MOTOR_UP],result[MOTOR_UP],&hcan1);
CAN_cmd_200(t_result,0,0,0,&hcan1);
osDelay(2);
if(RC_mess.RC_data.sw[4]==306||shoot_f==3)
{
pos2006=200;
// vofa[0]=motor_3508_data->speed_rpm;
// vofa[1]=speed;
// vofa_tx_main(vofa);
}
else
{
pos2006=0;
}
trigger_pos(pos2006);
CAN_cmd_1FF(t_result,0,0,0,&hcan1);
osDelay(1);
tick += delay_tick;
osDelayUntil(tick);

3
User/task/dji_task.h
View File

@ -2,4 +2,7 @@
#define _DJI_TASK_H_
#define THREE 3
#define DZ 0
#endif

9
User/task/go_task.c
View File

@ -16,15 +16,16 @@ void Task_Go(void *argument)
#if GOUSE==1
//HAL_Delay(2000);
// gimbalInit();
//HAL_GPIO_WritePin(ledGreen_GPIO_Port,ledGreen_Pin,GPIO_PIN_RESET);
GO_Init();
HAL_GPIO_WritePin(LED_G_GPIO_Port, LED_G_Pin,GPIO_PIN_RESET);
while(1)
{
LED_green();
//LED_green();
//LED_bule();
// gimbalFlow();
osDelay(1);
}
#else
while(1)

9
User/task/led_task.c
View File

@ -4,17 +4,16 @@
#include "main.h"
#include "led.h"
#include "buzzer.h"
int a1=0;
void Task_Led(void *argument) {
(void)argument; /* 未使用argument，消除警告 */
for (;;) {
while(1) {
// LED_red();
// LED_bule();
// see_you_again();
// LED_green();
LED_green();
osDelay(1);
// osDelay(500);
}
}

1
User/task/rc_task.c
View File

@ -7,6 +7,7 @@
#include "remote_control.h"
extern RC_mess_t RC_mess;
// sw[0]2 下306上1694 sw[5]3前306后1694 sw[4]4前1694后306 sw[1]xuan1 sw[3]xuan2
/**
* \brief 遥控器任务
*

38
User/task/shoot_task.c
View File

@ -5,41 +5,45 @@
#include <cmsis_os2.h>
#include "remote_control.h"
#include "shoot.h"
#include "odrive_shoot.h"
#include "read_spi.h"
#include "dji.h"
#include "go.h"
extern RC_mess_t RC_mess;
Encoder_t encoder;
double angles;
int32_t rounds;
int angleo = 0; //发射角度
#define GO1_SHOOT 0
#define ODRIVE_SHOOT 1
extern motor_measure_t *trigger_motor_data;//3508电机数据
extern GO go;
int gopos=0;
#define VESC_SHOOT 1
void Task_Shoot(void *argument)
{
(void)argument; /* 未使用argument，消除警告 */
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CAN;
Encoder_Init(&encoder);
#if GO1_SHOOT == 1
shooterInit();
#endif
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计算 */
//go.angleSetgo[0] = 0; //id暂时未知
GO_TURN_ball(TURN,go,gopos); //发射舵机位置
osDelay(500);
while(1)
{
#if GO1_SHOOT == 1
shoot_ball(0);
#if VESC_SHOOT == 1
gopos=35;//云台位置
GO_TURN_ball(GIMBAL,go,gopos); //锁云台
control_shoot();
#elif ODRIVE_SHOOT == 1
shootStep();
//shoot_odrive(angleo);
#endif
// 更新编码器数据
Update_Encoder(&encoder);
tick += delay_tick; /* 计算下一个唤醒时刻 */
osDelayUntil(tick); /* 运行结束，等待下一个周期唤醒 */
}