Compare commits
2 Commits
6489615e36
...
74d7f2ef20
| Author | SHA1 | Date | |
|---|---|---|---|
| 74d7f2ef20 | |||
| 30de35d147 |
4
MDK-ARM/.vscode/keil-assistant.log
vendored
4
MDK-ARM/.vscode/keil-assistant.log
vendored
@ -32,3 +32,7 @@
|
||||
|
||||
[info] Log at : 2025/6/15|11:17:22|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/16|19:01:05|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/17|19:06:14|GMT+0800
|
||||
|
||||
|
||||
8
MDK-ARM/.vscode/uv4.log
vendored
8
MDK-ARM/.vscode/uv4.log
vendored
@ -1,8 +1,10 @@
|
||||
*** Using Compiler 'V5.06 update 7 (build 960)', folder: 'D:\keil\ARM\ARMCC\Bin'
|
||||
Build target 'R1'
|
||||
compiling calc_lib.c...
|
||||
compiling shoot.cpp...
|
||||
compiling ballTask.cpp...
|
||||
compiling ball.cpp...
|
||||
linking...
|
||||
Program Size: Code=30644 RO-data=1884 RW-data=5816 ZI-data=33280
|
||||
Program Size: Code=30080 RO-data=1828 RW-data=268 ZI-data=32660
|
||||
FromELF: creating hex file...
|
||||
"R1\R1.axf" - 0 Error(s), 0 Warning(s).
|
||||
Build Time Elapsed: 00:00:03
|
||||
Build Time Elapsed: 00:00:07
|
||||
|
||||
2
MDK-ARM/.vscode/uv4.log.lock
vendored
2
MDK-ARM/.vscode/uv4.log.lock
vendored
@ -1 +1 @@
|
||||
2025/6/14 22:01:44
|
||||
2025/6/16 19:22:49
|
||||
@ -153,24 +153,7 @@
|
||||
<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>
|
||||
</SetRegEntry>
|
||||
</TargetDriverDllRegistry>
|
||||
<Breakpoint>
|
||||
<Bp>
|
||||
<Number>0</Number>
|
||||
<Type>0</Type>
|
||||
<LineNumber>82</LineNumber>
|
||||
<EnabledFlag>1</EnabledFlag>
|
||||
<Address>0</Address>
|
||||
<ByteObject>0</ByteObject>
|
||||
<HtxType>0</HtxType>
|
||||
<ManyObjects>0</ManyObjects>
|
||||
<SizeOfObject>0</SizeOfObject>
|
||||
<BreakByAccess>0</BreakByAccess>
|
||||
<BreakIfRCount>0</BreakIfRCount>
|
||||
<Filename>../Core/Src/main.c</Filename>
|
||||
<ExecCommand></ExecCommand>
|
||||
<Expression></Expression>
|
||||
</Bp>
|
||||
</Breakpoint>
|
||||
<Breakpoint/>
|
||||
<WatchWindow1>
|
||||
<Ww>
|
||||
<count>0</count>
|
||||
@ -205,12 +188,17 @@
|
||||
<Ww>
|
||||
<count>6</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>knob_increment</ItemText>
|
||||
<ItemText>task_struct,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>7</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>task_struct,0x0A</ItemText>
|
||||
<ItemText>aaa,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>8</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>wzcsb,0x0A</ItemText>
|
||||
</Ww>
|
||||
</WatchWindow1>
|
||||
<Tracepoint>
|
||||
@ -971,7 +959,7 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>User/device</GroupName>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
@ -1063,7 +1051,7 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>User/module</GroupName>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
@ -1107,7 +1095,7 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>User/task</GroupName>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
|
||||
@ -18,10 +18,10 @@
|
||||
#ifndef DT7
|
||||
#define DT7 0
|
||||
#endif
|
||||
//是否使用三摩擦
|
||||
#ifndef HANDLING3
|
||||
#define HANDLING3 1
|
||||
#endif
|
||||
// //是否使用三摩擦
|
||||
// #ifndef HANDLING3
|
||||
// #define HANDLING3 1
|
||||
// #endif
|
||||
//云台使用类型
|
||||
#ifndef GM6020ING
|
||||
#define GM6020ING 1
|
||||
|
||||
@ -68,7 +68,7 @@ int8_t NUC_SendPacket(void *data, uint16_t length) {
|
||||
|
||||
return DEVICE_OK; // 发送成功
|
||||
}
|
||||
|
||||
int wzcsb=0;
|
||||
int8_t NUC_RawParse(NUC_t *n) {
|
||||
if (n == NULL) return DEVICE_ERR_NULL;
|
||||
union {
|
||||
@ -101,10 +101,10 @@ int8_t NUC_RawParse(NUC_t *n) {
|
||||
instance.data[0] = nucbuf[3];
|
||||
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];
|
||||
instance.data[7] = nucbuf[10];
|
||||
instance.data[6] = nucbuf[9];
|
||||
instance.data[5] = nucbuf[8];
|
||||
instance.data[4] = nucbuf[7];
|
||||
n->vision.y = instance.x[1];
|
||||
|
||||
instance.data[11] = nucbuf[11];
|
||||
@ -119,6 +119,7 @@ int8_t NUC_RawParse(NUC_t *n) {
|
||||
|
||||
|
||||
error:
|
||||
wzcsb++;
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
|
||||
|
||||
@ -8,10 +8,6 @@
|
||||
|
||||
extern RC_ctrl_t rc_ctrl;
|
||||
extern int key;
|
||||
extern int16_t M2006_result;
|
||||
|
||||
// C键 sw[5]👆 1800 中1000 👇200
|
||||
// D键 sw[6]👆 200 👇1800
|
||||
|
||||
//伸缩
|
||||
#define M_SPEED 4000
|
||||
@ -20,20 +16,10 @@ extern int16_t M2006_result;
|
||||
#define O_ANGLE1 340
|
||||
#define O_ANGLE2 -240
|
||||
|
||||
// 三摩擦轮电机
|
||||
#define M_SPEED1 3000
|
||||
#define M_BACK -4000
|
||||
|
||||
#if HANDLING3 == 1
|
||||
//三摩擦轮运球！！！
|
||||
//添加平移3508 得跑位置吧
|
||||
|
||||
const fp32 Ball:: M3508_speed_PID[3] = {30, 0.03, 0};
|
||||
const fp32 Ball:: Extend_speed_PID[3] = { 40, 0.0, 0.1};
|
||||
const fp32 Ball:: Extend_angle_PID[3]= { 25, 0.1, 0};
|
||||
|
||||
//摩擦轮转速
|
||||
int speedm=0;
|
||||
|
||||
//PE11 气缸
|
||||
|
||||
@ -41,19 +27,6 @@ Ball ::Ball()
|
||||
{
|
||||
detect_init();
|
||||
|
||||
//三摩擦轮
|
||||
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 左207 右208
|
||||
Extern_Motor[0] = get_motor_point(6);
|
||||
Extern_Motor[1] = get_motor_point(7);
|
||||
@ -97,34 +70,44 @@ void Ball ::Extend_mcontrol(int angle1,int angle2)
|
||||
}
|
||||
|
||||
|
||||
void Ball ::Spin()
|
||||
|
||||
// E键 sw[1] 👆 200 shoot 中 1000 stop sw[2]👇1800
|
||||
// G键 sw[6]👆 1800 中 1000 👇200
|
||||
// sw[5] 👆 200 👇1800
|
||||
//左旋 sw[7] 200 --1800
|
||||
void Ball::rc_mode()
|
||||
{
|
||||
if(rc_ctrl.sw[6]==1800)
|
||||
{
|
||||
rc_key=UP2;
|
||||
}
|
||||
if(rc_ctrl.sw[6]==1000)
|
||||
{
|
||||
rc_key=MIDDLE2;
|
||||
}
|
||||
if(rc_ctrl.sw[6]==200)
|
||||
{
|
||||
rc_key=DOWN2;
|
||||
}
|
||||
|
||||
speedSet[MOTOR_1] = speed_set;
|
||||
result[MOTOR_1] = PID_calc(&speed_pid[MOTOR_1],hand_Motor[MOTOR_1]->speed_rpm,speedSet[MOTOR_1]);
|
||||
|
||||
speedSet[MOTOR_2] = speed_set;
|
||||
result[MOTOR_2] = PID_calc(&speed_pid[MOTOR_2],hand_Motor[MOTOR_2]->speed_rpm,speedSet[MOTOR_2]);
|
||||
|
||||
speedSet[MOTOR_3] = speed_set;
|
||||
result[MOTOR_3] = PID_calc(&speed_pid[MOTOR_3],hand_Motor[MOTOR_3]->speed_rpm,speedSet[MOTOR_3]);
|
||||
|
||||
}
|
||||
|
||||
void Ball::ballDown(void)
|
||||
{
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET);//确保闭合气缸
|
||||
speed_set=500;
|
||||
if(rc_ctrl.sw[5]==200)
|
||||
{
|
||||
extern_key=IN;
|
||||
}
|
||||
if(rc_ctrl.sw[5]==1800)
|
||||
{
|
||||
extern_key=OUT;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void Ball::Move_Extend(void)
|
||||
{
|
||||
if(rc_ctrl.sw[6] == 200) // 左拨杆上
|
||||
if(extern_key==IN)
|
||||
{
|
||||
Extend_mcontrol(I_ANGLE1, I_ANGLE2); // 内伸
|
||||
}
|
||||
else if(rc_ctrl.sw[6] == 1800) // 左拨杆下
|
||||
else if(extern_key==OUT)
|
||||
{
|
||||
Extend_mcontrol(O_ANGLE1, O_ANGLE2); // 外伸
|
||||
}
|
||||
@ -134,45 +117,24 @@ void Ball::Move_Extend(void)
|
||||
}
|
||||
}
|
||||
|
||||
// C键 sw[5]👆 200 中1000 👇1800
|
||||
// D键 sw[6]👆 200 👇1800
|
||||
void Ball::rc_mode()
|
||||
void Ball::ballDown(void)
|
||||
{
|
||||
if(rc_ctrl.sw[5]==200)
|
||||
{
|
||||
rc_key=UP2;
|
||||
}
|
||||
if(rc_ctrl.sw[5]==1000)
|
||||
{
|
||||
rc_key=MIDDLE2;
|
||||
}
|
||||
if(rc_ctrl.sw[5]==1800)
|
||||
{
|
||||
rc_key=DOWN2;
|
||||
}
|
||||
|
||||
if(rc_ctrl.sw[6]==200)
|
||||
{
|
||||
extern_key=IN;
|
||||
}
|
||||
if(rc_ctrl.sw[6]==1800)
|
||||
{
|
||||
extern_key=OUT;
|
||||
}
|
||||
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_SET); // 打开气缸爪子
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||||
|
||||
}
|
||||
|
||||
|
||||
void Ball::ball_control()
|
||||
{
|
||||
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); // 读取光电状态(有球 0，无球 1)
|
||||
|
||||
Move_Extend();
|
||||
|
||||
switch (rc_key){
|
||||
case MIDDLE2:
|
||||
speed_set=0;
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);//确保闭合气缸
|
||||
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); //确保爪气缸关闭
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||||
if (currentState1 == BALL_FINISH)
|
||||
{
|
||||
currentState1 = BALL_IDLE;
|
||||
@ -183,7 +145,7 @@ void Ball::ball_control()
|
||||
break;
|
||||
|
||||
case UP2:
|
||||
Three_Handing();
|
||||
ballHadling();
|
||||
break;
|
||||
|
||||
case DOWN2:
|
||||
@ -193,99 +155,37 @@ void Ball::ball_control()
|
||||
|
||||
}
|
||||
|
||||
Spin();
|
||||
|
||||
Send_control();
|
||||
|
||||
}
|
||||
|
||||
void Ball::Three_Handing()
|
||||
{
|
||||
switch (currentState1){
|
||||
case BALL_IDLE:
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);//确保闭合气缸
|
||||
currentState1 = BALL_FORWARD; // 切换到正转状态
|
||||
|
||||
break;
|
||||
case BALL_FORWARD:
|
||||
speed_set=M_SPEED1;
|
||||
if(is_reached_multiple(hand_Motor[MOTOR_1]->speed_rpm,
|
||||
hand_Motor[MOTOR_2]->speed_rpm,
|
||||
hand_Motor[MOTOR_3]->speed_rpm,
|
||||
speed_set,
|
||||
speed_set,
|
||||
speed_set,
|
||||
50.f,50))
|
||||
{
|
||||
currentState1 = BALL_DROP; // 切换到球下落状态
|
||||
}
|
||||
break;
|
||||
|
||||
case BALL_DROP:
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 打开气缸
|
||||
if (ball_state == 0) // 读光电 有球 1 无球 0
|
||||
{
|
||||
osDelay(200); // 延时200ms
|
||||
speed_set = M_BACK;
|
||||
currentState1 = BALL_REVERSE; // 切换到反转状态
|
||||
}
|
||||
break;
|
||||
case BALL_REVERSE:
|
||||
if (ball_state == 1)
|
||||
{
|
||||
speed_set = 0;
|
||||
currentState1 = BALL_CLOSE; // 切换到完成状态
|
||||
}
|
||||
break;
|
||||
case BALL_CLOSE:
|
||||
if(ball_state == 1)
|
||||
{
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);
|
||||
currentState1 = BALL_FINISH; // 切换到完成状态
|
||||
|
||||
}
|
||||
case BALL_FINISH:
|
||||
key = 0; // 重置按键状态
|
||||
speed_set = 0;
|
||||
osThreadFlagsSet(task_struct.thread.shoot, BALL_OK);
|
||||
//currentState1 = BALL_IDLE; // 直接回到空闲状态
|
||||
break;
|
||||
default:
|
||||
currentState1 = BALL_IDLE; // 默认回到空闲状态
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
void Ball::Send_control()
|
||||
{
|
||||
CAN_cmd_200(result[MOTOR_1],result[MOTOR_2],result[MOTOR_3],0,&hcan1);
|
||||
osDelay(1);
|
||||
|
||||
CAN_cmd_1FF(M2006_result,0,e_result[0],e_result[1],&hcan1);
|
||||
|
||||
CAN_cmd_1FF(0,0,e_result[0],e_result[1],&hcan1);
|
||||
osDelay(1);
|
||||
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
int ball_state = 0;
|
||||
int triggerCount = 0; // 光电传感器触发计数
|
||||
int last_ball_state = 1; // 上一次的光电状态
|
||||
//(有球 0,无球 1)
|
||||
void Ball::ballHadling(void)
|
||||
{
|
||||
|
||||
ball_state = HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin); // 读取光电状态（有球 0，无球 1）
|
||||
|
||||
switch (currentState1)
|
||||
{
|
||||
case BALL_IDLE:
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保气缸闭合
|
||||
if (key > 0) // 检测按键是否被按下
|
||||
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 == UP2) // 检测按键是否被按下
|
||||
{
|
||||
key = 0; // 重置按键状态
|
||||
triggerCount = 0; // 重置触发计数
|
||||
currentState1 = BALL_FORWARD; // 切换到正转状态
|
||||
currentState1 = BALL_FORWARD;
|
||||
}
|
||||
break;
|
||||
|
||||
@ -326,17 +226,19 @@ void Ball::ballHadling(void)
|
||||
{
|
||||
osDelay(100); // 延时去抖
|
||||
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); // 闭合气缸爪子
|
||||
currentState1 = BALL_REVERSE; // 切换到反转状态
|
||||
currentState1 = BALL_FINISH; // 切换到反转状态
|
||||
}
|
||||
break;
|
||||
|
||||
case BALL_REVERSE:
|
||||
case BALL_FINISH:
|
||||
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; // 回到空闲状态
|
||||
|
||||
//currentState1 = BALL_IDLE; // 回到空闲状态
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
@ -344,5 +246,4 @@ void Ball::ballHadling(void)
|
||||
break;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
@ -14,9 +14,9 @@
|
||||
// 定义状态枚举
|
||||
typedef enum {
|
||||
BALL_IDLE, // 空闲状态
|
||||
BALL_FORWARD, // 正转状态
|
||||
BALL_DROP, // 球下落状态
|
||||
BALL_REVERSE, // 反转状态
|
||||
BALL_FORWARD, //
|
||||
BALL_DROP, //
|
||||
BALL_REVERSE, //
|
||||
BALL_FLAG,
|
||||
BALL_CLOSE, // 关闭状态
|
||||
BALL_FINISH, // 完成状态
|
||||
@ -52,28 +52,23 @@ class Ball
|
||||
{
|
||||
public:
|
||||
Ball();
|
||||
void Spin(void);
|
||||
void ballHadling(void);
|
||||
void ballDown(void);
|
||||
void Send_control(void);
|
||||
void ballStop(void);
|
||||
void Move_Extend(void);
|
||||
void rc_mode(void);
|
||||
void Extend_mcontrol(int angle1,int angle2);
|
||||
void Three_Handing(void);
|
||||
void ball_control(void);
|
||||
|
||||
BallState_t currentState1; // 当前状态
|
||||
int flag;//暂时还没用到
|
||||
int flag;//暂时还没用到
|
||||
int ball_state ;//光电检测
|
||||
|
||||
//伸缩6020
|
||||
int16_t e_result[2];
|
||||
motor_measure_t * Extern_Motor[2];
|
||||
|
||||
float speed_set;
|
||||
int16_t result[MOTOR_NUM];
|
||||
motor_measure_t *hand_Motor[MOTOR_NUM];
|
||||
|
||||
//==========================公共变量==========================//
|
||||
int16_t rc_key; //遥控器按键
|
||||
int16_t extern_key;
|
||||
@ -83,23 +78,14 @@ public:
|
||||
|
||||
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];
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@ -26,18 +26,17 @@ const fp32 Shoot:: M2006_angle_PID[3] = { 25, 0, 0.1};
|
||||
#define INIT_POS -130
|
||||
#define TOP_POS -210
|
||||
#define GO_ERROR 1.0f
|
||||
#define Tigger_DO -5
|
||||
#define Tigger_DO -10
|
||||
#define Tigger_ZERO 100
|
||||
#define Tigger_ERROR 3
|
||||
|
||||
//💩
|
||||
int16_t M2006_result =0;
|
||||
|
||||
float knob_increment;
|
||||
|
||||
Shoot::Shoot()
|
||||
{
|
||||
//扳机初始化
|
||||
trigger_Motor= get_motor_point(4);//id 205
|
||||
trigger_Motor= get_motor_point(0);//id 201
|
||||
trigger_Motor->type=M2006;
|
||||
PID_init(&speed_pid,PID_POSITION,M2006_speed_PID,3000, 2000);//pid初始化
|
||||
PID_init(&angle_pid,PID_POSITION,M2006_angle_PID,3000, 2000);//pid初始化
|
||||
@ -69,16 +68,16 @@ void Shoot::trigger_control()
|
||||
int delta = 0;
|
||||
delta = PID_calc(&angle_pid,trigger_Motor->total_angle,t_posSet); // 计算位置环PID
|
||||
result = PID_calc(&speed_pid,trigger_Motor->speed_rpm,delta); // 计算速度环PID
|
||||
M2006_result=result;
|
||||
// result = PID_calc(&speed_pid,trigger_Motor->speed_rpm,t_speedSet);
|
||||
// CAN_cmd_1FF(result,0,0,0,&hcan1);
|
||||
|
||||
|
||||
CAN_cmd_200(result,0,0,0,&hcan1);
|
||||
}
|
||||
|
||||
|
||||
void Shoot :: distanceGet(const NUC_t &nuc_v)
|
||||
{
|
||||
|
||||
//test_distance=nuc_v.vision.x; // 获取自瞄数据
|
||||
distance=nuc_v.vision.x; // 获取自瞄数据
|
||||
|
||||
}
|
||||
|
||||
@ -115,36 +114,47 @@ int Shoot::GO_SendData(float pos,float limit)
|
||||
return 0;
|
||||
}
|
||||
|
||||
//E键 sw[1]👆 200 shoot 中 1000 stop 👇1800 error
|
||||
// F键 sw[0]👆 1800 👇200
|
||||
// E键 sw[1] 👆 200 shoot 中 1000 stop sw[2]200 sw[2]👇1800
|
||||
// G键 sw[6]👆 1800 中 1000 👇200
|
||||
//左旋 sw[7] 200 --1800
|
||||
void Shoot::rc_mode()
|
||||
{
|
||||
if(rc_ctrl.sw[1]==200)
|
||||
{
|
||||
rc_key=UP1;
|
||||
}
|
||||
if(rc_ctrl.sw[1]==1000)
|
||||
if(rc_ctrl.sw[1]==1800 && rc_ctrl.sw[2]==200)
|
||||
{
|
||||
rc_key=MIDDLE1;
|
||||
}
|
||||
if(rc_ctrl.sw[1]==1800)
|
||||
if(rc_ctrl.sw[2]==1800 && rc_ctrl.sw[1]==1800)
|
||||
{
|
||||
rc_key=DOWN1;
|
||||
}
|
||||
if(rc_ctrl.sw[0]==1800)
|
||||
{
|
||||
mode_key=TEST;
|
||||
}
|
||||
if(rc_ctrl.sw[0]==200)
|
||||
{
|
||||
mode_key=VSION;
|
||||
}
|
||||
|
||||
if(rc_ctrl.sw[7]==200)
|
||||
{
|
||||
trigger_key=WAIT;
|
||||
}
|
||||
if(rc_ctrl.sw[7]==1800)
|
||||
{
|
||||
trigger_key=SHOOT;
|
||||
}
|
||||
// if(rc_ctrl.sw[7]==200)
|
||||
// {
|
||||
// trigger_key=WAIT;
|
||||
// }
|
||||
// if(rc_ctrl.sw[7]==1800)
|
||||
// {
|
||||
// trigger_key=SHOOT;
|
||||
// }
|
||||
|
||||
//knob_increment=rc_ctrl.ch[2]/150;
|
||||
|
||||
//旋钮增量
|
||||
static int last_knob_value = 0; // 记录旋钮的上一次值
|
||||
int current_knob_value = rc_ctrl.sw[4]; // 获取当前旋钮值
|
||||
int current_knob_value = rc_ctrl.sw[7]; // 获取当前旋钮值
|
||||
|
||||
// 计算旋钮增量
|
||||
if (current_knob_value >= 200 && current_knob_value <= 1800) {
|
||||
@ -156,15 +166,51 @@ void Shoot::rc_mode()
|
||||
|
||||
}
|
||||
|
||||
|
||||
void Shoot::shoot_control() {
|
||||
|
||||
//方便调试
|
||||
feedback.fd_gopos = go1_data->Pos;
|
||||
feedback.fd_tpos = trigger_Motor->total_angle;
|
||||
|
||||
switch (mode_key)
|
||||
{
|
||||
case VSION:
|
||||
switch (rc_key) {
|
||||
case DOWN1:
|
||||
|
||||
control_pos = INIT_POS; // 默认中间挡位位置
|
||||
//fire_pos = control_pos + knob_increment; // 根据旋钮值调整发射位置
|
||||
fire_pos = distance; // 视觉拟合的力
|
||||
go1.Pos = fire_pos; // 设置蓄力电机位置
|
||||
t_posSet = Tigger_ZERO; // 扳机松开
|
||||
|
||||
if (currentState == SHOOT_READY) {
|
||||
// 如果当前状态是准备发射，松开钩子发射
|
||||
t_posSet = Tigger_ZERO; // 扳机扣动
|
||||
BSP_Buzzer_Stop();
|
||||
if (t_posSet >= 95) { // 假设120度为发射完成角度
|
||||
currentState = SHOOT_IDLE; // 切换到空闲状态
|
||||
is_hooked = false; // 重置钩子状态
|
||||
}
|
||||
} else {
|
||||
currentState = SHOOT_IDLE; // 默认回到空闲状态
|
||||
}
|
||||
break;
|
||||
|
||||
case MIDDLE1:
|
||||
shoot_Current();
|
||||
break;
|
||||
|
||||
case UP1:
|
||||
RemoveError();
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case TEST:
|
||||
switch (rc_key) {
|
||||
case DOWN1:
|
||||
case DOWN1:
|
||||
|
||||
control_pos = INIT_POS; // 默认中间挡位位置
|
||||
//fire_pos = control_pos + knob_increment; // 根据旋钮值调整发射位置
|
||||
@ -196,6 +242,13 @@ void Shoot::shoot_control() {
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
// 发送数据到蓄力电机
|
||||
GO_SendData(go1.Pos, 5.0f);
|
||||
@ -220,7 +273,7 @@ void Shoot :: shoot_Current()
|
||||
|
||||
case SHOOT_TOP:
|
||||
t_posSet = Tigger_DO; // 扳机扣动钩住
|
||||
if (trigger_Motor->total_angle<-2)
|
||||
if (trigger_Motor->total_angle<-6)
|
||||
{
|
||||
//判定为钩住
|
||||
is_hooked = true; // 标记钩子已钩住
|
||||
@ -234,7 +287,7 @@ void Shoot :: shoot_Current()
|
||||
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);
|
||||
BSP_Buzzer_Set(1,5000);
|
||||
// see_you_again();
|
||||
//currentState = SHOOT_WAIT; // 等待发射信号
|
||||
//在拨杆切换时触发了
|
||||
|
||||
@ -33,7 +33,9 @@ typedef enum
|
||||
MIDDLE1,
|
||||
DOWN1,
|
||||
SHOOT,
|
||||
WAIT
|
||||
WAIT,
|
||||
TEST,
|
||||
VSION
|
||||
}rc_mode;
|
||||
|
||||
// 光电传感器读取宏
|
||||
@ -77,11 +79,12 @@ public:
|
||||
|
||||
//==========================公共变量==========================
|
||||
int16_t rc_key; //遥控器按键
|
||||
int16_t mode_key;
|
||||
int16_t trigger_key;
|
||||
ShootState_t currentState; //状态机
|
||||
//volatile BallState_t ballStatus;//是否有球
|
||||
volatile uint32_t flag_thread;//接收传回的线程通知
|
||||
float distance; //视觉距离
|
||||
fp32 distance; //视觉距离
|
||||
|
||||
private:
|
||||
//扳机2006
|
||||
|
||||
@ -10,15 +10,6 @@ extern RC_ctrl_t rc_ctrl;
|
||||
Ball ball;
|
||||
//float vofa[8];
|
||||
|
||||
|
||||
// C键 sw[5]👆 1800 中1000 👇200
|
||||
// D键 sw[6]👆 200 外伸 👇1800 归位 //三摩擦架子
|
||||
// H键 sw[7]👆 200 👇1800 //取球
|
||||
|
||||
int angle1=330;
|
||||
int angle2=-240;
|
||||
int test111=0;
|
||||
|
||||
//检查光电
|
||||
int abc=0;
|
||||
|
||||
@ -37,19 +28,14 @@ void FunctionBall(void *argument)
|
||||
task_struct.stack_water_mark.ball = osThreadGetStackSpace(osThreadGetId());
|
||||
#endif
|
||||
|
||||
abc=HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin);
|
||||
abc=HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin);
|
||||
|
||||
ball.rc_mode(); // 遥控器模式
|
||||
|
||||
ball.ball_control(); // 控制球的动作
|
||||
|
||||
// vofa[0] = -speedm; // 发送电机角度数据
|
||||
// vofa[1] = ball.hand_Motor[0]->speed_rpm; // 发送电机角度数据
|
||||
// vofa[2] = -ball.hand_Motor[1]->speed_rpm; // 发送电机速度数据
|
||||
// vofa[3] = -ball.hand_Motor[2]->speed_rpm; // 发送电机速度数据
|
||||
// vofa_tx_main(vofa); // 发送数据到虚拟串口
|
||||
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
osDelayUntil(tick);
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
osDelayUntil(tick);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@ -10,15 +10,8 @@ extern RC_ctrl_t rc_ctrl;
|
||||
Shoot shoot;
|
||||
NUC_t nucData; // 自瞄
|
||||
|
||||
int shoot_flag = 0;
|
||||
|
||||
int a2;
|
||||
|
||||
int goangle=0;
|
||||
|
||||
//E键 sw[0]👆 200 中 1000 👇1800
|
||||
//F键 sw[1]👆 1800 👇200
|
||||
|
||||
void FunctionShoot(void *argument)
|
||||
{
|
||||
(void)argument; /* 未使用argument，消除警告 */
|
||||
@ -39,19 +32,17 @@ while(1)
|
||||
|
||||
if(shoot.flag_thread & BALL_OK)
|
||||
{
|
||||
a2=2;
|
||||
|
||||
}
|
||||
|
||||
|
||||
shoot_flag=HAL_GPIO_ReadPin(STOP_GPIO_Port, STOP_Pin);
|
||||
|
||||
if (osMessageQueueGet(task_struct.msgq.nuc, &nucData, NULL, 0) == osOK)
|
||||
{
|
||||
shoot.distanceGet(nucData);
|
||||
}
|
||||
|
||||
//shoot.GO_SendData(goangle,5);
|
||||
shoot.shoot_control();
|
||||
// shoot.GO_SendData(goangle,5);
|
||||
// shoot.shoot_control();
|
||||
// shoot.t_posSet=goangle;
|
||||
// shoot.trigger_control();
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user