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 */ /* USER CODE END USART6_Init 1 */
huart6.Instance = USART6; huart6.Instance = USART6;
huart6.Init.BaudRate = 115200; huart6.Init.BaudRate = 4000000;
huart6.Init.WordLength = UART_WORDLENGTH_8B; huart6.Init.WordLength = UART_WORDLENGTH_8B;
huart6.Init.StopBits = UART_STOPBITS_1; huart6.Init.StopBits = UART_STOPBITS_1;
huart6.Init.Parity = UART_PARITY_NONE; 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/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", "shoot.h": "c",
"dji.h": "c", "dji.h": "c",
"calc_lib.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' *** Using Compiler 'V5.06 update 7 (build 960)', folder: 'D:\keil\ARM\ARMCC\Bin'
Build target 'mycode1' 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). "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> <Ww>
<count>11</count> <count>11</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>angleo,0x0A</ItemText> <ItemText>encoder,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>12</count> <count>12</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>shoot</ItemText> <ItemText>mode,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>13</count> <count>13</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>multi_turn_angle,0x0A</ItemText> <ItemText>m</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>14</count> <count>14</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>angle_encoder</ItemText> <ItemText>trigger_motor_data,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>15</count> <count>15</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>back_angle,0x0A</ItemText> <ItemText>trigger_angle_o</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>16</count> <count>16</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>encoder,0x0A</ItemText> <ItemText>shoot</ItemText>
</Ww>
<Ww>
<count>17</count>
<WinNumber>1</WinNumber>
<ItemText>angles</ItemText>
</Ww> </Ww>
</WatchWindow1> </WatchWindow1>
<WatchWindow2> <WatchWindow2>
@ -322,7 +317,7 @@
<Group> <Group>
<GroupName>Application/User/Core</GroupName> <GroupName>Application/User/Core</GroupName>
<tvExp>1</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel> <cbSel>0</cbSel>
<RteFlg>0</RteFlg> <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.FilterMaskIdHigh = 0x0000;
can_filter_st.FilterMaskIdLow = 0x0000; can_filter_st.FilterMaskIdLow = 0x0000;
can_filter_st.FilterBank = 0; can_filter_st.FilterBank = 0;
can_filter_st.SlaveStartFilterBank = 14;
can_filter_st.FilterFIFOAssignment = CAN_RX_FIFO0; can_filter_st.FilterFIFOAssignment = CAN_RX_FIFO0;
HAL_CAN_ConfigFilter(&hcan1, &can_filter_st); HAL_CAN_ConfigFilter(&hcan1, &can_filter_st);
HAL_CAN_Start(&hcan1); HAL_CAN_Start(&hcan1);
HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING); HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING);
can_filter_st.SlaveStartFilterBank = 14;
can_filter_st.FilterBank = 14; can_filter_st.FilterBank = 14;
can_filter_st.FilterFIFOAssignment = CAN_RX_FIFO1;
HAL_CAN_ConfigFilter(&hcan2, &can_filter_st); HAL_CAN_ConfigFilter(&hcan2, &can_filter_st);
HAL_CAN_Start(&hcan2); HAL_CAN_Start(&hcan2);
HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING); 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: case BSP_UART_REF:
return &huart1; return &huart1;
case BSP_UART_AI: case BSP_UART_AI:
return &huart6; return &huart1;//NUC使用的是USART1
/* /*
case BSP_UART_XXX: case BSP_UART_XXX:
return &huartX; 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传输完成后进入此回调函数 //dma传输完成后进入此回调函数
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) 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))) 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_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; return;
} }
// CRC pass and start converting data to the motor // 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_B_GPIO_Port, LED_B_Pin, GPIO_PIN_RESET); //蓝色灯亮 // indicate CRC incorrect
HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_G_Pin, GPIO_PIN_RESET); //红色灯灭
GO_M8010_recv_data(Temp_buffer); 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; return;
} }
Pos = Pos * (3.14159f / 180.0f); // 角度转弧度
// assign motor target goal to the buffer // assign motor target goal to the buffer
motor->motor_send_data.head[0]=0xFE; motor->motor_send_data.head[0]=0xFE;
motor->motor_send_data.head[1]=0xEE; 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]; int32_t Pos = Temp_buffer[10]<<24 | Temp_buffer[9]<<16 | Temp_buffer[8]<<8 | Temp_buffer[7];
motor->Pos = Pos; motor->Pos = Pos;
motor->Pos = (motor->Pos * 6.28319f )/(32768*6.33f); motor->Pos = (motor->Pos * 6.28319f )/(32768*6.33f);
motor->Pos = motor->Pos * (180.0f / 3.14159f); // 弧度转角度
int8_t Temp = Temp_buffer[11] & 0xFF; int8_t Temp = Temp_buffer[11] & 0xFF;
motor->Temp = Temp; motor->Temp = Temp;
motor->MError = Temp_buffer[12] & 0x7; motor->MError = Temp_buffer[12] & 0x7;

114
User/device/GO_M8010_6_Driver.h
View File

@ -8,105 +8,109 @@ extern "C"{
#include "usart.h" #include "usart.h"
#include "string.h" #include "string.h"
#include <math.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 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 id :4; // 电机ID: 0,1...,14 15表示广播模式(暂时无法用)
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 status :3; // 工作模式: 0.停止 1.FOC校准 2.编码器校准 3.运行
uint8_t none :1; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ uint8_t none :1; // 保留位
} RIS_Mode_t /*__attribute__((packed))*/; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ 1Byte } RIS_Mode_t /*__attribute__((packed))*/; // 电机模式 1Byte
/** /**
* @brief <EFBFBD><EFBFBD><EFBFBD>״̬<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ * @brief 电机状态控制信息
* *
*/ */
typedef struct typedef struct
{ {
int16_t tor_des; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ؽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ť<EFBFBD><EFBFBD> unit: N.m (q8) int16_t tor_des; // 电机目标力矩 unit: N.m (q8)
int16_t spd_des; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ؽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD> unit: rad/s (q7) int16_t spd_des; // 电机目标速度 unit: rad/s (q7)
int32_t pos_des; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ؽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD> unit: rad (q15) int32_t pos_des; // 电机目标位置 unit: rad (q15)
uint16_t k_pos; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ؽڸն<EFBFBD>ϵ<EFBFBD><EFBFBD> unit: 0.0-1.0 (q15) uint16_t k_pos; // 电机位置环系数 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) 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 typedef struct
{ {
uint8_t head[2]; // <EFBFBD><EFBFBD>ͷ 2Byte uint8_t head[2]; // 包头 2Byte
RIS_Mode_t mode; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ 1Byte RIS_Mode_t mode; // 电机工作模式 1Byte
RIS_Comd_t comd; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 12Byte RIS_Comd_t comd; // 电机控制命令 12Byte
uint16_t CRC16; // CRC 2Byte 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 { 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 id; // 电机ID, 0xBB表示广播
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> unsigned short mode; // 工作模式: 0.停止 5.正转 10.FOC控制
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> uint16_t correct; // 校正状态: 1.校正完成 0.未校正
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 MError; // 电机错误: 0.正常 1.过流 2.过载 3.欠压 4.过温
int Temp; // <EFBFBD><EFBFBD> int Temp; // 温度
float tar_pos; // target position float tar_pos; // 目标位置
float tar_w; // target speed float tar_w; // 目标速度
float T; // <EFBFBD><EFBFBD>ǰʵ<EFBFBD>ʵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> float T; // 当前实际力矩
float W; // <EFBFBD><EFBFBD>ǰʵ<EFBFBD>ʵ<EFBFBD><EFBFBD><EFBFBD>ٶȣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>٣<EFBFBD> float W; // 当前实际速度 (单位: rad/s)
float Pos; // <EFBFBD><EFBFBD>ǰ<EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD> float Pos; // 当前实际位置
int footForce; // They dont even know what 7 is this so we dont update this int footForce; // 预留字段, 暂未使用
uint8_t buffer[17]; uint8_t buffer[17]; // 数据发送缓冲区
uint8_t Rec_buffer[16]; uint8_t Rec_buffer[16]; // 数据接收缓冲区
ControlData_t motor_send_data; ControlData_t motor_send_data; // 电机控制数据结构
}GO_Motorfield; } GO_Motorfield;
/** /**
*@brief <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD> *@brief 电机初始化
*/ */
void GO_M8010_init(void); void GO_M8010_init(void);
/** /**
*@brief go<EFBFBD><EFBFBD><EFBFBD>Ϳ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> *@brief go发送控制命令
*@input[in] huart <EFBFBD><EFBFBD><EFBFBD>ô<EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><EFBFBD> *@input[in] huart 所用串口指针
*@input[in] id <EFBFBD><EFBFBD><EFBFBD>ص<EFBFBD><EFBFBD>id *@input[in] id 被控电机id
*@input[in] rev <EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>֪<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɶ<EFBFBD>õģ<EFBFBD>githubΪ<EFBFBD>ش<EFBFBD>issue *@input[in] rev 暂时不知道干啥用的，github为回答issue
*@input[in] T <EFBFBD><EFBFBD><EFBFBD>أ<EFBFBD><EFBFBD><EFBFBD>λN<EFBFBD><EFBFBD>m *@input[in] T 力矩，单位N·m
*@input[in] Pos Ŀ<EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD><EFBFBD><EFBFBD>λrad *@input[in] Pos 目标位置，单位rad
*@input[in] W Ŀ<EFBFBD><EFBFBD><EFBFBD>ٶȣ<EFBFBD><EFBFBD><EFBFBD>λrad/s *@input[in] W 目标速度，单位rad/s
*@input[in] K_P λ<EFBFBD>û<EFBFBD><EFBFBD><EFBFBD>kp *@input[in] K_P 位置环kp
*@input[in] K_W <EFBFBD>ٶȻ<EFBFBD>kp *@input[in] K_W 速度环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 *@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); 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, 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); float tar_pos, float tar_w, float K_P,float K_W);
/** /**
*@brief go<EFBFBD><EFBFBD><EFBFBD>Ϳ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> *@brief go获取电机指针
*@input[in] id <EFBFBD><EFBFBD><EFBFBD>ص<EFBFBD><EFBFBD>id *@input[in] id 电机的id
*@revtal <EFBFBD><EFBFBD><EFBFBD>ص<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><EFBFBD> *@revtal 返回电机的指针
*/ */
GO_Motorfield* getGoPoint(uint8_t id); GO_Motorfield* getGoPoint(uint8_t id);
#ifdef __cplusplus #ifdef __cplusplus

200
User/device/djiMotor.c
View File

@ -22,6 +22,7 @@
#include "TopDefine.h" #include "TopDefine.h"
#include<cmsis_os2.h> #include<cmsis_os2.h>
#include "FreeRTOS.h" #include "FreeRTOS.h"
#include <math.h>
#include "odrive_can.h" #include "odrive_can.h"
@ -37,7 +38,7 @@
(ptr)->temperate = (data)[6]; \ (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)->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指向电机测量数据结构体的指针，data包含电机测量数据的数组.
/*(ptr)->real_angle = (ptr)->real_angle % 360; */ /*(ptr)->real_angle = (ptr)->real_angle % 360; */
#define get_motor_offset(ptr, data) \ #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)->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; \ (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) \ // #define get_5065motor_measure(ptr, data) \
@ -67,7 +79,12 @@
#if DEBUG == 1 #if DEBUG == 1
//电机回传数据结构体
motor_measure_t motor_chassis[5]; 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 #else
static motor_measure_t motor_chassis[5]; static motor_measure_t motor_chassis[5];
#endif #endif
@ -81,6 +98,41 @@ static uint8_t can_send_data_200[8];
CAN_RxHeaderTypeDef dji_rx_header; CAN_RxHeaderTypeDef dji_rx_header;
uint8_t dji_rx_data[8]; 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 数据处理函数 * @brief 数据处理函数
* @param[in] none * @param[in] none
@ -90,13 +142,10 @@ void djiMotorEncode()
{ {
switch (dji_rx_header.StdId) switch (dji_rx_header.StdId)
{ {
case 0x201: case M3508_1:
case 0x202: case M3508_2:
case 0x203: case M3508_3:
case 0x204: case M3508_4:
case 0x205:
case 0x206:
case 0x207:
{ {
static uint8_t i = 0; static uint8_t i = 0;
//get motor id //get motor id
@ -110,6 +159,36 @@ void djiMotorEncode()
} }
break; 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: 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 #if FREERTOS_DJI == 0
/** /**
* @brief hal库CAN回调函数,接收电机数据 * @brief hal库CAN回调函数,接收电机数据
@ -132,7 +231,7 @@ void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
} }
#else #else
static osEventFlagsId_t eventReceive; static osEventFlagsId_t eventReceive;//事件标志
/** /**
* @brief 自定义大疆电机回调函数 * @brief 自定义大疆电机回调函数
* @param[in] none * @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] motor1: (0x209) 电机控制电流
* @param[in] motor2: (0x20A) 电机控制电流 * @param[in] motor2: (0x20A) 电机控制电流
* @param[in] motor3: (0x20B) 电机控制电流 * @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) void CAN_cmd_2FF(int16_t motor1, int16_t motor2, int16_t motor3, CAN_HandleTypeDef*hcan)
{ {
uint32_t send_mail_box; 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.IDE = CAN_ID_STD;
tx_meessage_2ff.RTR = CAN_RTR_DATA; tx_meessage_2ff.RTR = CAN_RTR_DATA;
tx_meessage_2ff.DLC = 0x08; tx_meessage_2ff.DLC = 0x08;
@ -254,4 +353,83 @@ motor_measure_t *get_motor_point(uint8_t i)
{ {
return &motor_chassis[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 M2006 = 2
}motor_type_e; }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 //rm motor data
typedef struct typedef struct
@ -45,12 +57,40 @@ typedef struct
} odrive_measure_t; } 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 //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_RAD 0.000766990394f
#define MOTOR_ECD_TO_ANGLE_6020 (360.0 / 8191.0 ) #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 #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); 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 数据处理函数 * @brief 数据处理函数
@ -109,6 +162,12 @@ extern motor_measure_t *get_motor_point(uint8_t i);
*/ */
void djiMotorEncode(void); 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 #ifdef __cplusplus
} }
#endif #endif

109
User/device/nuc.c
View File

@ -1,6 +1,6 @@
#include "nuc.h" #include "nuc.h"
#include <string.h> #include <string.h>
//#include "crc16.h" #include "crc_ccitt.h"
uint8_t nucbuf[32]; uint8_t nucbuf[32];
@ -32,60 +32,77 @@ int8_t NUC_Restart(void) {
return DEVICE_OK; return DEVICE_OK;
} }
bool_t NUC_WaitDmaCplt(void) { bool_t NUC_WaitDmaCplt(void) {
// return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll,500) == // return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll,500) ==
// SIGNAL_NUC_RAW_REDY); // SIGNAL_NUC_RAW_REDY);
return 1;
} }
int8_t NUC_RawParse(NUC_t *n){ int8_t NUC_RawParse(NUC_t *n) {
if(n ==NULL) return DEVICE_ERR_NULL; if (n == NULL) return DEVICE_ERR_NULL;
union union {
{ float x[3];
float x[3]; char data[12];
char data[12]; } instance; // 方便在浮点数和字符数组之间进行数据转换
}instance;//方便在浮点数和字符数组之间进行数据转换
if(nucbuf[0]!=HEAD) goto error; //发送ID不是底盘 // 校验数据包头
else{ if (nucbuf[0] != HEAD) {
n->status_fromnuc =nucbuf[1]; return DEVICE_ERR;
n->ctrl_status =nucbuf[2];
switch (n->status_fromnuc)
{
case VISION:
/* 协议格式
0xFF HEAD
0x0X 控制帧
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:
break;
}
return DEVICE_OK;
} }
error: // 提取数据包中的 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
0x02 控制帧
0x01 相机帧
x fp32
y fp32
z fp32
0xFE TAIL
*/
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;
}
else
{
return DEVICE_ERR; return DEVICE_ERR;
}
error:
return DEVICE_ERR;
} }

8
User/device/nuc.h
View File

@ -13,7 +13,7 @@
#define ODOM (0x04) #define ODOM (0x04)
#define PICK (0x06) #define PICK (0x06)
#define VISION (0x01) #define VISION (0x09)
//写结构体存入视觉信息 //写结构体存入视觉信息
@ -27,9 +27,9 @@ typedef struct{
struct struct
{ {
float x; fp32 x;
float y; fp32 y;
float z; fp32 z;
}vision; }vision;
}NUC_t; }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 #else
//DMA双缓冲区+串口空闲中断 //DMA双缓冲区+串口空闲中断
// 02 53 44 1x1 3x2
static osEventFlagsId_t eventReceive; static osEventFlagsId_t eventReceive;
RC_mess_t RC_mess; RC_mess_t RC_mess;

86
User/module/ball.c
View File

@ -3,84 +3,82 @@
#include "cmsis_os.h" #include "cmsis_os.h"
#include "FreeRTOS.h" #include "FreeRTOS.h"
#include "bsp_delay.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 ballcome 1
#define balldown 0 #define balldown 0
extern GO go;
//光电识别 //光电识别
int ball_in(void) int ball_in(void)
{ {
if (HAL_GPIO_ReadPin(ball_up_GPIO_Port, ball_up_Pin) == GPIO_PIN_SET) return (HAL_GPIO_ReadPin(ball_up_GPIO_Port, ball_up_Pin));
{
return ballcome;
}
else
{
return balldown;
}
} }
int pass_ball(void) 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); HAL_GPIO_WritePin(paw_GPIO_Port, paw_Pin, GPIO_PIN_SET);
osDelay(2); paw_state =1;//爪子开
delay_us(100); // osDelay(5);
//下推 HAL_GPIO_WritePin(down_GPIO_Port, down_Pin, GPIO_PIN_RESET);//打出
HAL_GPIO_WritePin(down_GPIO_Port, down_Pin, GPIO_PIN_SET); 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) int catch_ball(int inball)
{ {
inball =pass_ball(); inball =pass_ball();
if(inball==1) if(inball==1)
{ {
//接球
HAL_GPIO_WritePin(key_GPIO_Port, key_Pin, GPIO_PIN_SET);
return ballcome; return ballcome;
} }
else else
{ {
HAL_GPIO_WritePin(key_GPIO_Port, key_Pin, GPIO_PIN_RESET);
return balldown; return balldown;
} }

2
User/module/ball.h
View File

@ -12,7 +12,7 @@ typedef struct ball
int ball_in(void); int ball_in(void);
int pass_ball(void); int pass_ball(void);
void handling_ball(int hand, int paw); void handling_ball(int hand, int angle);
int catch_ball(int inball); 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_type_def angle_pid;//3508位置环pid结构体
//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_speed_PID[3]={50.0,0.3,0.0}; //P,I,D(速度环)
fp32 M3508_angle_PID[3]={25.0,0.0,1.5}; //P,I,D(角度环) fp32 M3508_angle_PID[3]={50.0,0.0,1.5}; //P,I,D(角度环)
//速度环pid参数 //速度环pid参数
fp32 M3508_PID[3]={4.9,0.01,0.0}; fp32 M3508_PID[3]={4.9,0.01,0.0};
int16_t result; //速度环
float angleSet[MOTOR_NUM]; //位置环
//trigger //trigger
//编码器数据 //编码器数据
const motor_measure_t *trigger_motor_data;//3508电机数据 const motor_measure_t *trigger_motor_data;//3508电机数据
pid_type_def t_speed_pid;//2006速度环pid结构体 pid_type_def t_speed_pid;//2006速度环pid结构体
pid_type_def t_angle_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_speed_PID[3]={10.0,0.1,0.0}; //P,I,D(速度环)
fp32 M2006_angle_PID[3]={25.0,0.0,1.5}; //P,I,D(角度环) fp32 M2006_angle_PID[3]={5.0,0.0,0.05}; //P,I,D(角度环)
//速度环pid参数 //速度环pid参数
fp32 M2006_PID[3]={4.9,0.01,0.0}; fp32 M2006_PID[3]={4.9,0.01,0.0};
int16_t t_result; //速度环 //can最后发送的数据 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) void motor_init(void)
{ {
// motor_3508_data=get_motor_point(0); // motor_3508_data=get_motor_point(0);
trigger_motor_data=get_motor_point(0); trigger_motor_data=get_2006_motor_point(0);
// PID_init(&speed_pid,PID_POSITION,M3508_speed_PID,3000,1000); GM6020_motor_data=get_6020_motor_point(0);
// PID_init(&angle_pid,PID_POSITION,M3508_angle_PID,7000,2000);
// 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_speed_pid,PID_POSITION,M2006_speed_PID,1600,600);
PID_init(&t_angle_pid,PID_POSITION,M2006_angle_PID,500000,100000); 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) 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); 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]); 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); fp32 angle_get;
fp32 my_angle;
} int32_t M6020_speed;
void motor_pos(fp32 angle) //6020串级
{ angle_get= angle;
int16_t delta[1];
//外环角度环
delta[0]=PID_calc(&angle_pid,motor_3508_data->total_angle,angle); my_angle=PID_calc(&angle_6020_pid,GM6020_motor_data->total_angle,angle_get);//角度环
result=PID_calc(&speed_pid,motor_3508_data->speed_rpm,delta[0]); //内环速度环
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; }motorput_e;
//float t_angleSet[1]; //位置环
void motor_init(void); void motor_init(void);
void motor_speed(fp32 speed);
void motor_pos(fp32 angle);
void trigger_pos(fp32 angle); void trigger_pos(fp32 angle);
void my_GM6020_control(fp32 angle);
#endif #endif

