Compare commits
3 Commits
46cc491963
...
f9b87f8f39
| Author | SHA1 | Date | |
|---|---|---|---|
| f9b87f8f39 | |||
| a235d14661 | |||
| 9688eca871 |
@ -102,7 +102,7 @@ void MX_USART6_UART_Init(void)
|
||||
|
||||
/* USER CODE END USART6_Init 1 */
|
||||
huart6.Instance = USART6;
|
||||
huart6.Init.BaudRate = 4000000;
|
||||
huart6.Init.BaudRate = 115200;
|
||||
huart6.Init.WordLength = UART_WORDLENGTH_8B;
|
||||
huart6.Init.StopBits = UART_STOPBITS_1;
|
||||
huart6.Init.Parity = UART_PARITY_NONE;
|
||||
|
||||
36
MDK-ARM/.vscode/c_cpp_properties.json
vendored
36
MDK-ARM/.vscode/c_cpp_properties.json
vendored
@ -3,26 +3,26 @@
|
||||
{
|
||||
"name": "R1-shooter",
|
||||
"includePath": [
|
||||
"d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\Core\\Inc",
|
||||
"d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Inc",
|
||||
"d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Inc\\Legacy",
|
||||
"d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\include",
|
||||
"d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\CMSIS_RTOS_V2",
|
||||
"d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\RVDS\\ARM_CM4F",
|
||||
"d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\Drivers\\CMSIS\\Device\\ST\\STM32F4xx\\Include",
|
||||
"d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\Drivers\\CMSIS\\Include",
|
||||
"d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\User\\bsp",
|
||||
"d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\User\\module",
|
||||
"d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\User\\task",
|
||||
"d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\User\\lib",
|
||||
"d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\User\\device",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Core\\Inc",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Inc",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Inc\\Legacy",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\include",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\CMSIS_RTOS_V2",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\RVDS\\ARM_CM4F",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Drivers\\CMSIS\\Device\\ST\\STM32F4xx\\Include",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Drivers\\CMSIS\\Include",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\User\\bsp",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\User\\module",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\User\\task",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\User\\lib",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\User\\device",
|
||||
"D:\\keil\\ARM\\ARMCC\\include",
|
||||
"D:\\keil\\ARM\\ARMCC\\include\\rw",
|
||||
"d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\MDK-ARM",
|
||||
"d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\Core\\Src",
|
||||
"d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Src",
|
||||
"d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source",
|
||||
"d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\MemMang"
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\MDK-ARM",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Core\\Src",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Src",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\MemMang"
|
||||
],
|
||||
"defines": [
|
||||
"USE_HAL_DRIVER",
|
||||
|
||||
20
MDK-ARM/.vscode/keil-assistant.log
vendored
20
MDK-ARM/.vscode/keil-assistant.log
vendored
@ -104,3 +104,23 @@
|
||||
[info] Log at : 2025/5/22|16:11:31|GMT+0800
|
||||
>>>>>>> 8fc2f50677ecabc32b37eac7a7df89d916fefef0
|
||||
|
||||
[info] Log at : 2025/5/23|21:04:08|GMT+0800
|
||||
|
||||
[info] Log at : 2025/5/25|14:26:08|GMT+0800
|
||||
|
||||
[info] Log at : 2025/5/26|19:07:26|GMT+0800
|
||||
|
||||
[info] Log at : 2025/5/26|20:16:52|GMT+0800
|
||||
|
||||
[info] Log at : 2025/5/27|00:06:58|GMT+0800
|
||||
|
||||
[info] Log at : 2025/5/27|19:35:30|GMT+0800
|
||||
|
||||
[info] Log at : 2025/5/28|13:50:56|GMT+0800
|
||||
|
||||
[info] Log at : 2025/5/29|19:03:54|GMT+0800
|
||||
|
||||
[info] Log at : 2025/5/30|19:34:50|GMT+0800
|
||||
|
||||
[info] Log at : 2025/5/31|14:39:23|GMT+0800
|
||||
|
||||
|
||||
6
MDK-ARM/.vscode/uv4.log
vendored
6
MDK-ARM/.vscode/uv4.log
vendored
@ -1,8 +1,8 @@
|
||||
*** Using Compiler 'V5.06 update 7 (build 960)', folder: 'D:\keil\ARM\ARMCC\Bin'
|
||||
Build target 'R1-shooter'
|
||||
compiling gimbalTask.cpp...
|
||||
compiling usart.c...
|
||||
linking...
|
||||
Program Size: Code=28656 RO-data=1824 RW-data=252 ZI-data=23940
|
||||
Program Size: Code=30588 RO-data=2144 RW-data=276 ZI-data=24012
|
||||
FromELF: creating hex file...
|
||||
"R1-shooter\R1-shooter.axf" - 0 Error(s), 0 Warning(s).
|
||||
Build Time Elapsed: 00:00:03
|
||||
Build Time Elapsed: 00:00:05
|
||||
|
||||
2
MDK-ARM/.vscode/uv4.log.lock
vendored
2
MDK-ARM/.vscode/uv4.log.lock
vendored
@ -1 +1 @@
|
||||
2025/5/23 20:55:43
|
||||
2025/5/31 16:57:56
|
||||
@ -103,7 +103,7 @@
|
||||
<bEvRecOn>1</bEvRecOn>
|
||||
<bSchkAxf>0</bSchkAxf>
|
||||
<bTchkAxf>0</bTchkAxf>
|
||||
<nTsel>6</nTsel>
|
||||
<nTsel>3</nTsel>
|
||||
<sDll></sDll>
|
||||
<sDllPa></sDllPa>
|
||||
<sDlgDll></sDlgDll>
|
||||
@ -114,7 +114,7 @@
|
||||
<tDlgDll></tDlgDll>
|
||||
<tDlgPa></tDlgPa>
|
||||
<tIfile></tIfile>
|
||||
<pMon>STLink\ST-LINKIII-KEIL_SWO.dll</pMon>
|
||||
<pMon>BIN\CMSIS_AGDI.dll</pMon>
|
||||
</DebugOpt>
|
||||
<TargetDriverDllRegistry>
|
||||
<SetRegEntry>
|
||||
@ -140,7 +140,7 @@
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
<Key>DLGUARM</Key>
|
||||
<Name>(105=-1,-1,-1,-1,0)</Name>
|
||||
<Name></Name>
|
||||
</SetRegEntry>
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
@ -153,40 +153,7 @@
|
||||
<Name>-U00160029510000164E574E32 -O2254 -SF10000 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP (ARM Core") -D00(2BA01477) -L00(0) -TO131090 -TC10000000 -TT10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC800 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
|
||||
</SetRegEntry>
|
||||
</TargetDriverDllRegistry>
|
||||
<Breakpoint>
|
||||
<Bp>
|
||||
<Number>0</Number>
|
||||
<Type>0</Type>
|
||||
<LineNumber>169</LineNumber>
|
||||
<EnabledFlag>1</EnabledFlag>
|
||||
<Address>134235782</Address>
|
||||
<ByteObject>0</ByteObject>
|
||||
<HtxType>0</HtxType>
|
||||
<ManyObjects>0</ManyObjects>
|
||||
<SizeOfObject>0</SizeOfObject>
|
||||
<BreakByAccess>0</BreakByAccess>
|
||||
<BreakIfRCount>1</BreakIfRCount>
|
||||
<Filename>..\User\device\djiMotor.c</Filename>
|
||||
<ExecCommand></ExecCommand>
|
||||
<Expression>\\R1_shooter\../User/device/djiMotor.c\169</Expression>
|
||||
</Bp>
|
||||
<Bp>
|
||||
<Number>1</Number>
|
||||
<Type>0</Type>
|
||||
<LineNumber>174</LineNumber>
|
||||
<EnabledFlag>1</EnabledFlag>
|
||||
<Address>134235770</Address>
|
||||
<ByteObject>0</ByteObject>
|
||||
<HtxType>0</HtxType>
|
||||
<ManyObjects>0</ManyObjects>
|
||||
<SizeOfObject>0</SizeOfObject>
|
||||
<BreakByAccess>0</BreakByAccess>
|
||||
<BreakIfRCount>1</BreakIfRCount>
|
||||
<Filename>..\User\device\djiMotor.c</Filename>
|
||||
<ExecCommand></ExecCommand>
|
||||
<Expression>\\R1_shooter\../User/device/djiMotor.c\174</Expression>
|
||||
</Bp>
|
||||
</Breakpoint>
|
||||
<Breakpoint/>
|
||||
<WatchWindow1>
|
||||
<Ww>
|
||||
<count>0</count>
|
||||
@ -251,57 +218,57 @@
|
||||
<Ww>
|
||||
<count>12</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>ball_state,0x0A</ItemText>
|
||||
<ItemText>shoot,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>13</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>flag,0x0A</ItemText>
|
||||
<ItemText>speedm,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>14</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>shoot,0x0A</ItemText>
|
||||
<ItemText>shoot_flag,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>15</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>speedm,0x0A</ItemText>
|
||||
<ItemText>angle1,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>16</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>a1,0x0A</ItemText>
|
||||
<ItemText>motor_5065</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>17</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>a2,0x0A</ItemText>
|
||||
<ItemText>test_distance</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>18</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>shoot_flag,0x0A</ItemText>
|
||||
<ItemText>xxx</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>19</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>angle1,0x0A</ItemText>
|
||||
<ItemText>test_speed,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>20</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>rx_header</ItemText>
|
||||
<ItemText>speedm,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>21</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>rx_data</ItemText>
|
||||
<ItemText>Torque,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>22</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>motor_5065</ItemText>
|
||||
<ItemText>triggerspeed,0x0A</ItemText>
|
||||
</Ww>
|
||||
</WatchWindow1>
|
||||
<WatchWindow2>
|
||||
@ -1113,7 +1080,7 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>User/module</GroupName>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
|
||||
13
README.md
13
README.md
@ -14,4 +14,15 @@ r1上层
|
||||
|
||||
//PC6 接球光电
|
||||
|
||||
//PI6 运球光电
|
||||
//PI6 运球光电
|
||||
|
||||
## 上层过程
|
||||
|
||||
+ 一个按键切换 运球 /发射
|
||||
+ 👈 运球 +
|
||||
+ .3
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@ -32,7 +32,7 @@ void GO_M8010_init (){
|
||||
|
||||
}
|
||||
// HAL_UART_RegisterCallback(&huart1, HAL_UART_TX_COMPLETE_CB_ID, uartTxCB);
|
||||
HAL_UART_RegisterCallback(&huart6, HAL_UART_TX_COMPLETE_CB_ID, uartTxCB);
|
||||
// HAL_UART_RegisterCallback(&huart6, HAL_UART_TX_COMPLETE_CB_ID, uartTxCB);
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -78,11 +78,18 @@
|
||||
|
||||
}
|
||||
|
||||
//小米
|
||||
#define ID_xiaomi 1
|
||||
#define Pmax 1
|
||||
#define speed_full_scale 200.0f
|
||||
#define current_full_scale 4.0f
|
||||
|
||||
|
||||
#if DEBUG == 1
|
||||
|
||||
//电机回传数据结构体
|
||||
motor_measure_t motor_chassis[10];
|
||||
CAN_MotorFeedback_t motor_5065[2];
|
||||
CAN_MotorFeedback_t motor_5065[3];
|
||||
#else
|
||||
static motor_measure_t motor_chassis[5];
|
||||
#endif
|
||||
@ -93,16 +100,25 @@ static uint8_t can_send_data_2ff[8];
|
||||
static CAN_TxHeaderTypeDef tx_message_200;
|
||||
static uint8_t can_send_data_200[8];
|
||||
|
||||
//can1 dji
|
||||
CAN_RxHeaderTypeDef dji_rx_header;
|
||||
uint8_t dji_rx_data[8];
|
||||
|
||||
CAN_RawTx_t raw_tx;
|
||||
|
||||
//can2 vesc xiaomi
|
||||
CAN_RxHeaderTypeDef rx_header;
|
||||
uint8_t rx_data[8];
|
||||
|
||||
static CAN_TxHeaderTypeDef vesc_tx_message;
|
||||
static uint8_t vesc_send_data[4];
|
||||
//小米
|
||||
uint8_t can_send_data[8];
|
||||
// 定义 CAN 传输头部信息
|
||||
static CAN_TxHeaderTypeDef tx_message;
|
||||
|
||||
MotorFeedback_t MotorFeedback;
|
||||
|
||||
CAN_RawTx_t raw_tx;
|
||||
/**
|
||||
* @brief 数据处理函数
|
||||
* @param[in] none
|
||||
@ -168,15 +184,28 @@ void can2MotorEncode()
|
||||
// 存储消息到对应的电机结构体中
|
||||
VescMotor_Decode(&motor_5065[0], &rx_header,rx_data);
|
||||
break;
|
||||
|
||||
case CAN_VESC5065_M2_MSG1:
|
||||
// 存储消息到对应的电机结构体中
|
||||
VescMotor_Decode(&motor_5065[1], &rx_header,rx_data);
|
||||
case CAN_VESC5065_M3_MSG1:
|
||||
// 存储消息到对应的电机结构体中
|
||||
VescMotor_Decode(&motor_5065[2], &rx_header,rx_data);
|
||||
break;
|
||||
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
switch (rx_data[0])
|
||||
{
|
||||
case ID_xiaomi:
|
||||
Parse_CAN_Response (rx_data,&MotorFeedback);
|
||||
break ;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#if FREERTOS_DJI == 0
|
||||
@ -387,7 +416,7 @@ return DEVICE_OK;
|
||||
uint32_t send_mail_box;
|
||||
|
||||
id = controller_id | ((uint32_t)CAN_PACKET_SET_RPM << 8);
|
||||
assert_param_rpm(&RPM);
|
||||
//assert_param_rpm(&RPM);
|
||||
data = (int32_t)(RPM);
|
||||
|
||||
vesc_tx_message.ExtId = id;
|
||||
@ -437,3 +466,69 @@ return DEVICE_OK;
|
||||
{
|
||||
return &motor_5065[i];
|
||||
}
|
||||
|
||||
|
||||
//小米
|
||||
void Parse_CAN_Response(uint8_t* rx_data, MotorFeedback_t* feedback) {
|
||||
// 1. 解析位置
|
||||
int16_t pos_code = (rx_data[1] << 8) | rx_data[2];
|
||||
feedback->position_deg = ((int16_t)(pos_code - 0x8000) / 32768.0f) * 360.0f * Pmax;
|
||||
|
||||
// 2. 解析速度
|
||||
int16_t speed_code = ((rx_data[3] << 4) | (rx_data[4] >> 4)) - 0x800;
|
||||
feedback->speed_rads = (speed_code / 2048.0f) * speed_full_scale;
|
||||
|
||||
// 3. 解析电流
|
||||
int16_t current_code = (((rx_data[4] & 0x0F) << 8) | rx_data[5]) - 0x800;
|
||||
feedback->current_A = (current_code / 2048.0f) * current_full_scale;
|
||||
|
||||
// 记录电机ID
|
||||
feedback->can_id = rx_data[0];
|
||||
}
|
||||
|
||||
|
||||
void CAN_XiaoMi(int id,JZ_xiaomi_t *JZ_xiaomi,CAN_HandleTypeDef*hcan)
|
||||
{
|
||||
uint32_t send_mail_box;
|
||||
static int is_open=0; //是否开启电机，根据手册，第一次使用需要ff开启
|
||||
tx_message.StdId = id;
|
||||
tx_message.IDE = CAN_ID_STD;
|
||||
tx_message.RTR = CAN_RTR_DATA;
|
||||
tx_message.DLC = 0x08;
|
||||
if(is_open){
|
||||
|
||||
uint16_t pos_code = (uint16_t)((JZ_xiaomi->position / (Pmax * 360.0f)) * 32768.0f + 32768.0f);
|
||||
uint16_t speed_code = (uint16_t)((JZ_xiaomi->speed / 200.0f) * 2048.0f + 2048.0f);
|
||||
uint16_t current_code = (uint16_t)((JZ_xiaomi->K_C / 4.0f) * 2048.0f + 2048.0f);
|
||||
|
||||
can_send_data[0] = (pos_code >> 8) & 0xFF; // 位置高8位
|
||||
can_send_data[1] = pos_code & 0xFF; // 位置低8位
|
||||
|
||||
can_send_data[2] = (speed_code >> 4) & 0xFF; // 速度高8位（取12位中的高8）
|
||||
can_send_data[3] = ((speed_code & 0x0F) << 4) | ((JZ_xiaomi->K_P >> 8) & 0x0F); // 速度低4位 + Kp高4位
|
||||
|
||||
can_send_data[4] = JZ_xiaomi->K_P & 0xFF; // Kp低8位
|
||||
|
||||
can_send_data[5] = (JZ_xiaomi->K_D >> 4) & 0xFF; // Kd高8位（取12位中的高8）
|
||||
can_send_data[6] = ((JZ_xiaomi->K_D & 0x0F) << 4) | ((current_code >> 8) & 0x0F); // Kd低4位 + 电流高4位
|
||||
|
||||
can_send_data[7] = current_code & 0xFF; // 电流低8位
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
can_send_data[0] = 0xFF;
|
||||
can_send_data[1] = 0xFF;
|
||||
can_send_data[2] = 0xFF;
|
||||
can_send_data[3] = 0xFF;
|
||||
can_send_data[4] = 0xFF;
|
||||
can_send_data[5] = 0xFF;
|
||||
can_send_data[6] = 0xFF;
|
||||
can_send_data[7] = 0xFC;
|
||||
is_open=1;
|
||||
}
|
||||
|
||||
|
||||
HAL_CAN_AddTxMessage(hcan, &tx_message, can_send_data, &send_mail_box);
|
||||
}
|
||||
|
||||
|
||||
@ -10,7 +10,7 @@ extern "C"{
|
||||
#include "device.h"
|
||||
#include "can.h"
|
||||
#include <math.h>
|
||||
|
||||
#include "string.h"
|
||||
|
||||
typedef enum{
|
||||
GM6020 = 0,
|
||||
@ -44,6 +44,7 @@ enum{
|
||||
GM6020_2 = 0x208,
|
||||
CAN_VESC5065_M1_MSG1 =0x901, //vesc的数据回传使用了扩展id，[0:7]为驱动器id，[8:15]为帧类型
|
||||
CAN_VESC5065_M2_MSG1 =0x902,
|
||||
CAN_VESC5065_M3_MSG1 =0x903,
|
||||
};
|
||||
|
||||
//vesc指令
|
||||
@ -170,6 +171,38 @@ void CAN_VESC_HEAD(uint8_t controller_id);
|
||||
|
||||
int8_t CAN_VESC_Control(int id,int speed ,CAN_HandleTypeDef*hcan);
|
||||
|
||||
|
||||
//小米
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int16_t position;
|
||||
int16_t speed;
|
||||
int16_t K_P;
|
||||
int16_t K_D;
|
||||
int16_t K_C;
|
||||
|
||||
int16_t Pmax;//决定最大角度值，+-1 -> 最大+-360°
|
||||
|
||||
}JZ_xiaomi_t;
|
||||
|
||||
typedef struct {
|
||||
|
||||
uint8_t can_id;
|
||||
float position_deg;
|
||||
float speed_rads;
|
||||
float current_A;
|
||||
|
||||
} MotorFeedback_t;
|
||||
|
||||
|
||||
|
||||
void Parse_CAN_Response(uint8_t* rx_data, MotorFeedback_t* feedback) ;
|
||||
|
||||
|
||||
void CAN_XiaoMi(int id,JZ_xiaomi_t *JZ_xiaomi,CAN_HandleTypeDef*hcan);
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -77,19 +77,12 @@ int8_t NUC_RawParse(NUC_t *n) {
|
||||
} instance; // 方便在浮点数和字符数组之间进行数据转换
|
||||
|
||||
// 校验数据包头
|
||||
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) {
|
||||
|
||||
if(nucbuf[0]!=HEAD) goto error; //发送ID不是底盘
|
||||
else
|
||||
{
|
||||
n->status_fromnuc = nucbuf[1];
|
||||
n->ctrl_status = nucbuf[2];
|
||||
|
||||
switch (n->status_fromnuc) {
|
||||
n->ctrl_status = nucbuf[2];
|
||||
switch (n->status_fromnuc) {
|
||||
case VISION:
|
||||
/* 协议格式
|
||||
0xFF HEAD
|
||||
@ -100,42 +93,103 @@ int8_t NUC_RawParse(NUC_t *n) {
|
||||
z fp32
|
||||
0xFE TAIL
|
||||
*/
|
||||
if (nucbuf[15] != TAIL) goto error;
|
||||
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[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[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];
|
||||
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_OK;
|
||||
}
|
||||
|
||||
|
||||
error:
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
|
||||
|
||||
// int8_t NUC_RawParse(NUC_t *n) {
|
||||
// if (n == NULL) return DEVICE_ERR_NULL;
|
||||
// union {
|
||||
// float x[3];
|
||||
// char data[12];
|
||||
// } instance; // 方便在浮点数和字符数组之间进行数据转换
|
||||
|
||||
// // 校验数据包头
|
||||
// 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
|
||||
// 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;
|
||||
|
||||
// }
|
||||
|
||||
// error:
|
||||
// return DEVICE_ERR;
|
||||
// }
|
||||
|
||||
|
||||
int8_t NUC_HandleOffline(NUC_t *cmd)
|
||||
{
|
||||
if(cmd == NULL) return DEVICE_ERR_NULL;
|
||||
|
||||
@ -7,8 +7,8 @@
|
||||
extern RC_ctrl_t rc_ctrl;
|
||||
extern int key;
|
||||
|
||||
// C键 sw[4]👆 200 中1000 👇1800
|
||||
// D键 sw[5]👆 1800 👇200
|
||||
// C键 sw[5]👆 1800 中1000 👇200
|
||||
// D键 sw[6]👆 200 👇1800
|
||||
|
||||
#define M_SPEED 4000
|
||||
|
||||
@ -29,7 +29,20 @@ Ball ::Ball()
|
||||
{
|
||||
detect_init();
|
||||
|
||||
//两个伸缩6020
|
||||
//三摩擦轮
|
||||
for(int i = 0;i < MOTOR_NUM;i ++)
|
||||
{
|
||||
hand_Motor[i] = get_motor_point(i);
|
||||
if(i <=3)
|
||||
{
|
||||
hand_Motor[i]->type = M3508;//设置电机类型
|
||||
PID_init(&speed_pid[i],PID_POSITION,M3508_speed_PID,16000, 10000);//pid初始化
|
||||
}
|
||||
result[i] = 0;
|
||||
speedSet[i] = 0;
|
||||
}
|
||||
|
||||
//两个伸缩6020
|
||||
Extern_Motor[0] = get_motor_point(6);
|
||||
Extern_Motor[1] = get_motor_point(7);
|
||||
|
||||
@ -46,19 +59,13 @@ Ball ::Ball()
|
||||
angleSet[0] = 0;
|
||||
angleSet[1] = 0;
|
||||
|
||||
//三摩擦轮
|
||||
for(int i = 0;i < MOTOR_NUM;i ++)
|
||||
{
|
||||
hand_Motor[i] = get_motor_point(i);
|
||||
if(i <=3)
|
||||
{
|
||||
hand_Motor[i]->type = M3508;//设置电机类型
|
||||
PID_init(&speed_pid[i],PID_POSITION,M3508_speed_PID,16000, 10000);//pid初始化
|
||||
}
|
||||
result[i] = 0;
|
||||
speedSet[i] = 0;
|
||||
}
|
||||
|
||||
//小米电机初始化
|
||||
JZ_xiaomi.position = 0; //
|
||||
JZ_xiaomi.speed = 20; //
|
||||
JZ_xiaomi.K_P = 100; //
|
||||
JZ_xiaomi.K_D =20; //
|
||||
JZ_xiaomi.K_C = 2 ;
|
||||
JZ_xiaomi.Pmax =1;
|
||||
|
||||
//状态机状态初始化
|
||||
currentState1= BALL_IDLE;
|
||||
@ -66,6 +73,7 @@ Ball ::Ball()
|
||||
|
||||
}
|
||||
|
||||
|
||||
void Ball ::Extend_control(int angle)
|
||||
{
|
||||
int16_t delta[2];
|
||||
@ -92,20 +100,8 @@ void Ball ::Extend_control(int angle)
|
||||
// e_result[1] = PID_calc(&extend_speed_pid, Extern_Motor[1]->speed_rpm, delta[1]);
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
// void Ball ::Extend_control(int angle)
|
||||
// {
|
||||
// int16_t delta;
|
||||
// angleSet = angle;
|
||||
|
||||
// delta = PID_calc(&extend_angle_pid, Extern_Motor->total_angle, angleSet);
|
||||
// e_result = PID_calc(&extend_speed_pid, Extern_Motor->speed_rpm, delta);
|
||||
|
||||
// }
|
||||
|
||||
|
||||
void Ball ::Spin(float speed)
|
||||
{
|
||||
@ -130,6 +126,7 @@ void Ball::ballDown(void)
|
||||
|
||||
void Ball::ballStop(void)
|
||||
{
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保闭合气缸
|
||||
speedm=0;
|
||||
|
||||
}
|
||||
@ -150,77 +147,70 @@ void Ball::ballTake(void)
|
||||
|
||||
void Ball::ballHadling(void)
|
||||
{
|
||||
static int last_sw5 = 0; // 保存上一次拨杆状态
|
||||
ball_state = HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin); // 有球 1 无球 0
|
||||
|
||||
ball_state =HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin);//有球 1 无球 0
|
||||
switch (currentState1)
|
||||
{
|
||||
case BALL_IDLE:
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);//确保闭合气缸
|
||||
if (rc_ctrl.sw[4] > 1000||key > 0) // 检测按键是否被按下,自动回弹的
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保闭合气缸
|
||||
// 只在拨杆从非200切到200时触发
|
||||
if ((rc_ctrl.sw[5] == 200 && last_sw5 != 200) || key > 0)
|
||||
{
|
||||
speedm=-4000;
|
||||
speedm = -4000;
|
||||
currentState1 = BALL_FORWARD; // 切换到正转状态
|
||||
}
|
||||
break;
|
||||
|
||||
case BALL_FORWARD:
|
||||
|
||||
if ( hand_Motor[MOTOR_1]->speed_rpm >= 3980&&hand_Motor[MOTOR_1]->speed_rpm <= 4020 &&
|
||||
hand_Motor[MOTOR_2]->speed_rpm <= -3980&&hand_Motor[MOTOR_2]->speed_rpm >= -4020 &&
|
||||
hand_Motor[MOTOR_3]->speed_rpm <= -3980&&hand_Motor[MOTOR_3]->speed_rpm >= -4020 )
|
||||
if (hand_Motor[MOTOR_1]->speed_rpm >= 3980 && hand_Motor[MOTOR_1]->speed_rpm <= 4020 &&
|
||||
hand_Motor[MOTOR_2]->speed_rpm <= -3980 && hand_Motor[MOTOR_2]->speed_rpm >= -4020 &&
|
||||
hand_Motor[MOTOR_3]->speed_rpm <= -3980 && hand_Motor[MOTOR_3]->speed_rpm >= -4020)
|
||||
{
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET);// 打开气缸
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 打开气缸
|
||||
currentState1 = BALL_DROP; // 切换到球下落状态
|
||||
}
|
||||
break;
|
||||
|
||||
case BALL_DROP:
|
||||
|
||||
if (ball_state == 0) //读光电 有球 1 无球 0
|
||||
if (ball_state == 0) // 读光电 有球 1 无球 0
|
||||
{
|
||||
osDelay(200); // 延时200ms
|
||||
speedm=3500;
|
||||
speedm = 3500;
|
||||
currentState1 = BALL_REVERSE; // 切换到反转状态
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case BALL_REVERSE:
|
||||
|
||||
if (ball_state == 1)
|
||||
if (ball_state == 1)
|
||||
{
|
||||
|
||||
speedm=0; // 停止电机
|
||||
speedm = 0; // 停止电机
|
||||
currentState1 = BALL_CLOSE; // 切换到完成状态
|
||||
}
|
||||
|
||||
break;
|
||||
break;
|
||||
|
||||
case BALL_CLOSE:
|
||||
|
||||
if(ball_state == 1)
|
||||
{
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);
|
||||
currentState1 = BALL_FINISH; // 切换到完成状态
|
||||
}
|
||||
|
||||
break;
|
||||
if (ball_state == 1)
|
||||
{
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);
|
||||
currentState1 = BALL_FINISH; // 切换到完成状态
|
||||
}
|
||||
break;
|
||||
|
||||
case BALL_FINISH:
|
||||
osDelay(200); // 延时50ms
|
||||
key=0; // 重置按键状态
|
||||
speedm=0;
|
||||
osDelay(200); // 延时200ms
|
||||
key = 0; // 重置按键状态
|
||||
speedm = 0;
|
||||
osThreadFlagsSet(task_struct.thread.shoot, BALL_OK);
|
||||
currentState1 = BALL_IDLE; // 回到空闲状态
|
||||
currentState1 = BALL_IDLE; // 直接回到空闲状态
|
||||
break;
|
||||
|
||||
default:
|
||||
currentState1 = BALL_IDLE; // 默认回到空闲状态
|
||||
currentState1 = BALL_IDLE; // 默认回到空闲状态
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
|
||||
last_sw5 = rc_ctrl.sw[5]; // 更新上一次拨杆状态
|
||||
}
|
||||
|
||||
void Ball::Send_control()
|
||||
|
||||
@ -55,11 +55,14 @@ public:
|
||||
void ballStop(void);
|
||||
void ballTake(void);
|
||||
void Extend_control(int angle);
|
||||
|
||||
|
||||
BallState_t currentState1; // 当前状态
|
||||
int flag;//暂时还没用到
|
||||
int ball_state ;//光电检测
|
||||
|
||||
//小米电机伸缩
|
||||
JZ_xiaomi_t JZ_xiaomi;
|
||||
|
||||
//伸缩6020
|
||||
int16_t e_result[2];
|
||||
motor_measure_t * Extern_Motor[2];
|
||||
@ -72,15 +75,13 @@ public:
|
||||
volatile uint32_t flag_thread;//接收传回的线程通知
|
||||
|
||||
private:
|
||||
//滤波一下
|
||||
|
||||
|
||||
//三个摩擦轮
|
||||
static const float M3508_speed_PID[3];
|
||||
static const float M3508_angle_PID[3];
|
||||
|
||||
static const float Extend_speed_PID[3];
|
||||
static const float Extend_angle_PID[3];
|
||||
static const float Extend_angle_PID[3];
|
||||
|
||||
//电机速度pid结构体
|
||||
pid_type_def speed_pid[MOTOR_NUM];
|
||||
|
||||
@ -8,24 +8,32 @@
|
||||
#include "vofa.h"
|
||||
|
||||
extern RC_ctrl_t rc_ctrl;
|
||||
NUC_t nuc_v;
|
||||
|
||||
float vofa[8];
|
||||
#define SHOOT_SPEED 30500
|
||||
#define SHOOT_SPEED_BACK -2500
|
||||
#define Error 50
|
||||
|
||||
int test_speed =30500;
|
||||
int triggerspeed = -5000; // 扳机速度
|
||||
int test_speed =35200;
|
||||
float test_distance =1;
|
||||
float xxx;
|
||||
|
||||
#define STOP 0
|
||||
|
||||
int Torque = -5000; // 扳机恒力
|
||||
#define Trigger_Torque -5000
|
||||
#define Trigger_Slow 2000//扳机慢速 不用加负号
|
||||
|
||||
//sw[7]👆 1694 中 1000 👇306
|
||||
//sw[2]👆 1694 👇306
|
||||
|
||||
//F键 sw[0]👆 1800 中 1000 👇200
|
||||
//E键 sw[1]👆 1800 👇200
|
||||
//E键 sw[1]👆 200 shoot 中 1000 stop 👇1800 error
|
||||
//F键 sw[0]👆 1800 开 👇200 关
|
||||
//B键 sw[3]👆 200 开 👇1800 关
|
||||
|
||||
const fp32 Shoot:: M2006_speed_PID[3] = {5, 0, 0};
|
||||
const fp32 Shoot:: M2006_speed_PID[3] = {50, 0, 0};
|
||||
int t=0;
|
||||
|
||||
Shoot::Shoot()
|
||||
@ -33,8 +41,8 @@ Shoot::Shoot()
|
||||
//扳机初始化
|
||||
trigger_Motor= get_motor_point(4);
|
||||
trigger_Motor->type=M2006;
|
||||
PID_init(&speed_pid,PID_POSITION,M2006_speed_PID,6000, 1000);//pid初始化
|
||||
speedSet = 0;
|
||||
PID_init(&speed_pid,PID_POSITION,M2006_speed_PID,8000, 1000);//pid初始化
|
||||
t_speedSet = 0;
|
||||
result = 0;
|
||||
|
||||
//发射摩擦轮
|
||||
@ -46,80 +54,84 @@ Shoot::Shoot()
|
||||
|
||||
void Shoot::trigger_control()
|
||||
{
|
||||
///speedSet=speed;
|
||||
//result = PID_calc(&speed_pid, trigger_Motor->speed_rpm, speedSet);
|
||||
//result = Trigger_Torque;
|
||||
// result = PID_calc(&speed_pid,trigger_Motor->speed_rpm,t_speedSet);
|
||||
CAN_cmd_1FF(result,0,0,0,&hcan1);
|
||||
|
||||
}
|
||||
|
||||
void Shoot::shootThree()
|
||||
{
|
||||
if((rc_ctrl.sw[7]==1694))
|
||||
{
|
||||
|
||||
speed_5065 = SHOOT_SPEED;
|
||||
|
||||
}
|
||||
if((rc_ctrl.sw[7]==1000))
|
||||
{
|
||||
speed_5065=STOP;
|
||||
//发一次会堵塞另一个
|
||||
// CAN_VESC_HEAD(1);
|
||||
// CAN_VESC_HEAD(2);
|
||||
}
|
||||
if(rc_ctrl.sw[7]==306)
|
||||
{
|
||||
speed_5065=SHOOT_SPEED_BACK;
|
||||
|
||||
}
|
||||
CAN_VESC_Control(1,speed_5065,&hcan2);
|
||||
// CAN_VESC_RPM(1, speed_5065);
|
||||
// CAN_VESC_RPM(2, speed_5065);
|
||||
|
||||
// vofa[0] = motor_5065[1]->rotor_speed;
|
||||
// vofa[1] = motor_5065[0]->rotor_speed; // 发送电机速度数据
|
||||
// vofa_tx_main(vofa); // 发送数据到虚拟串口
|
||||
}
|
||||
|
||||
void Shoot::shootBack()
|
||||
{
|
||||
speed_5065=SHOOT_SPEED_BACK;
|
||||
CAN_VESC_Control(1,speed_5065,&hcan2);
|
||||
|
||||
}
|
||||
|
||||
void Shoot::shootStop()
|
||||
{
|
||||
speed_5065=STOP;
|
||||
CAN_VESC_Control(1,speed_5065,&hcan2);
|
||||
|
||||
speed_5065=STOP;
|
||||
result=STOP;//扳机慢速归位
|
||||
|
||||
|
||||
}
|
||||
|
||||
void Shoot::shootStateMachine() {
|
||||
void Shoot::errorControl()
|
||||
{
|
||||
result=Trigger_Slow;//扳机慢速归位
|
||||
speed_5065=SHOOT_SPEED_BACK; //摩擦归位
|
||||
|
||||
|
||||
}
|
||||
|
||||
distance =3.5;
|
||||
|
||||
void Shoot :: distanceGet(const NUC_t &nuc_v)
|
||||
{
|
||||
|
||||
test_distance=nuc_v.vision.x; // 获取自瞄数据
|
||||
|
||||
}
|
||||
|
||||
void Shoot::vesc_send()
|
||||
{
|
||||
CAN_VESC_Control(1, speed_5065, &hcan2);
|
||||
}
|
||||
|
||||
void Shoot::vofaWatch()
|
||||
{
|
||||
vofa[0] = motor_5065[1]->rotor_speed;
|
||||
vofa[1] = motor_5065[0]->rotor_speed; // 发送电机速度数据
|
||||
vofa[2] = speed_5065; // 发送电机速度数据
|
||||
vofa[4] = trigger_Motor->speed_rpm; // 发送扳机电机速度数据
|
||||
vofa_tx_main(vofa); // 发送数据到虚拟串口
|
||||
}
|
||||
|
||||
|
||||
void Shoot::shootStateMachine() {
|
||||
|
||||
xxx=polynomial(test_distance);
|
||||
distance =test_distance;
|
||||
switch (currentState) {
|
||||
case SHOOT_IDLE: {
|
||||
speed_5065=STOP;
|
||||
result=STOP;
|
||||
t_speedSet=STOP;
|
||||
// 空闲状态，等待遥控器输入
|
||||
if((rc_ctrl.sw[0]==1800)) {
|
||||
currentState = SHOOT_FIRE; // 切换到发射状态
|
||||
if((rc_ctrl.sw[1]==200)) {
|
||||
currentState = SHOOT_READY; // 切换到发射状态
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case SHOOT_FIRE: {
|
||||
// 发射状态，控制电机发射
|
||||
// speed_5065 = test_speed;
|
||||
speed_5065 =distanceToSpeed(distance);
|
||||
case SHOOT_READY:
|
||||
{
|
||||
// 发射状态，控制电机发射
|
||||
//speed_5065 = test_speed;
|
||||
speed_5065 =polynomial(distance);
|
||||
if(motor_5065[0]->rotor_speed>=speed_5065-Error && motor_5065[0]->rotor_speed<=speed_5065+Error &&
|
||||
motor_5065[1]->rotor_speed>=speed_5065-Error && motor_5065[1]->rotor_speed<=speed_5065+Error)
|
||||
{
|
||||
result=Trigger_Torque;//恒力扳机
|
||||
if((rc_ctrl.sw[3]==1800)) {
|
||||
currentState = SHOOT_FIRE; // 切换到发射状态
|
||||
}
|
||||
|
||||
}
|
||||
break;
|
||||
|
||||
}
|
||||
|
||||
case SHOOT_FIRE: {
|
||||
|
||||
t_speedSet=triggerspeed;// 扳机速度影响？
|
||||
// 检测光电传感器是否触发
|
||||
if (IS_PHOTOELECTRIC_TRIGGERED()) {
|
||||
currentState = SHOOT_BACK; // 切换到光电触发状态
|
||||
@ -127,11 +139,13 @@ void Shoot::shootStateMachine() {
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
|
||||
case SHOOT_BACK: {
|
||||
// 光电触发状态，停止发射
|
||||
result=STOP;
|
||||
t_speedSet=STOP;
|
||||
speed_5065=STOP;
|
||||
|
||||
|
||||
// 切换到返回状态
|
||||
currentState = SHOOT_RETURN;
|
||||
break;
|
||||
@ -140,12 +154,12 @@ void Shoot::shootStateMachine() {
|
||||
case SHOOT_RETURN: {
|
||||
// 自动返回状态，控制电机反转
|
||||
speed_5065=SHOOT_SPEED_BACK;
|
||||
result=-Trigger_Torque;
|
||||
t_speedSet=2000;
|
||||
|
||||
// 检测返回完成（可以通过时间或其他传感器判断）
|
||||
if (rc_ctrl.sw[0]==1000) { // 假设遥控器开关控制返回完成
|
||||
if (rc_ctrl.sw[1]==1000) { // 假设遥控器开关控制返回完成
|
||||
speed_5065=SHOOT_SPEED_BACK;
|
||||
result=STOP;
|
||||
t_speedSet=STOP;
|
||||
currentState = SHOOT_IDLE; // 切换回空闲状态
|
||||
}
|
||||
break;
|
||||
@ -157,21 +171,17 @@ void Shoot::shootStateMachine() {
|
||||
break;
|
||||
}
|
||||
}
|
||||
CAN_VESC_Control(1,speed_5065,&hcan2);
|
||||
|
||||
vofa[0] = motor_5065[1]->rotor_speed;
|
||||
vofa[1] = motor_5065[0]->rotor_speed; // 发送电机速度数据
|
||||
vofa[2] = speed_5065; // 发送电机速度数据
|
||||
|
||||
vofa_tx_main(vofa); // 发送数据到虚拟串口
|
||||
|
||||
result = PID_calc(&speed_pid,trigger_Motor->speed_rpm,t_speedSet);
|
||||
// CAN_VESC_Control(1,speed_5065,&hcan2);
|
||||
|
||||
}
|
||||
|
||||
//拟合函数
|
||||
float distanceToSpeed(float x) {
|
||||
return -2.3958f * x * x * x * x
|
||||
+ 59.615f * x * x * x
|
||||
- 525.63f * x * x
|
||||
+ 2136.4f * x
|
||||
+ 28001.0f;
|
||||
//MATLAB拟合函数
|
||||
float polynomial(float x) {
|
||||
return 6348.2377f * powf(x, 5.0f)
|
||||
+ -128275.8296f * powf(x, 4.0f)
|
||||
+ 1030651.0231f * powf(x, 3.0f)
|
||||
+ -4115199.9362f * powf(x, 2.0f)
|
||||
+ 8167973.1254f * x
|
||||
+ -6420648.3555f;
|
||||
}
|
||||
|
||||
@ -2,10 +2,12 @@
|
||||
#define SHOOT_HPP
|
||||
#include "djiMotor.h"
|
||||
#include "pid.h"
|
||||
#include "nuc.h"
|
||||
|
||||
// 定义状态枚举
|
||||
typedef enum {
|
||||
SHOOT_IDLE, // 空闲状态
|
||||
SHOOT_READY,
|
||||
SHOOT_FIRE, // 发射状态
|
||||
SHOOT_BACK, // 光电触发状态
|
||||
SHOOT_RETURN // 自动返回状态
|
||||
@ -28,16 +30,19 @@ class Shoot
|
||||
{
|
||||
public:
|
||||
Shoot();
|
||||
void shootThree(void);
|
||||
void shootStop(void);
|
||||
void shootBack(void);
|
||||
void shootStateMachine(void);
|
||||
void trigger_control(void);
|
||||
void errorControl(void);
|
||||
void vofaWatch(void);
|
||||
void vesc_send(void);
|
||||
void distanceGet(const NUC_t &nuc);
|
||||
float speed_5065;
|
||||
float speed_trigger;
|
||||
ShootState_t currentState;
|
||||
|
||||
int16_t result;
|
||||
float t_speedSet;
|
||||
//暂时放在公共,task里使用
|
||||
CAN_MotorFeedback_t *motor_5065[2];
|
||||
//==========================公共变量==========================
|
||||
@ -54,7 +59,7 @@ private:
|
||||
//电机速度pid结构体
|
||||
pid_type_def speed_pid;
|
||||
motor_measure_t *trigger_Motor;
|
||||
float speedSet;
|
||||
|
||||
|
||||
|
||||
};
|
||||
@ -62,4 +67,8 @@ private:
|
||||
|
||||
float distanceToSpeed(float x);
|
||||
|
||||
float polynomial(float x);
|
||||
|
||||
float polynomial2(float x);
|
||||
|
||||
#endif
|
||||
|
||||
@ -10,10 +10,10 @@ extern RC_ctrl_t rc_ctrl;
|
||||
Ball ball;
|
||||
//float vofa[8];
|
||||
|
||||
// 左旋钮 sw[2] 左1800 右200
|
||||
// 右旋钮 sw[3] 左1800 右200
|
||||
// C键 sw[4]👆 200 中1000 👇1800
|
||||
// D键 sw[5]👆 1800 👇200
|
||||
|
||||
// C键 sw[5]👆 1800 中1000 👇200
|
||||
// D键 sw[6]👆 200 外伸 👇1800 归位 //三摩擦架子
|
||||
// H键 sw[7]👆 200 👇1800 //取球
|
||||
|
||||
int angle1=34;
|
||||
int angle2=23;
|
||||
@ -26,7 +26,7 @@ int speed_set=0;
|
||||
int speed_feedback=0;
|
||||
|
||||
extern int speedm;
|
||||
extern int speedm1;
|
||||
|
||||
|
||||
void FunctionBall(void *argument)
|
||||
{
|
||||
@ -44,44 +44,47 @@ void FunctionBall(void *argument)
|
||||
abc=HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin);
|
||||
|
||||
|
||||
if(rc_ctrl.sw[2]>=1200)
|
||||
if(rc_ctrl.sw[6] == 200)
|
||||
{
|
||||
angle1=75;
|
||||
}
|
||||
if(rc_ctrl.sw[2]<1200)
|
||||
if(rc_ctrl.sw[6]==1800)
|
||||
{
|
||||
angle1=0;
|
||||
|
||||
}
|
||||
|
||||
//运球
|
||||
if(rc_ctrl.sw[3]>=1200)
|
||||
{
|
||||
a1=1;
|
||||
//👇
|
||||
ball.ballHadling(); // 处理摩擦轮转动
|
||||
}
|
||||
if(rc_ctrl.sw[5]==200)
|
||||
{
|
||||
//👇
|
||||
ball.ballHadling(); // 处理摩擦轮转动
|
||||
}
|
||||
//下球
|
||||
if(rc_ctrl.sw[4]==200)
|
||||
if(rc_ctrl.sw[5]==1800)
|
||||
{
|
||||
//👆
|
||||
ball.ballDown();
|
||||
}
|
||||
//中间
|
||||
if(rc_ctrl.sw[7]==200)
|
||||
{
|
||||
if(rc_ctrl.sw[5]==1000)
|
||||
{
|
||||
//👆
|
||||
ball.ballDown();
|
||||
}
|
||||
//回弹停止
|
||||
if(rc_ctrl.sw[3]<1000)
|
||||
{
|
||||
//👆
|
||||
//中
|
||||
ball.ballStop();
|
||||
ball.ballHadling(); // 处理摩擦轮转动
|
||||
}
|
||||
if(rc_ctrl.sw[7]==1800)
|
||||
{
|
||||
ball.ballTake();
|
||||
}
|
||||
|
||||
}
|
||||
if(rc_ctrl.sw[7]==1800)
|
||||
{
|
||||
//ball.ballTake();
|
||||
|
||||
|
||||
}
|
||||
|
||||
// CAN_XiaoMi(1,&ball.JZ_xiaomi,&hcan2);
|
||||
|
||||
ball.Extend_control(angle1);
|
||||
ball.Spin(speedm);
|
||||
ball.Send_control();
|
||||
|
||||
|
||||
@ -22,7 +22,6 @@ void Function_nuc(void *argument)
|
||||
|
||||
NUC_Init(&cmd_fromnuc);
|
||||
|
||||
|
||||
uint32_t tick = osKernelGetTickCount();
|
||||
|
||||
while(1)
|
||||
|
||||
@ -15,10 +15,8 @@ int shoot_flag = 0;
|
||||
|
||||
int a2;
|
||||
|
||||
// sw[0]2 下306上1694 sw[5]3前306后1694 sw[4]4前1694后306 sw[1]xuan1 sw[3]xuan2
|
||||
|
||||
//F键 sw[0]👆 1800 中 1000 👇200
|
||||
//E键 sw[1]👆 1800 👇200
|
||||
//E键 sw[0]👆 200 中 1000 👇1800
|
||||
//F键 sw[1]👆 1800 👇200
|
||||
|
||||
void FunctionShoot(void *argument)
|
||||
{
|
||||
@ -40,35 +38,46 @@ while(1)
|
||||
if(shoot.flag_thread & BALL_OK)
|
||||
{
|
||||
a2=2;
|
||||
// shoot.shootThree();
|
||||
}
|
||||
|
||||
|
||||
shoot_flag=HAL_GPIO_ReadPin(STOP_GPIO_Port, STOP_Pin);
|
||||
|
||||
if (osMessageQueueGet(task_struct.msgq.nuc, &nucData, NULL, 0) == osOK)
|
||||
{
|
||||
// 使用接收到的视觉数据调整云台
|
||||
//gimbal.gimbalVision(nucData);
|
||||
shoot.distanceGet(nucData);
|
||||
}
|
||||
if(rc_ctrl.sw[1]>1000)
|
||||
|
||||
|
||||
if(rc_ctrl.sw[0]>1000)
|
||||
{
|
||||
shoot.shootStateMachine();
|
||||
if(rc_ctrl.sw[0]==200)
|
||||
|
||||
if(rc_ctrl.sw[1]==1800)
|
||||
{
|
||||
shoot.shootBack();
|
||||
shoot.errorControl();
|
||||
}
|
||||
if(rc_ctrl.sw[0]==1000)
|
||||
if(rc_ctrl.sw[1]==1000)
|
||||
{
|
||||
shoot.shootStop();
|
||||
shoot.shootStateMachine();//shoot
|
||||
}
|
||||
if(rc_ctrl.sw[1]==200)
|
||||
{
|
||||
shoot.shootStateMachine();//shoot
|
||||
}
|
||||
}
|
||||
if(rc_ctrl.sw[1]==200)
|
||||
if(rc_ctrl.sw[0]==200)
|
||||
{
|
||||
shoot.shootStop();
|
||||
|
||||
}
|
||||
|
||||
shoot.trigger_control();
|
||||
shoot.vesc_send(); // 发送电机速度数据
|
||||
|
||||
shoot.trigger_control();
|
||||
|
||||
shoot.vofaWatch();
|
||||
|
||||
osDelay(2);
|
||||
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
|
||||
Loading…
Reference in New Issue
Block a user