基本进攻全过程
This commit is contained in:
parent
ffd13d2e63
commit
3654e052e7
@ -57,7 +57,6 @@ void EXTI0_IRQHandler(void);
|
||||
void EXTI4_IRQHandler(void);
|
||||
void DMA1_Stream1_IRQHandler(void);
|
||||
void CAN1_RX0_IRQHandler(void);
|
||||
void CAN1_RX1_IRQHandler(void);
|
||||
void EXTI9_5_IRQHandler(void);
|
||||
void TIM1_UP_TIM10_IRQHandler(void);
|
||||
void USART1_IRQHandler(void);
|
||||
@ -65,7 +64,6 @@ void USART3_IRQHandler(void);
|
||||
void DMA2_Stream1_IRQHandler(void);
|
||||
void DMA2_Stream2_IRQHandler(void);
|
||||
void DMA2_Stream3_IRQHandler(void);
|
||||
void CAN2_RX0_IRQHandler(void);
|
||||
void CAN2_RX1_IRQHandler(void);
|
||||
void DMA2_Stream5_IRQHandler(void);
|
||||
void DMA2_Stream6_IRQHandler(void);
|
||||
|
||||
@ -81,7 +81,7 @@ void MX_CAN2_Init(void)
|
||||
hcan2.Init.AutoWakeUp = DISABLE;
|
||||
hcan2.Init.AutoRetransmission = DISABLE;
|
||||
hcan2.Init.ReceiveFifoLocked = DISABLE;
|
||||
hcan2.Init.TransmitFifoPriority = ENABLE;
|
||||
hcan2.Init.TransmitFifoPriority = DISABLE;
|
||||
if (HAL_CAN_Init(&hcan2) != HAL_OK)
|
||||
{
|
||||
Error_Handler();
|
||||
@ -124,8 +124,6 @@ void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle)
|
||||
/* CAN1 interrupt Init */
|
||||
HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 5, 0);
|
||||
HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn);
|
||||
HAL_NVIC_SetPriority(CAN1_RX1_IRQn, 5, 0);
|
||||
HAL_NVIC_EnableIRQ(CAN1_RX1_IRQn);
|
||||
/* USER CODE BEGIN CAN1_MspInit 1 */
|
||||
|
||||
/* USER CODE END CAN1_MspInit 1 */
|
||||
@ -155,8 +153,6 @@ void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle)
|
||||
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
|
||||
|
||||
/* CAN2 interrupt Init */
|
||||
HAL_NVIC_SetPriority(CAN2_RX0_IRQn, 5, 0);
|
||||
HAL_NVIC_EnableIRQ(CAN2_RX0_IRQn);
|
||||
HAL_NVIC_SetPriority(CAN2_RX1_IRQn, 5, 0);
|
||||
HAL_NVIC_EnableIRQ(CAN2_RX1_IRQn);
|
||||
/* USER CODE BEGIN CAN2_MspInit 1 */
|
||||
@ -187,7 +183,6 @@ void HAL_CAN_MspDeInit(CAN_HandleTypeDef* canHandle)
|
||||
|
||||
/* CAN1 interrupt Deinit */
|
||||
HAL_NVIC_DisableIRQ(CAN1_RX0_IRQn);
|
||||
HAL_NVIC_DisableIRQ(CAN1_RX1_IRQn);
|
||||
/* USER CODE BEGIN CAN1_MspDeInit 1 */
|
||||
|
||||
/* USER CODE END CAN1_MspDeInit 1 */
|
||||
@ -211,7 +206,6 @@ void HAL_CAN_MspDeInit(CAN_HandleTypeDef* canHandle)
|
||||
HAL_GPIO_DeInit(GPIOB, GPIO_PIN_5|GPIO_PIN_6);
|
||||
|
||||
/* CAN2 interrupt Deinit */
|
||||
HAL_NVIC_DisableIRQ(CAN2_RX0_IRQn);
|
||||
HAL_NVIC_DisableIRQ(CAN2_RX1_IRQn);
|
||||
/* USER CODE BEGIN CAN2_MspDeInit 1 */
|
||||
|
||||
|
||||
@ -250,20 +250,6 @@ void CAN1_RX0_IRQHandler(void)
|
||||
/* USER CODE END CAN1_RX0_IRQn 1 */
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief This function handles CAN1 RX1 interrupt.
|
||||
*/
|
||||
void CAN1_RX1_IRQHandler(void)
|
||||
{
|
||||
/* USER CODE BEGIN CAN1_RX1_IRQn 0 */
|
||||
|
||||
/* USER CODE END CAN1_RX1_IRQn 0 */
|
||||
HAL_CAN_IRQHandler(&hcan1);
|
||||
/* USER CODE BEGIN CAN1_RX1_IRQn 1 */
|
||||
|
||||
/* USER CODE END CAN1_RX1_IRQn 1 */
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief This function handles EXTI line[9:5] interrupts.
|
||||
*/
|
||||
@ -363,20 +349,6 @@ void DMA2_Stream3_IRQHandler(void)
|
||||
/* USER CODE END DMA2_Stream3_IRQn 1 */
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief This function handles CAN2 RX0 interrupts.
|
||||
*/
|
||||
void CAN2_RX0_IRQHandler(void)
|
||||
{
|
||||
/* USER CODE BEGIN CAN2_RX0_IRQn 0 */
|
||||
|
||||
/* USER CODE END CAN2_RX0_IRQn 0 */
|
||||
HAL_CAN_IRQHandler(&hcan2);
|
||||
/* USER CODE BEGIN CAN2_RX0_IRQn 1 */
|
||||
|
||||
/* USER CODE END CAN2_RX0_IRQn 1 */
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief This function handles CAN2 RX1 interrupt.
|
||||
*/
|
||||
|
||||
16
MDK-ARM/.vscode/keil-assistant.log
vendored
16
MDK-ARM/.vscode/keil-assistant.log
vendored
@ -44,3 +44,19 @@
|
||||
|
||||
[info] Log at : 2025/6/20|09:52:08|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/20|19:02:32|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/21|01:54:17|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/21|09:59:57|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/21|17:02:34|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/22|13:36:33|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/23|15:15:24|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/23|16:29:17|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/24|12:24:51|GMT+0800
|
||||
|
||||
|
||||
6
MDK-ARM/.vscode/uv4.log
vendored
6
MDK-ARM/.vscode/uv4.log
vendored
@ -1,4 +1,8 @@
|
||||
*** Using Compiler 'V5.06 update 7 (build 960)', folder: 'D:\keil\ARM\ARMCC\Bin'
|
||||
Build target 'R1'
|
||||
compiling ball.cpp...
|
||||
linking...
|
||||
Program Size: Code=30768 RO-data=1832 RW-data=260 ZI-data=32244
|
||||
FromELF: creating hex file...
|
||||
"R1\R1.axf" - 0 Error(s), 0 Warning(s).
|
||||
Build Time Elapsed: 00:00:07
|
||||
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/6/20 9:52:48
|
||||
2025/6/24 2:11:53
|
||||
@ -150,7 +150,7 @@
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
<Key>ST-LINKIII-KEIL_SWO</Key>
|
||||
<Name>-U-O142 -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 -FO7 -FD20000000 -FC800 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
|
||||
<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/>
|
||||
@ -168,47 +168,17 @@
|
||||
<Ww>
|
||||
<count>2</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>cmd_fromnuc</ItemText>
|
||||
<ItemText>ball,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>3</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>nucbuf</ItemText>
|
||||
<ItemText>aaaa,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>4</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>goangle,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>5</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>ball,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>6</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>task_struct,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>7</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>abc,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>8</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>ball_state,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>9</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>triggerCount,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>10</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>last_ball_state,0x0A</ItemText>
|
||||
<ItemText>aaaxxx,0x0A</ItemText>
|
||||
</Ww>
|
||||
</WatchWindow1>
|
||||
<Tracepoint>
|
||||
|
||||
@ -23,3 +23,14 @@ r1上层
|
||||
|
||||
+ 用了将运球和伸缩绑定到一起
|
||||
+ 串口收数加个滤波
|
||||
|
||||
## 思路
|
||||
|
||||
+ 👆 传球档 👆 配合档
|
||||
+ 中 初始档 中 初始档
|
||||
+ 👇 发射档 👇 运球档
|
||||
+ 我现在已经收到了运球完成的信号
|
||||
+ 发射就应该去蓄力接球了
|
||||
+ 蓄力到位,收到掉落信号和已伸出信号
|
||||
+ 根据视觉拟合信息的动态调整
|
||||
+ 拨置👇发射清空掉落信号 (发送归位信号?)
|
||||
|
||||
4
R1.ioc
4
R1.ioc
@ -21,7 +21,7 @@ CAN2.CalculateTimeQuantum=71.42857142857143
|
||||
CAN2.IPParameters=CalculateTimeQuantum,CalculateTimeBit,CalculateBaudRate,BS1,BS2,Prescaler,TXFP,NART
|
||||
CAN2.NART=DISABLE
|
||||
CAN2.Prescaler=3
|
||||
CAN2.TXFP=ENABLE
|
||||
CAN2.TXFP=DISABLE
|
||||
Dma.Request0=SPI1_RX
|
||||
Dma.Request1=SPI1_TX
|
||||
Dma.Request2=USART3_RX
|
||||
@ -181,8 +181,6 @@ MxCube.Version=6.14.1
|
||||
MxDb.Version=DB.6.0.141
|
||||
NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
|
||||
NVIC.CAN1_RX0_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
|
||||
NVIC.CAN1_RX1_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
|
||||
NVIC.CAN2_RX0_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
|
||||
NVIC.CAN2_RX1_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
|
||||
NVIC.DMA1_Stream1_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
|
||||
NVIC.DMA2_Stream1_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
|
||||
|
||||
@ -14,18 +14,17 @@
|
||||
#ifndef _AUTO
|
||||
#define _AUTO 0
|
||||
#endif
|
||||
|
||||
|
||||
#define TESTUP 1
|
||||
#define ONE_CONTROL 0
|
||||
|
||||
//是否使用大疆DT7遥控器
|
||||
#ifndef DT7
|
||||
#define DT7 0
|
||||
#endif
|
||||
// //是否使用三摩擦
|
||||
// #ifndef HANDLING3
|
||||
// #define HANDLING3 1
|
||||
// #endif
|
||||
//云台使用类型
|
||||
#ifndef GM6020ING
|
||||
#define GM6020ING 1
|
||||
#endif
|
||||
|
||||
|
||||
//=============================================
|
||||
//================任务通知,时间组================//
|
||||
//事件组
|
||||
@ -35,17 +34,13 @@
|
||||
//================任务通知================//
|
||||
//运球
|
||||
#define BALL_DOWN (1<<1)
|
||||
//接到放球命令
|
||||
#define PUT_CMD (1<<2)
|
||||
//运球结束
|
||||
#define PUT_OK (1<<2)
|
||||
|
||||
//运球结束
|
||||
#define PREPARE (1<<0)
|
||||
|
||||
#define EXTEND_OK (1<<5)
|
||||
//伸缩结束
|
||||
#define EXTEND_OK (1<<3)
|
||||
|
||||
|
||||
//take任务要等待上面两个标志位
|
||||
#define TAKE_WAIT (0x0C)
|
||||
//要发送ok了
|
||||
#define BALL_SEND (1<<6)
|
||||
//能够处理放球命令
|
||||
|
||||
@ -66,17 +66,17 @@
|
||||
(ptr)->offset_ecd = (ptr)->ecd; \
|
||||
}
|
||||
|
||||
static void VescMotor_Decode(CAN_MotorFeedback_t *feedback, CAN_RxHeaderTypeDef *rx_header, const uint8_t *rx_data)
|
||||
{
|
||||
// 检查接收到的ID是否匹配我们期望的VESC状态消息ID
|
||||
// 提取数据字段
|
||||
feedback->rotor_speed = ((int32_t)(rx_data[0]) << 24) | ((int32_t)(rx_data[1]) << 16) | ((int32_t)(rx_data[2]) << 8) | ((int32_t)(rx_data[3]));
|
||||
feedback->torque_current = ((int16_t)(rx_data[5]) << 8) | (int16_t)(rx_data[4]);
|
||||
feedback->torque_current /= 1000; // 将单位从 0.1A 转换为实际电流值
|
||||
feedback->duty_cycle = ((int16_t)(rx_data[7]) << 8) | (int16_t)(rx_data[6]);
|
||||
feedback->duty_cycle /= 1000.0f; // 将单位从千分之一转换为实际的占空比值 (-1.0 到 1.0)
|
||||
// static void VescMotor_Decode(CAN_MotorFeedback_t *feedback, CAN_RxHeaderTypeDef *rx_header, const uint8_t *rx_data)
|
||||
// {
|
||||
// // 检查接收到的ID是否匹配我们期望的VESC状态消息ID
|
||||
// // 提取数据字段
|
||||
// feedback->rotor_speed = ((int32_t)(rx_data[0]) << 24) | ((int32_t)(rx_data[1]) << 16) | ((int32_t)(rx_data[2]) << 8) | ((int32_t)(rx_data[3]));
|
||||
// feedback->torque_current = ((int16_t)(rx_data[5]) << 8) | (int16_t)(rx_data[4]);
|
||||
// feedback->torque_current /= 1000; // 将单位从 0.1A 转换为实际电流值
|
||||
// feedback->duty_cycle = ((int16_t)(rx_data[7]) << 8) | (int16_t)(rx_data[6]);
|
||||
// feedback->duty_cycle /= 1000.0f; // 将单位从千分之一转换为实际的占空比值 (-1.0 到 1.0)
|
||||
|
||||
}
|
||||
// }
|
||||
|
||||
//小米
|
||||
#define ID_xiaomi 1
|
||||
@ -178,23 +178,6 @@ void djiMotorEncode()
|
||||
|
||||
void can2MotorEncode()
|
||||
{
|
||||
switch (rx_header.ExtId) {
|
||||
|
||||
case CAN_VESC5065_M1_MSG1:
|
||||
// 存储消息到对应的电机结构体中
|
||||
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])
|
||||
{
|
||||
@ -239,6 +222,8 @@ void Dji_Motor_CB()
|
||||
void can2_Motor_CB(void)
|
||||
{
|
||||
HAL_CAN_GetRxMessage(&hcan2, CAN_RX_FIFO1, &rx_header, rx_data);
|
||||
|
||||
//osThreadFlagsSet(thread_alert, EVENT_CAN);
|
||||
}
|
||||
|
||||
/**
|
||||
@ -249,12 +234,15 @@ void can2_Motor_CB(void)
|
||||
void djiInit(void)
|
||||
{
|
||||
thread_alert = osThreadGetId();
|
||||
|
||||
|
||||
|
||||
BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_RX_FIFO0_MSG_PENDING_CB,
|
||||
Dji_Motor_CB);
|
||||
BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_RX_FIFO1_MSG_PENDING_CB,
|
||||
can2_Motor_CB);
|
||||
|
||||
can_filter_init();
|
||||
can_filter_init();
|
||||
}
|
||||
|
||||
/**
|
||||
@ -486,6 +474,27 @@ return DEVICE_OK;
|
||||
feedback->can_id = rx_data[0];
|
||||
}
|
||||
|
||||
void XiaomiWait_init(int id,CAN_HandleTypeDef*hcan)
|
||||
{
|
||||
uint32_t send_mail_box;
|
||||
|
||||
tx_message.StdId = id;
|
||||
tx_message.IDE = CAN_ID_STD;
|
||||
tx_message.RTR = CAN_RTR_DATA;
|
||||
tx_message.DLC = 0x08;
|
||||
|
||||
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;
|
||||
|
||||
HAL_CAN_AddTxMessage(hcan, &tx_message, can_send_data, &send_mail_box);
|
||||
}
|
||||
|
||||
|
||||
void CAN_XiaoMi(int id,JZ_xiaomi_t *JZ_xiaomi,CAN_HandleTypeDef*hcan)
|
||||
{
|
||||
@ -507,12 +516,12 @@ void CAN_XiaoMi(int id,JZ_xiaomi_t *JZ_xiaomi,CAN_HandleTypeDef*hcan)
|
||||
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[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[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位
|
||||
can_send_data[7] = current_code & 0xFF; // 电流低8位 力矩
|
||||
|
||||
}
|
||||
else
|
||||
|
||||
@ -199,6 +199,7 @@ int8_t CAN_VESC_Control(int id,int speed ,CAN_HandleTypeDef*hcan);
|
||||
|
||||
void Parse_CAN_Response(uint8_t* rx_data, MotorFeedback_t* feedback) ;
|
||||
|
||||
void XiaomiWait_init(int id,CAN_HandleTypeDef*hcan);
|
||||
|
||||
void CAN_XiaoMi(int id,JZ_xiaomi_t *JZ_xiaomi,CAN_HandleTypeDef*hcan);
|
||||
|
||||
|
||||
@ -86,7 +86,8 @@ int8_t NUC_RawParse(NUC_t *n) {
|
||||
case VISION:
|
||||
/* 协议格式
|
||||
0xFF HEAD
|
||||
0x02 控制帧
|
||||
0x09
|
||||
控制帧
|
||||
0x01 相机帧
|
||||
x fp32
|
||||
y fp32
|
||||
|
||||
@ -22,18 +22,18 @@ void user_delay_ms(uint16_t ms)
|
||||
}
|
||||
}
|
||||
|
||||
//¸¡µãÊý·¶Î§ÏÞÖÆ
|
||||
void abs_limit_fp(fp32 *num, fp32 Limit)
|
||||
{
|
||||
if (*num > Limit)
|
||||
{
|
||||
*num = Limit;
|
||||
}
|
||||
else if (*num < -Limit)
|
||||
{
|
||||
*num = -Limit;
|
||||
}
|
||||
}
|
||||
// //¸¡µãÊý·¶Î§ÏÞÖÆ
|
||||
// void abs_limit_fp(fp32 *num, fp32 Limit)
|
||||
// {
|
||||
// if (*num > Limit)
|
||||
// {
|
||||
// *num = Limit;
|
||||
// }
|
||||
// else if (*num < -Limit)
|
||||
// {
|
||||
// *num = -Limit;
|
||||
// }
|
||||
// }
|
||||
|
||||
//ÕûÊý·¶Î§ÏÞÖÆ
|
||||
void abs_limit_int(int64_t *num, int64_t Limit)
|
||||
@ -137,3 +137,33 @@ bool is_reached_multiple(float current1, float current2, float current3, float t
|
||||
}
|
||||
return false; // 未满足条件达到阈值，返回 0
|
||||
}
|
||||
|
||||
|
||||
//限幅
|
||||
void abs_limit_fp(fp32 *num, fp32 Limit)
|
||||
{
|
||||
if (*num > Limit)
|
||||
{
|
||||
*num = Limit;
|
||||
}
|
||||
else if (*num < -Limit)
|
||||
{
|
||||
*num = -Limit;
|
||||
}
|
||||
}
|
||||
//改原始值，限制最大最小
|
||||
fp32 abs_limit_min_max_fp(fp32 *num, fp32 Limit_min,fp32 Limit_max)
|
||||
{
|
||||
if (*num > Limit_max)
|
||||
{
|
||||
*num = Limit_max;
|
||||
return Limit_max;
|
||||
}
|
||||
else if (*num < Limit_min)
|
||||
{
|
||||
*num = Limit_min;
|
||||
return Limit_min;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -34,6 +34,8 @@ float expo_map(float value, float expo_factor);
|
||||
|
||||
bool is_reached_multiple(float current1, float current2, float current3, float target1, float target2, float target3, float mistake, int threshold);
|
||||
|
||||
fp32 abs_limit_min_max_fp(fp32 *num, fp32 Limit_min,fp32 Limit_max);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -7,13 +7,10 @@
|
||||
#include "shoot.hpp"
|
||||
|
||||
extern RC_ctrl_t rc_ctrl;
|
||||
extern int key;
|
||||
|
||||
//伸缩
|
||||
#define I_ANGLE 0
|
||||
#define O_ANGLE 140
|
||||
|
||||
|
||||
#define I_ANGLE 147
|
||||
#define O_ANGLE 187
|
||||
|
||||
//PE11 气缸
|
||||
|
||||
@ -25,12 +22,20 @@ Ball ::Ball()
|
||||
feedback=get_CyberGear_point();
|
||||
|
||||
//小米电机初始化
|
||||
xiaomi.position = 0; //
|
||||
xiaomi.speed = 20; //
|
||||
xiaomi.K_P = 100; //
|
||||
xiaomi.K_D =20; //
|
||||
xiaomi.K_C = 2 ;
|
||||
xiaomi.Pmax =1;
|
||||
xiaomi.position = I_ANGLE; //
|
||||
xiaomi.speed = 25; //
|
||||
xiaomi.K_P = 80; // 位置增益
|
||||
xiaomi.K_D =20; // 位置阻尼
|
||||
xiaomi.K_C = 50 ; // 力矩
|
||||
xiaomi.Pmax =1; //好像没啥用
|
||||
|
||||
// //小米电机初始化
|
||||
// xiaomi.position = 0; //
|
||||
// xiaomi.speed = 0; //
|
||||
// xiaomi.K_P = 0; //
|
||||
// xiaomi.K_D =0; //
|
||||
// xiaomi.K_C = 0 ;
|
||||
// xiaomi.Pmax =0;
|
||||
|
||||
//状态机状态初始化
|
||||
currentState1= BALL_IDLE;
|
||||
@ -68,7 +73,15 @@ void Ball::rc_mode()
|
||||
|
||||
}
|
||||
|
||||
#if TESTBALL
|
||||
void Ball::Send_control()
|
||||
{
|
||||
|
||||
CAN_XiaoMi(1,&xiaomi,&hcan2);
|
||||
osDelay(1);
|
||||
|
||||
}
|
||||
|
||||
#if TESTUP
|
||||
|
||||
void Ball::ballDown(void)
|
||||
{
|
||||
@ -82,11 +95,11 @@ void Ball::Move_Extend()
|
||||
{
|
||||
if(extern_key==IN)
|
||||
{
|
||||
xiaomi.position = 0;
|
||||
xiaomi.position = I_ANGLE;
|
||||
}
|
||||
if(extern_key==OUT)
|
||||
{
|
||||
xiaomi.position = 90;
|
||||
xiaomi.position = O_ANGLE;
|
||||
}
|
||||
}
|
||||
|
||||
@ -124,16 +137,6 @@ void Ball::ball_control()
|
||||
|
||||
}
|
||||
|
||||
|
||||
void Ball::Send_control()
|
||||
{
|
||||
|
||||
CAN_XiaoMi(1,&xiaomi,&hcan2);
|
||||
osDelay(1);
|
||||
|
||||
}
|
||||
|
||||
|
||||
int ball_state = 0;
|
||||
int triggerCount = 0; // 光电传感器触发计数
|
||||
int last_ball_state = 1; // 上一次的光电状态
|
||||
@ -146,9 +149,8 @@ void Ball::ballHadling(void)
|
||||
case BALL_IDLE:
|
||||
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); //确保爪气缸关闭
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||||
if (key > 0 ||rc_key == DOWN2) // 检测按键是否被按下
|
||||
if (rc_key == DOWN2) // 检测按键是否被按下
|
||||
{
|
||||
key = 0; // 重置按键状态
|
||||
triggerCount = 0; // 重置触发计数
|
||||
currentState1 = BALL_FORWARD;
|
||||
}
|
||||
@ -177,7 +179,6 @@ void Ball::ballHadling(void)
|
||||
break;
|
||||
|
||||
case BALL_FLAG:
|
||||
osDelay(10); // 延时 50ms
|
||||
if (triggerCount == 1 && ball_state == 0 && last_ball_state == 1) // 第二次检测到球
|
||||
{
|
||||
triggerCount++; // 增加触发计数
|
||||
@ -189,7 +190,7 @@ void Ball::ballHadling(void)
|
||||
case BALL_CLOSE:
|
||||
if (triggerCount == 2) // 确保是第二次触发
|
||||
{
|
||||
osDelay(50);
|
||||
//osDelay(5);
|
||||
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); // 闭合气缸爪子
|
||||
currentState1 = BALL_FINISH; // 切换到反转状态
|
||||
}
|
||||
@ -199,7 +200,6 @@ void Ball::ballHadling(void)
|
||||
osDelay(50); // 延时 50ms
|
||||
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); // 确保气缸爪子闭合
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||||
key = 0; // 重置按键状态
|
||||
triggerCount = 0; // 重置触发计数
|
||||
|
||||
//currentState1 = BALL_IDLE; // 回到空闲状态
|
||||
@ -218,63 +218,6 @@ void Ball::ballHadling(void)
|
||||
|
||||
#if ONE_CONTROL
|
||||
|
||||
//任务通知来作全过程
|
||||
|
||||
void Ball::ballDown(void)
|
||||
{
|
||||
xiaomi.position = I_ANGLE;
|
||||
if(feedback->position_deg== I_ANGLE) // 确保伸缩电机到位
|
||||
{
|
||||
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_SET); // 打开气缸爪子
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||||
}
|
||||
if(ball_state==0)//检测到球
|
||||
{
|
||||
osThreadFlagsSet(task_struct.thread.ball, BALL_DOWN);
|
||||
//osThreadFlagsSet(task_struct.thread.shoot, BALL_DOWN);
|
||||
xiaomi.position = O_ANGLE;
|
||||
|
||||
}
|
||||
if(hand_thread & BALL_DOWN)
|
||||
{
|
||||
if(feedback->position_deg== O_ANGLE)
|
||||
{
|
||||
osThreadFlagsSet(task_struct.thread.shoot, EXTEND_OK); //告诉发射球出去了
|
||||
osThreadFlagsSet(task_struct.thread.ball, EXTEND_OK); //告诉发射球出去了
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
void Ball::Idle_control()
|
||||
{
|
||||
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); //确保爪气缸关闭
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||||
|
||||
if(hand_thread & EXTEND_OK)
|
||||
{
|
||||
xiaomi.position = O_ANGLE; // 伸缩电机伸出去
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
xiaomi.position = I_ANGLE; // 伸缩电机回位,底盘可能跑的更好
|
||||
|
||||
}
|
||||
|
||||
if (currentState1 == BALL_FINISH)
|
||||
{
|
||||
currentState1 = BALL_IDLE;
|
||||
}
|
||||
else {
|
||||
currentState1 = BALL_IDLE; // 默认回到空闲状态
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void Ball::ball_control()
|
||||
{
|
||||
hand_thread = osThreadFlagsGet(); // 获取任务通知标志位
|
||||
@ -289,9 +232,9 @@ void Ball::ball_control()
|
||||
ballDown();
|
||||
break;
|
||||
|
||||
case DOWN2:
|
||||
ballHadling();
|
||||
|
||||
case DOWN2:
|
||||
osThreadFlagsSet(task_struct.thread.shoot, PREPARE); // 通知发射任务快点去蓄力
|
||||
ballHadling();
|
||||
break;
|
||||
|
||||
}
|
||||
@ -301,14 +244,62 @@ void Ball::ball_control()
|
||||
}
|
||||
|
||||
|
||||
void Ball::Send_control()
|
||||
{
|
||||
|
||||
CAN_XiaoMi(1,&xiaomi,&hcan2);
|
||||
osDelay(1);
|
||||
//任务通知来作全过程
|
||||
|
||||
void Ball::ballDown(void)
|
||||
{
|
||||
switch(currentState1)
|
||||
{
|
||||
case BALL_IDLE:
|
||||
xiaomi.position = I_ANGLE; // 保持收回
|
||||
if(feedback->position_deg >= I_ANGLE-0.8 && feedback->position_deg <= I_ANGLE+0.8)
|
||||
{
|
||||
currentState1 = EXTEND_DOWN;
|
||||
}
|
||||
break;
|
||||
|
||||
case EXTEND_DOWN:
|
||||
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_SET);
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);
|
||||
// 检测到球自由下落一次就切换状态
|
||||
if(ball_state == 0)
|
||||
{
|
||||
currentState1 = EXTEND_OUT;
|
||||
}
|
||||
break;
|
||||
|
||||
case EXTEND_OUT:
|
||||
xiaomi.position = O_ANGLE; // 保持伸出
|
||||
if(feedback->position_deg >= O_ANGLE-0.2 && feedback->position_deg <= O_ANGLE+0.2)
|
||||
{
|
||||
osThreadFlagsSet(task_struct.thread.shoot, EXTEND_OK);
|
||||
currentState1 = EXTEND_FINISH; // 保持伸出，直到拨杆复位
|
||||
}
|
||||
break;
|
||||
|
||||
case EXTEND_FINISH:
|
||||
xiaomi.position = O_ANGLE; // 一直保持伸出
|
||||
// 等待拨杆复位（如切到MIDDLE2），Idle_control会负责回位
|
||||
break;
|
||||
|
||||
default:
|
||||
currentState1 = BALL_IDLE;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void Ball::Idle_control()
|
||||
{
|
||||
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); // 确保爪气缸关闭
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||||
|
||||
osThreadFlagsClear(PREPARE);
|
||||
osThreadFlagsClear(EXTEND_OK);
|
||||
|
||||
// 拨杆回到中间挡位时，回位并重置状态机
|
||||
xiaomi.position = I_ANGLE;
|
||||
currentState1 = BALL_IDLE;
|
||||
}
|
||||
|
||||
int ball_state = 0;
|
||||
int triggerCount = 0; // 光电传感器触发计数
|
||||
@ -322,11 +313,15 @@ void Ball::ballHadling(void)
|
||||
case BALL_IDLE:
|
||||
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); //确保爪气缸关闭
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||||
if (key > 0 ||rc_key == DOWN2) // 检测按键是否被按下
|
||||
{
|
||||
key = 0; // 重置按键状态
|
||||
if (rc_key == DOWN2)
|
||||
{
|
||||
xiaomi.position = O_ANGLE;//外伸
|
||||
triggerCount = 0; // 重置触发计数
|
||||
currentState1 = BALL_FORWARD;
|
||||
if(feedback->position_deg>= O_ANGLE-1)// 确保伸缩电机到位
|
||||
{
|
||||
currentState1 = BALL_FORWARD;
|
||||
}
|
||||
|
||||
}
|
||||
break;
|
||||
|
||||
@ -342,7 +337,6 @@ void Ball::ballHadling(void)
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 关闭下气缸
|
||||
if (ball_state == 0 && last_ball_state == 1) // 检测到状态从无球变为有球
|
||||
{
|
||||
//osDelay(10); // 延时去抖动
|
||||
triggerCount++; // 增加触发计数
|
||||
if (triggerCount == 1) // 第一次触发
|
||||
{
|
||||
@ -353,7 +347,7 @@ void Ball::ballHadling(void)
|
||||
break;
|
||||
|
||||
case BALL_FLAG:
|
||||
osDelay(10); // 延时 50ms
|
||||
// osDelay(10); // 延时 50ms
|
||||
if (triggerCount == 1 && ball_state == 0 && last_ball_state == 1) // 第二次检测到球
|
||||
{
|
||||
triggerCount++; // 增加触发计数
|
||||
@ -375,11 +369,9 @@ void Ball::ballHadling(void)
|
||||
osDelay(50); // 延时 50ms
|
||||
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); // 确保气缸爪子闭合
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||||
key = 0; // 重置按键状态
|
||||
|
||||
triggerCount = 0; // 重置触发计数
|
||||
|
||||
//currentState1 = BALL_IDLE; // 回到空闲状态
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
|
||||
@ -11,22 +11,23 @@
|
||||
#include "filter.h"
|
||||
#include "calc_lib.h"
|
||||
|
||||
#define TESTBALL 0
|
||||
#define ONE_CONTROL 1
|
||||
|
||||
|
||||
// 定义状态枚举
|
||||
typedef enum {
|
||||
BALL_IDLE, // 空闲状态
|
||||
BALL_FORWARD, //
|
||||
BALL_DROP, //
|
||||
BALL_REVERSE, //
|
||||
BALL_FLAG,
|
||||
BALL_CLOSE, // 关闭状态
|
||||
BALL_FINISH, // 完成状态
|
||||
|
||||
//持球状态
|
||||
BALL_TAKE,
|
||||
BALL_LOSE
|
||||
EXTEND_DOWN,
|
||||
EXTEND_OUT,
|
||||
EXTEND_FINISH,
|
||||
BALL_LOSE,
|
||||
|
||||
} BallState_t;
|
||||
|
||||
@ -64,7 +65,7 @@ public:
|
||||
void Extend_mcontrol(int angle1,int angle2);
|
||||
void ball_control(void);
|
||||
|
||||
BallState_t currentState1; // 当前状态
|
||||
BallState_t currentState1; // 运球任务状态机
|
||||
int flag_thread;//暂时还没用到
|
||||
int ball_state ;//光电检测
|
||||
|
||||
|
||||
@ -22,12 +22,16 @@ NUC_t nuc_v;
|
||||
|
||||
//尽量别动这组pid
|
||||
const fp32 Shoot:: M2006_speed_PID[3] = {5, 0, 0.01};
|
||||
const fp32 Shoot:: M2006_angle_PID[3] = { 25, 0.1, 0};
|
||||
const fp32 Shoot:: M2006_angle_PID[3] = { 15, 0.1, 0};
|
||||
|
||||
#define TO_TOP 10.0f
|
||||
#define TO_BOTTOM 5.0f
|
||||
|
||||
#define INIT_POS -130
|
||||
#define TOP_POS -210
|
||||
#define BOTTOM_POS 0
|
||||
#define GO_ERROR 1.0f
|
||||
#define Tigger_DO -10
|
||||
#define Tigger_DO 0
|
||||
#define Tigger_ZERO 100
|
||||
#define Tigger_ERROR 3
|
||||
|
||||
@ -61,6 +65,7 @@ Shoot::Shoot()
|
||||
|
||||
currentState= SHOOT_IDLE;
|
||||
|
||||
LowPassFilter2p_Init(&distance_filter,500.0f,80.0f); //给distance 做滤波
|
||||
|
||||
}
|
||||
|
||||
@ -78,7 +83,8 @@ void Shoot::trigger_control()
|
||||
void Shoot :: distanceGet(const NUC_t &nuc_v)
|
||||
{
|
||||
|
||||
distance=nuc_v.vision.x; // 获取自瞄数据
|
||||
distance= LowPassFilter2p_Apply(&distance_filter, nuc_v.vision.x); // 对视觉距离进行滤波处理
|
||||
//distance=nuc_v.vision.x; // 获取自瞄数据
|
||||
|
||||
}
|
||||
|
||||
@ -151,22 +157,41 @@ void Shoot::rc_mode()
|
||||
// trigger_key=SHOOT;
|
||||
// }
|
||||
|
||||
//knob_increment=rc_ctrl.ch[2]/150;
|
||||
// //旋钮增量
|
||||
// static int last_knob_value = 0; // 记录旋钮的上一次值
|
||||
// int current_knob_value = rc_ctrl.sw[7]; // 获取当前旋钮值
|
||||
|
||||
//旋钮增量
|
||||
static int last_knob_value = 0; // 记录旋钮的上一次值
|
||||
int current_knob_value = rc_ctrl.sw[7]; // 获取当前旋钮值
|
||||
// // 计算旋钮增量
|
||||
// if (current_knob_value >= 200 && current_knob_value <= 1800) {
|
||||
// knob_increment = (current_knob_value - last_knob_value) / 15.0f; // 每80单位对应一个增量
|
||||
// } else {
|
||||
// knob_increment = 0; // 如果旋钮值超出范围，不产生增量
|
||||
// }
|
||||
|
||||
// 计算旋钮增量
|
||||
if (current_knob_value >= 200 && current_knob_value <= 1800) {
|
||||
knob_increment = (current_knob_value - last_knob_value) / 15.0f; // 每80单位对应一个增量
|
||||
} else {
|
||||
knob_increment = 0; // 如果旋钮值超出范围，不产生增量
|
||||
}
|
||||
// 旋钮物理范围
|
||||
const int knob_min = 200;
|
||||
const int knob_max = 1800;
|
||||
|
||||
// 目标映射范围
|
||||
const float map_min = 130.0f;
|
||||
const float map_max = -60.0f;
|
||||
|
||||
int current_knob_value = rc_ctrl.sw[7];
|
||||
|
||||
// 限制范围
|
||||
if (current_knob_value < knob_min) current_knob_value = knob_min;
|
||||
if (current_knob_value > knob_max) current_knob_value = knob_max;
|
||||
|
||||
// 线性映射
|
||||
knob_increment = map_min + (map_max - map_min) * (current_knob_value - knob_min) / (knob_max - knob_min);
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
#if TESTUP
|
||||
|
||||
void Shoot::shoot_control() {
|
||||
|
||||
//方便调试
|
||||
@ -181,13 +206,13 @@ void Shoot::shoot_control() {
|
||||
|
||||
control_pos = INIT_POS; // 默认中间挡位位置
|
||||
go1.Pos = fire_pos; // 设置蓄力电机位置
|
||||
t_posSet = Tigger_ZERO; // 扳机松开
|
||||
//t_posSet = Tigger_ZERO; // 扳机松开
|
||||
|
||||
if (currentState == SHOOT_READY) {
|
||||
// 如果当前状态是准备发射，松开钩子发射
|
||||
t_posSet = Tigger_ZERO; // 扳机扣动
|
||||
BSP_Buzzer_Stop();
|
||||
if (t_posSet >= 95) { // 假设120度为发射完成角度
|
||||
if (t_posSet >= Tigger_ZERO-20) { // 假设120度为发射完成角度
|
||||
currentState = SHOOT_IDLE; // 切换到空闲状态
|
||||
is_hooked = false; // 重置钩子状态
|
||||
}
|
||||
@ -216,16 +241,14 @@ break;
|
||||
case DOWN1:
|
||||
|
||||
control_pos = INIT_POS; // 默认中间挡位位置
|
||||
//fire_pos = control_pos + knob_increment; // 根据旋钮值调整发射位置
|
||||
//fire_pos = control_pos + knob_increment; // 根据旋钮值调整发射位置
|
||||
go1.Pos = fire_pos; // 设置蓄力电机位置
|
||||
t_posSet = Tigger_ZERO; // 扳机松开
|
||||
//t_posSet = Tigger_ZERO; // 扳机松开
|
||||
|
||||
if (currentState == SHOOT_READY) {
|
||||
// 如果当前状态是准备发射，松开钩子发射
|
||||
t_posSet = Tigger_ZERO; // 扳机扣动
|
||||
BSP_Buzzer_Stop();
|
||||
if (t_posSet >= 95) { // 假设120度为发射完成角度
|
||||
if (t_posSet >= Tigger_ZERO-20) { // 假设120度为发射完成角度
|
||||
currentState = SHOOT_IDLE; // 切换到空闲状态
|
||||
is_hooked = false; // 重置钩子状态
|
||||
}
|
||||
@ -253,6 +276,8 @@ break;
|
||||
}
|
||||
|
||||
|
||||
|
||||
abs_limit_min_max_fp(&go1.Pos ,-210.0f,2.0f);
|
||||
// 发送数据到蓄力电机
|
||||
GO_SendData(go1.Pos, 5.0f);
|
||||
|
||||
@ -276,7 +301,7 @@ void Shoot :: shoot_Current()
|
||||
|
||||
case SHOOT_TOP:
|
||||
t_posSet = Tigger_DO; // 扳机扣动钩住
|
||||
if (trigger_Motor->total_angle<-6)
|
||||
if (trigger_Motor->total_angle<Tigger_DO+1.0f && trigger_Motor->total_angle>Tigger_DO-1.0f)
|
||||
{
|
||||
//判定为钩住
|
||||
is_hooked = true; // 标记钩子已钩住
|
||||
@ -322,3 +347,180 @@ void Shoot::RemoveError() {
|
||||
|
||||
go1.Pos = control_pos;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
#if ONE_CONTROL
|
||||
|
||||
|
||||
void Shoot::shoot_control() {
|
||||
|
||||
//方便调试 反馈信息
|
||||
feedback.fd_gopos = go1_data->Pos;
|
||||
feedback.fd_tpos = trigger_Motor->total_angle;
|
||||
|
||||
|
||||
shoot_thread = osThreadFlagsGet(); // 获取任务通知标志位
|
||||
|
||||
switch (mode_key)
|
||||
{
|
||||
case VSION:
|
||||
fire_pos = distance; // 视觉拟合的力
|
||||
switch (rc_key) {
|
||||
case MIDDLE1:
|
||||
|
||||
if(shoot_thread & PREPARE) // 如果收到放球完成的通知
|
||||
{
|
||||
ball_receive(); // 执行接收球的操作
|
||||
|
||||
}
|
||||
|
||||
if(shoot_thread & EXTEND_OK) // 如果收到伸缩完成的通知
|
||||
{
|
||||
go1.Pos = fire_pos; // 设置蓄力电机位置
|
||||
if(feedback.fd_gopos >= fire_pos - GO_ERROR && feedback.fd_gopos <= fire_pos + GO_ERROR)
|
||||
{
|
||||
BSP_Buzzer_Start();
|
||||
BSP_Buzzer_Set(1,5000);
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case DOWN1:
|
||||
if(shoot_thread & EXTEND_OK)
|
||||
{
|
||||
BSP_Buzzer_Stop();
|
||||
t_posSet = Tigger_ZERO; // 扳机扣动 射出
|
||||
currentState = SHOOT_IDLE; // 默认回到空闲状态
|
||||
osThreadFlagsClear(EXTEND_OK);
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case UP1:
|
||||
RemoveError();
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
//无自瞄拟合测试档
|
||||
case TEST:
|
||||
switch (rc_key) {
|
||||
case MIDDLE1:
|
||||
|
||||
if(shoot_thread & PREPARE) // 如果收到放球完成的通知
|
||||
{
|
||||
ball_receive(); // 执行接收球的操作
|
||||
|
||||
}
|
||||
|
||||
if(shoot_thread & EXTEND_OK) // 如果收到伸缩完成的通知
|
||||
{
|
||||
fire_pos = INIT_POS + knob_increment; // 根据旋钮值调整发射位置
|
||||
go1.Pos = fire_pos; // 设置蓄力电机位置
|
||||
if(feedback.fd_gopos >= fire_pos - GO_ERROR && feedback.fd_gopos <= fire_pos + GO_ERROR)
|
||||
{
|
||||
BSP_Buzzer_Start();
|
||||
BSP_Buzzer_Set(1,5000);
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case DOWN1:
|
||||
if(shoot_thread & EXTEND_OK)
|
||||
{
|
||||
BSP_Buzzer_Stop();
|
||||
t_posSet = Tigger_ZERO; // 扳机扣动 射出
|
||||
currentState = SHOOT_IDLE; // 默认回到空闲状态
|
||||
osThreadFlagsClear(EXTEND_OK);
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case UP1:
|
||||
RemoveError();
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
abs_limit_min_max_fp(&go1.Pos ,-210.0f,2.0f);
|
||||
// 发送数据到蓄力电机
|
||||
GO_SendData(go1.Pos, limit_speed);
|
||||
|
||||
// 控制扳机电机
|
||||
trigger_control();
|
||||
}
|
||||
|
||||
//配合运球到发射
|
||||
void Shoot ::ball_receive()
|
||||
{
|
||||
switch (currentState)
|
||||
{
|
||||
case SHOOT_IDLE:
|
||||
// 初始状态：钩子移动到顶部，钩住拉簧
|
||||
control_pos = TOP_POS;
|
||||
limit_speed=TO_TOP;//快速上去
|
||||
t_posSet = Tigger_ZERO;
|
||||
if(feedback.fd_gopos >= TOP_POS - 0.5f && feedback.fd_gopos <= TOP_POS + 0.5f)
|
||||
{
|
||||
currentState = GO_TOP; // 切换到准备发射状态
|
||||
}
|
||||
|
||||
break;
|
||||
case GO_TOP:
|
||||
t_posSet = Tigger_DO;
|
||||
if (trigger_Motor->total_angle<Tigger_DO+1.0f && trigger_Motor->total_angle>Tigger_DO-1.0f)
|
||||
{
|
||||
|
||||
currentState = BAKC; // 切换到准备发射状态
|
||||
}
|
||||
break;
|
||||
case BAKC:
|
||||
control_pos = BOTTOM_POS;
|
||||
limit_speed=TO_BOTTOM;//慢速下来
|
||||
if(feedback.fd_gopos >= BOTTOM_POS - 1.0f && feedback.fd_gopos <= BOTTOM_POS + 1.0f)
|
||||
{
|
||||
|
||||
osThreadFlagsClear(PREPARE); // 清除任务通知标志位
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
//调整go电机位置
|
||||
go1.Pos = control_pos;
|
||||
}
|
||||
|
||||
void Shoot::RemoveError() {
|
||||
currentState = SHOOT_IDLE;
|
||||
control_pos = TOP_POS;
|
||||
if (feedback.fd_gopos >= control_pos - 0.5f&& feedback.fd_gopos<= control_pos + 0.5f)
|
||||
{
|
||||
t_posSet = Tigger_ZERO;
|
||||
is_hooked = false;
|
||||
}
|
||||
if(trigger_Motor->total_angle>=Tigger_ZERO-10)
|
||||
{
|
||||
//认为完全松开
|
||||
control_pos=INIT_POS;
|
||||
BSP_Buzzer_Stop();
|
||||
|
||||
}
|
||||
|
||||
go1.Pos = control_pos;
|
||||
}
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
@ -5,6 +5,7 @@
|
||||
#include "GO_M8010_6_Driver.h"
|
||||
#include "nuc.h"
|
||||
#include "usart.h"
|
||||
#include "filter.h"
|
||||
|
||||
// 定义状态枚举
|
||||
typedef enum {
|
||||
@ -14,7 +15,10 @@ typedef enum {
|
||||
SHOOT_READY,
|
||||
SHOOT_WAIT,
|
||||
SHOOT_FIRE, // 发射状态
|
||||
SHOOT_RETURN // 自动返回状态
|
||||
SHOOT_RETURN, // 自动返回状态
|
||||
//运射配合
|
||||
GO_TOP,
|
||||
BAKC
|
||||
} ShootState_t;
|
||||
|
||||
// // 定义状态枚举
|
||||
@ -52,8 +56,8 @@ public:
|
||||
void shoot_control(void);
|
||||
void shoot_Current(void);
|
||||
void RemoveError(void);
|
||||
void ball_receive(void);
|
||||
|
||||
|
||||
int GO_SendData(float pos,float limit);
|
||||
|
||||
//go电机
|
||||
@ -67,8 +71,7 @@ public:
|
||||
float fd_tpos;
|
||||
}feedback;
|
||||
|
||||
|
||||
|
||||
|
||||
//扳机
|
||||
float speed_trigger;
|
||||
int16_t result;
|
||||
@ -77,13 +80,16 @@ public:
|
||||
float fd_tpos; //扳机反馈位置
|
||||
motor_measure_t *trigger_Motor;
|
||||
|
||||
//滤波器
|
||||
LowPassFilter2p_t distance_filter; // 用于滤波视觉距离
|
||||
|
||||
//==========================公共变量==========================
|
||||
int16_t rc_key; //遥控器按键
|
||||
int16_t mode_key;
|
||||
int16_t trigger_key;
|
||||
ShootState_t currentState; //状态机
|
||||
//volatile BallState_t ballStatus;//是否有球
|
||||
volatile uint32_t flag_thread;//接收传回的线程通知
|
||||
|
||||
volatile uint32_t shoot_thread;//接收传回的线程通知
|
||||
fp32 distance; //视觉距离
|
||||
|
||||
private:
|
||||
@ -98,6 +104,7 @@ private:
|
||||
|
||||
bool is_hooked;// 标记钩子是否已经钩住拉簧
|
||||
|
||||
float limit_speed;//go电机限速
|
||||
|
||||
|
||||
};
|
||||
|
||||
@ -12,6 +12,7 @@ Ball ball;
|
||||
|
||||
//检查光电
|
||||
int abc=0;
|
||||
int aaaa=146;
|
||||
|
||||
extern int speedm;
|
||||
|
||||
@ -20,6 +21,10 @@ void FunctionBall(void *argument)
|
||||
(void)argument; /* 未使用argument，消除警告 */
|
||||
|
||||
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_BALL;
|
||||
|
||||
osDelay(6000);//等待极致控制板启动
|
||||
XiaomiWait_init(1,&hcan2); //小米电机初始化
|
||||
|
||||
uint32_t tick = osKernelGetTickCount();
|
||||
|
||||
while(1)
|
||||
@ -34,6 +39,8 @@ void FunctionBall(void *argument)
|
||||
|
||||
ball.ball_control(); // 控制球的动作
|
||||
|
||||
// ball.xiaomi.position = aaaa;
|
||||
// CAN_XiaoMi(1,&ball.xiaomi,&hcan2);
|
||||
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
osDelayUntil(tick);
|
||||
|
||||
@ -23,6 +23,8 @@ void Function_nuc(void *argument)
|
||||
|
||||
NUC_Init(&cmd_fromnuc);
|
||||
|
||||
HAL_GPIO_WritePin(LED_G_GPIO_Port,LED_G_Pin,GPIO_PIN_SET);
|
||||
|
||||
uint32_t tick = osKernelGetTickCount();
|
||||
|
||||
while(1)
|
||||
|
||||
@ -10,7 +10,7 @@ extern RC_ctrl_t rc_ctrl;
|
||||
Shoot shoot;
|
||||
NUC_t nucData; // 自瞄
|
||||
|
||||
int goangle=0;
|
||||
int aaaxxx=0;
|
||||
|
||||
void FunctionShoot(void *argument)
|
||||
{
|
||||
@ -27,20 +27,16 @@ while(1)
|
||||
#endif
|
||||
|
||||
shoot.rc_mode(); //遥控器模式
|
||||
//我放的任务通知 运球成功放置过来后
|
||||
shoot.flag_thread=osThreadFlagsGet();
|
||||
|
||||
if(shoot.flag_thread & BALL_DOWN)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
|
||||
if (osMessageQueueGet(task_struct.msgq.nuc, &nucData, NULL, 0) == osOK)
|
||||
{
|
||||
shoot.distanceGet(nucData);
|
||||
}
|
||||
|
||||
shoot.shoot_control();
|
||||
|
||||
// shoot.t_posSet=aaaxxx;
|
||||
// shoot.trigger_control();
|
||||
// shoot.GO_SendData(goangle,5);
|
||||
// shoot.shoot_control();
|
||||
// shoot.t_posSet=goangle;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user