29
User/module/go.c
View File

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

5
User/module/led.c
View File

@ -4,10 +4,9 @@
void LED_red(void) void LED_red(void)
{ {
//GPIO_PIN_12 //GPIO_PIN_12
//HAL_GPIO_TogglePin(ledBlue_GPIO_Port, ledBlue_Pin);
HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_R_Pin, GPIO_PIN_SET); 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); HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_R_Pin, GPIO_PIN_RESET);
} }

113
User/module/shoot.c
View File

@ -1,92 +1,51 @@
#include "shoot.h" #include "shoot.h"
#include "remote_control.h" #include "remote_control.h"
#include "go.h"
#include "calc_lib.h"
extern RC_mess_t RC_mess; extern RC_mess_t RC_mess;
extern motor_measure_t *trigger_motor_data;//3508电机数据 extern motor_measure_t *trigger_motor_data;//3508电机数据
extern GO go;
int shoot_flag = 0;
#define GO1_SHOOT 0 int speed_5065=0;
#define ODRIVE_SHOOT 1
#define KP 0.12 #define KP 0.12
#define KD 0.008 #define KD 0.008
//sw[1] 306--1694
#if GO1_SHOOT == 1 void control_shoot(void)
GO_SHOOT goshoot;
void shooterInit(void)
{ {
int i;
GO_M8010_init();
for(i = 0;i < GO_NUM;i ++)
{
goshoot.goData[i] = getGoPoint(i);//获取电机数据指针
goshoot.angleSetgo[i] = 0; if(RC_mess.RC_data.sw[1]>800)
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); if(HAL_GPIO_ReadPin(ball_in1_GPIO_Port, ball_in1_Pin) == GPIO_PIN_RESET)
goshoot.offestAngle[i] = goshoot.goData[i]->Pos; {
HAL_Delay(100); CAN_VESC_HEAD(1);
// GO_M8010_send_data(&huart1, i,0,0,0,0,0,0); CAN_VESC_HEAD(2);
GO_M8010_send_data(&huart6, i,0,0,0,0,0,0); }
goshoot.offestAngle[i] = goshoot.goData[i]->Pos; else
HAL_Delay(100); {
} //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; float allowRange;
}GO_SHOOT; }GO_SHOOT;
void shooterInit(void); void control_shoot(void);
void shoot_ball(int key);
void shoot_odrive(int angle);
#endif #endif

