不知道就这样吧
This commit is contained in:
parent
9688eca871
commit
a235d14661
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\\r1upper-1\\Core\\Inc",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Inc",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Inc\\Legacy",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\include",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\CMSIS_RTOS_V2",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\RVDS\\ARM_CM4F",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Drivers\\CMSIS\\Device\\ST\\STM32F4xx\\Include",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Drivers\\CMSIS\\Include",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\bsp",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\module",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\task",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\lib",
|
||||
"d:\\Desktop\\r1\\r1_upper\\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\\r1upper-1\\MDK-ARM",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Core\\Src",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Src",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source",
|
||||
"d:\\Desktop\\r1\\r1_upper\\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",
|
||||
|
||||
16
MDK-ARM/.vscode/keil-assistant.log
vendored
16
MDK-ARM/.vscode/keil-assistant.log
vendored
@ -106,3 +106,19 @@
|
||||
|
||||
[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
|
||||
|
||||
|
||||
12
MDK-ARM/.vscode/uv4.log
vendored
12
MDK-ARM/.vscode/uv4.log
vendored
@ -1,16 +1,8 @@
|
||||
*** Using Compiler 'V5.06 update 7 (build 960)', folder: 'D:\keil\ARM\ARMCC\Bin'
|
||||
Build target 'R1-shooter'
|
||||
compiling motor.cpp...
|
||||
compiling ball.cpp...
|
||||
compiling djiMotor.c...
|
||||
compiling gimbal.cpp...
|
||||
compiling encodeCan.cpp...
|
||||
compiling shoot.cpp...
|
||||
compiling ballTask.cpp...
|
||||
compiling shootTask.cpp...
|
||||
compiling gimbalTask.cpp...
|
||||
linking...
|
||||
Program Size: Code=28808 RO-data=1800 RW-data=264 ZI-data=23712
|
||||
Program Size: Code=30584 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:05
|
||||
Build Time Elapsed: 00:00:03
|
||||
|
||||
2
MDK-ARM/.vscode/uv4.log.lock
vendored
2
MDK-ARM/.vscode/uv4.log.lock
vendored
@ -1 +1 @@
|
||||
2025/5/23 21:17:13
|
||||
2025/5/28 19:17:30
|
||||
@ -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>
|
||||
@ -303,6 +270,36 @@
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>motor_5065</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>23</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>test_distance</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>24</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>xxx</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>25</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>test_speed,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>26</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>speedm,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>27</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>Torque,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>28</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>triggerspeed,0x0A</ItemText>
|
||||
</Ww>
|
||||
</WatchWindow1>
|
||||
<WatchWindow2>
|
||||
<Ww>
|
||||
@ -1113,7 +1110,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
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@ -416,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;
|
||||
|
||||
@ -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
|
||||
|
||||
@ -17,6 +17,8 @@ extern int key;
|
||||
//添加平移3508 得跑位置吧
|
||||
|
||||
const fp32 Ball:: M3508_speed_PID[3] = {15, 0.03, 0};
|
||||
const fp32 Ball:: Extend_speed_PID[3] = { 25, 0, 0.};
|
||||
const fp32 Ball:: Extend_angle_PID[3]= { 60, 0, 0.1};
|
||||
|
||||
//摩擦轮转速
|
||||
int speedm=0;
|
||||
@ -40,6 +42,23 @@ Ball ::Ball()
|
||||
speedSet[i] = 0;
|
||||
}
|
||||
|
||||
//两个伸缩6020
|
||||
Extern_Motor[0] = get_motor_point(6);
|
||||
Extern_Motor[1] = get_motor_point(7);
|
||||
|
||||
Extern_Motor[0]->type = GM6020;
|
||||
Extern_Motor[1]->type = GM6020;
|
||||
PID_init(&extend_speed_pid[0],PID_POSITION,Extend_speed_PID,25000, 2000);
|
||||
PID_init(&extend_angle_pid[0],PID_POSITION,Extend_angle_PID,25000, 1000);
|
||||
|
||||
PID_init(&extend_speed_pid[1],PID_POSITION,Extend_speed_PID,25000, 2000);
|
||||
PID_init(&extend_angle_pid[1],PID_POSITION,Extend_angle_PID,25000, 1000);
|
||||
|
||||
e_result[0] = 0;
|
||||
e_result[1] = 0;
|
||||
angleSet[0] = 0;
|
||||
angleSet[1] = 0;
|
||||
|
||||
//小米电机初始化
|
||||
JZ_xiaomi.position = 0; //
|
||||
JZ_xiaomi.speed = 20; //
|
||||
@ -55,6 +74,35 @@ Ball ::Ball()
|
||||
}
|
||||
|
||||
|
||||
void Ball ::Extend_control(int angle)
|
||||
{
|
||||
int16_t delta[2];
|
||||
angleSet[0] = angle;
|
||||
angleSet[1] = -angle;
|
||||
|
||||
delta[0] = PID_calc(&extend_angle_pid[0],Extern_Motor[0]->total_angle , angleSet[0]);
|
||||
e_result[0] = PID_calc(&extend_speed_pid[0], Extern_Motor[0]->speed_rpm, delta[0]);
|
||||
|
||||
delta[1] = PID_calc(&extend_angle_pid[1], Extern_Motor[1]->total_angle , angleSet[1]);
|
||||
e_result[1] = PID_calc(&extend_speed_pid[1], Extern_Motor[1]->speed_rpm, delta[1]);
|
||||
|
||||
// delta[1] = PID_calc(&extend_angle_pid, angle_get[1], angleSet[1]);
|
||||
// e_result[1] = PID_calc(&extend_speed_pid, Extern_Motor[1]->speed_rpm, delta[1]);
|
||||
|
||||
// angle_get[0]=motor_angle_change(Extern_Motor[0]->real_round, angleSet[0]);
|
||||
// angle_get[1]=motor_angle_change(Extern_Motor[1]->real_round, angleSet[1]);
|
||||
|
||||
|
||||
// delta[0] = PID_calc(&extend_angle_pid, angle_get[0], angleSet[0]);
|
||||
// e_result[0] = PID_calc(&extend_speed_pid, Extern_Motor[0]->speed_rpm, delta[0]);
|
||||
|
||||
// delta[1] = PID_calc(&extend_angle_pid, angle_get[1], angleSet[1]);
|
||||
// e_result[1] = PID_calc(&extend_speed_pid, Extern_Motor[1]->speed_rpm, delta[1]);
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
void Ball ::Spin(float speed)
|
||||
{
|
||||
|
||||
@ -78,6 +126,7 @@ void Ball::ballDown(void)
|
||||
|
||||
void Ball::ballStop(void)
|
||||
{
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保闭合气缸
|
||||
speedm=0;
|
||||
|
||||
}
|
||||
@ -98,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()
|
||||
@ -176,6 +218,9 @@ void Ball::Send_control()
|
||||
CAN_cmd_200(result[MOTOR_1],result[MOTOR_2],result[MOTOR_3],0,&hcan1);
|
||||
osDelay(1);
|
||||
|
||||
CAN_cmd_1FF(0,0,e_result[0],e_result[1],&hcan1);
|
||||
osDelay(2);
|
||||
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
@ -54,6 +54,7 @@ public:
|
||||
void Send_control(void);
|
||||
void ballStop(void);
|
||||
void ballTake(void);
|
||||
void Extend_control(int angle);
|
||||
|
||||
BallState_t currentState1; // 当前状态
|
||||
int flag;//暂时还没用到
|
||||
@ -62,6 +63,10 @@ public:
|
||||
//小米电机伸缩
|
||||
JZ_xiaomi_t JZ_xiaomi;
|
||||
|
||||
//伸缩6020
|
||||
int16_t e_result[2];
|
||||
motor_measure_t * Extern_Motor[2];
|
||||
|
||||
int16_t result[MOTOR_NUM];
|
||||
motor_measure_t *hand_Motor[MOTOR_NUM];
|
||||
//==========================公共变量==========================//
|
||||
@ -75,12 +80,17 @@ 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];
|
||||
|
||||
//电机速度pid结构体
|
||||
pid_type_def speed_pid[MOTOR_NUM];
|
||||
//位置环pid
|
||||
pid_type_def angle_pid[MOTOR_NUM];
|
||||
|
||||
pid_type_def extend_speed_pid[2];
|
||||
pid_type_def extend_angle_pid[2];
|
||||
|
||||
float angleSet[2];
|
||||
float speedSet[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 =34700;
|
||||
float test_distance =3.6;
|
||||
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.z;
|
||||
|
||||
}
|
||||
|
||||
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,31 @@ 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 1185.3918f * powf(x, 5.0f)
|
||||
+ -45876.846f * powf(x, 4.0f)
|
||||
+ 517061.22f * powf(x, 3.0f)
|
||||
+ -2567947.4f * powf(x, 2.0f)
|
||||
+ 5954537.4f * x
|
||||
+ -5258769.0f;
|
||||
}
|
||||
|
||||
|
||||
float polynomial2(float x) {
|
||||
return 49162.7402f * powf(x, 5.0f)
|
||||
+ -917344.6015f * powf(x, 4.0f)
|
||||
+ 6840516.5228f * powf(x, 3.0f)
|
||||
+ -25479457.6038f * powf(x, 2.0f)
|
||||
+ 47406744.9129f * x
|
||||
+ -35217628.5565f;
|
||||
}
|
||||
|
||||
// float polynomial(float x) {
|
||||
// return 1185.3918*pow(x,5) + -45876.846*pow(x,4) + 517061.22*pow(x,3) + -2567947.4*pow(x,2) + 5954537.4*x + -5258769;
|
||||
// }
|
||||
|
||||
@ -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,42 +44,46 @@ 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);
|
||||
}
|
||||
|
||||
// CAN_XiaoMi(1,&ball.JZ_xiaomi,&hcan2);
|
||||
|
||||
ball.Spin(speedm);
|
||||
ball.Send_control();
|
||||
|
||||
@ -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