150 lines
3.7 KiB
C++
150 lines
3.7 KiB
C++
#include "ball.hpp"
|
|
#include "bsp_delay.h"
|
|
#include "remote_control.h"
|
|
#include "detect.h"
|
|
|
|
extern RC_ctrl_t rc_ctrl;
|
|
extern int key;
|
|
|
|
// 定义状态机变量
|
|
BallState_t currentState1 = BALL_IDLE; // 当前状态
|
|
uint32_t startTime = 0; // 用于记录延时的起始时间
|
|
int speedm=0;
|
|
int speedm1=0;
|
|
|
|
#define MOTOR_SPEED 1000
|
|
|
|
const fp32 Ball:: M3508_speed_PID[3] = {5, 0, 0};
|
|
|
|
//PE11 气缸
|
|
|
|
//三摩擦轮运球！！！
|
|
|
|
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, 6000);//pid初始化
|
|
}
|
|
result[i] = 0;
|
|
speedSet[i] = 0;
|
|
}
|
|
|
|
}
|
|
|
|
void Ball ::Spin(float speed,float speed2)
|
|
{
|
|
|
|
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] = speed2;
|
|
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]);
|
|
|
|
// for(int i = 1;i < MOTOR_NUM;i ++)
|
|
// {
|
|
// speedSet[i] = speed;
|
|
// result[i] = PID_calc(&speed_pid[i],hand_Motor[i]->speed_rpm,speedSet[i]);
|
|
|
|
// }
|
|
|
|
}
|
|
|
|
int flag =0;
|
|
int ball_state = 0;
|
|
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) // 检测按键是否被按下
|
|
{
|
|
speedm=-4500;
|
|
speedm1=-3500;
|
|
currentState1 = BALL_FORWARD; // 切换到正转状态
|
|
}
|
|
break;
|
|
|
|
case BALL_FORWARD:
|
|
|
|
if ( hand_Motor[MOTOR_1]->speed_rpm >= 4480&&hand_Motor[MOTOR_1]->speed_rpm <= 4520 &&
|
|
hand_Motor[MOTOR_2]->speed_rpm <= -2980-500&&hand_Motor[MOTOR_2]->speed_rpm >= -3020-500 &&
|
|
hand_Motor[MOTOR_3]->speed_rpm <= -4480&&hand_Motor[MOTOR_3]->speed_rpm >= -4020-1000 )
|
|
{
|
|
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET);// 打开气缸
|
|
currentState1 = BALL_DROP; // 切换到球下落状态
|
|
}
|
|
break;
|
|
|
|
case BALL_DROP:
|
|
|
|
if (ball_state == 1) //读光电 有球 0 无球 1
|
|
{
|
|
osDelay(200); // 延时200ms
|
|
speedm=3000;
|
|
speedm1=3000;
|
|
currentState1 = BALL_REVERSE; // 切换到反转状态
|
|
}
|
|
|
|
break;
|
|
|
|
case BALL_REVERSE:
|
|
|
|
if (ball_state == 0)
|
|
{
|
|
|
|
flag=2; //抽象的计次
|
|
speedm=0; // 停止电机
|
|
speedm1=0;
|
|
currentState1 = BALL_CLOSE; // 切换到完成状态
|
|
}
|
|
|
|
break;
|
|
|
|
case BALL_CLOSE:
|
|
osDelay(200); // 延时50ms
|
|
if (flag == 2)
|
|
{
|
|
if(ball_state == 0)
|
|
{
|
|
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;
|
|
speedm1=0;
|
|
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);
|
|
|
|
}
|
|
|