34
User/task/ball_task.c
View File

@ -5,10 +5,15 @@
#include <cmsis_os2.h> #include <cmsis_os2.h>
#include "remote_control.h" #include "remote_control.h"
#include "ball.h" #include "ball.h"
#include "go.h"
extern RC_mess_t RC_mess; extern RC_mess_t RC_mess;
extern GO go;
BASKETBALL basketball; BASKETBALL basketball;
int bb=0;
int Turn_pos=0;
void Task_Ball(void *argument) void Task_Ball(void *argument)
{ {
(void)argument; /* 未使用argument，消除警告 */ (void)argument; /* 未使用argument，消除警告 */
@ -16,11 +21,36 @@ void Task_Ball(void *argument)
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计算 */ uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计算 */
basketball.hand=0; 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) 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; /* 计算下一个唤醒时刻 */ tick += delay_tick; /* 计算下一个唤醒时刻 */
osDelayUntil(tick); /* 运行结束，等待下一个周期唤醒 */ osDelayUntil(tick); /* 运行结束，等待下一个周期唤醒 */

2
User/task/can_task.c
View File

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

42
User/task/dji_task.c
View File

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

3
User/task/dji_task.h
View File

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

11
User/task/go_task.c
View File

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

11
User/task/led_task.c
View File

@ -4,17 +4,16 @@
#include "main.h" #include "main.h"
#include "led.h" #include "led.h"
#include "buzzer.h" #include "buzzer.h"
int a1=0;
void Task_Led(void *argument) { void Task_Led(void *argument) {
(void)argument; /* 未使用argument，消除警告 */ (void)argument; /* 未使用argument，消除警告 */
for (;;) { while(1) {
// LED_red(); // LED_red();
// LED_bule(); // 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" #include "remote_control.h"
extern RC_mess_t RC_mess; 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 遥控器任务 * \brief 遥控器任务
* *

38
User/task/shoot_task.c
View File

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