342 lines
9.5 KiB
C++
342 lines
9.5 KiB
C++
#include "ball.hpp"
|
||
#include "bsp_delay.h"
|
||
#include "remote_control.h"
|
||
#include "detect.h"
|
||
#include "userTask.h"
|
||
#include "user_math.h"
|
||
extern RC_ctrl_t rc_ctrl;
|
||
extern int key;
|
||
|
||
// C键 sw[4]👆 200 中1000 👇1800
|
||
// D键 sw[5]👆 1800 👇200
|
||
|
||
#define START 0
|
||
#define OUT 100
|
||
|
||
#if HANDLING3 == 1
|
||
//三摩擦轮运球！！！
|
||
//添加平移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;
|
||
int speedm1=0;
|
||
//PE11 气缸
|
||
|
||
Ball ::Ball()
|
||
{
|
||
detect_init();
|
||
|
||
//两个伸缩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;
|
||
|
||
//三摩擦轮
|
||
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;
|
||
}
|
||
|
||
|
||
//状态机状态初始化
|
||
currentState1= BALL_IDLE;
|
||
|
||
|
||
// for(int i = 0;i < MOTOR_NUM;i ++)
|
||
// {
|
||
// //摩擦轮滤波器初始化
|
||
// LowPassFilter2p_Init(filter.in + i , 500,
|
||
// -1.0f);
|
||
// LowPassFilter2p_Init(filter.out + i, 500,
|
||
// -1.0f);
|
||
// }
|
||
|
||
}
|
||
|
||
void Ball :: Filter_init(float target_freq)
|
||
{
|
||
for(int i = 0;i < MOTOR_NUM;i ++)
|
||
{
|
||
//摩擦轮滤波器初始化
|
||
LowPassFilter2p_Init(filter.in + i , target_freq,
|
||
-1.0f);
|
||
LowPassFilter2p_Init(filter.out + i, target_freq,
|
||
-1.0f);
|
||
}
|
||
}
|
||
|
||
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 ::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)
|
||
{
|
||
|
||
speedSet[MOTOR_1] = -speed;
|
||
result[MOTOR_1] = PID_calc(&speed_pid[MOTOR_1],hand_Motor[MOTOR_1]->speed_rpm,speedSet[MOTOR_1]);
|
||
|
||
speedSet[MOTOR_2] = speed;
|
||
result[MOTOR_2] = PID_calc(&speed_pid[MOTOR_2],hand_Motor[MOTOR_2]->speed_rpm,speedSet[MOTOR_2]);
|
||
|
||
speedSet[MOTOR_3] = speed;
|
||
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);//确保闭合气缸
|
||
speedm=-500;
|
||
|
||
}
|
||
|
||
void Ball::ballStop(void)
|
||
{
|
||
speedm=0;
|
||
|
||
}
|
||
|
||
int ball_have = 0;
|
||
void Ball::ballTake(void)
|
||
{
|
||
ball_have=HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin);//有球 1 无球 0
|
||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET);//确保闭合气缸
|
||
speedm=2000;
|
||
if(ball_have==1)
|
||
{
|
||
speedm=0;
|
||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);//确保闭合气缸
|
||
}
|
||
}
|
||
|
||
|
||
|
||
int flag =0;
|
||
int ball_state = 0;
|
||
|
||
void Ball::ballHadling(void)
|
||
{
|
||
|
||
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 (key > 0) // 检测按键是否被按下
|
||
if (rc_ctrl.sw[4] > 1000||key > 0) // 检测按键是否被按下,自动回弹的
|
||
{
|
||
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 )
|
||
{
|
||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET);// 打开气缸
|
||
currentState1 = BALL_DROP; // 切换到球下落状态
|
||
}
|
||
break;
|
||
|
||
case BALL_DROP:
|
||
|
||
if (ball_state == 0) //读光电 有球 1 无球 0
|
||
{
|
||
osDelay(200); // 延时200ms
|
||
speedm=3500;
|
||
currentState1 = BALL_REVERSE; // 切换到反转状态
|
||
}
|
||
|
||
break;
|
||
|
||
case BALL_REVERSE:
|
||
|
||
if (ball_state == 1)
|
||
{
|
||
|
||
speedm=0; // 停止电机
|
||
currentState1 = BALL_CLOSE; // 切换到完成状态
|
||
}
|
||
|
||
break;
|
||
|
||
case BALL_CLOSE:
|
||
// osDelay(200); // 延时50ms
|
||
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; // 重置按键状态
|
||
flag=0;
|
||
speedm=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(0,0,e_result[0],e_result[1],&hcan1);
|
||
osDelay(2);
|
||
|
||
}
|
||
|
||
#else
|
||
|
||
int ball_state = 0;
|
||
int triggerCount = 0; // 光电传感器触发计数
|
||
int last_ball_state = 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) // 检测按键是否被按下
|
||
{
|
||
key = 0; // 重置按键状态
|
||
triggerCount = 0; // 重置触发计数
|
||
currentState1 = BALL_FORWARD; // 切换到正转状态
|
||
}
|
||
break;
|
||
|
||
case BALL_FORWARD:
|
||
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_SET); // 打开气缸爪子
|
||
osDelay(5);
|
||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET); // 打开下气缸
|
||
currentState1 = BALL_DROP; // 切换到球下落状态
|
||
break;
|
||
|
||
case BALL_DROP:
|
||
osDelay(100); // 延时 100ms
|
||
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) // 第一次触发
|
||
{
|
||
currentState1 = BALL_FLAG; // 切换到等待第二次触发状态
|
||
}
|
||
}
|
||
last_ball_state = ball_state; // 更新上一次的状态
|
||
break;
|
||
|
||
case BALL_FLAG:
|
||
osDelay(10); // 延时 50ms
|
||
if (triggerCount == 1 && ball_state == 0 && last_ball_state == 1) // 第二次检测到球
|
||
{
|
||
triggerCount++; // 增加触发计数
|
||
currentState1 = BALL_CLOSE; // 切换到闭合气缸状态
|
||
}
|
||
last_ball_state = ball_state; // 更新上一次的状态
|
||
break;
|
||
|
||
case BALL_CLOSE:
|
||
if (triggerCount == 2) // 确保是第二次触发
|
||
{
|
||
osDelay(100); // 延时去抖
|
||
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); // 闭合气缸爪子
|
||
currentState1 = BALL_REVERSE; // 切换到反转状态
|
||
}
|
||
break;
|
||
|
||
case BALL_REVERSE:
|
||
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:
|
||
currentState1 = BALL_IDLE; // 默认回到空闲状态
|
||
break;
|
||
}
|
||
}
|
||
#endif
|
||
|