70 lines
1.5 KiB
C++
70 lines
1.5 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 currentBallState = BALL_IDLE;
|
|
|
|
#define MOTOR_SPEED 5000
|
|
|
|
const fp32 Ball:: M3508_speed_PID[3] = {5, 0, 0};
|
|
|
|
//三摩擦轮运球！！！
|
|
|
|
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)
|
|
{
|
|
for(int i = 0;i < MOTOR_NUM;i ++)
|
|
{
|
|
speedSet[i] = speed;
|
|
result[i] = PID_calc(&speed_pid[i],hand_Motor[i]->speed_rpm,speedSet[i]);
|
|
|
|
}
|
|
CAN_cmd_200(result[MOTOR_1],result[MOTOR_2],result[MOTOR_3],0,&hcan1);
|
|
osDelay(1);
|
|
|
|
}
|
|
|
|
void Ball::ballHadling(void)
|
|
{
|
|
static bool isReversed = false; // 静态变量，记录当前摩擦轮状态，初始为正转
|
|
|
|
if (key > 0) // 检测按键是否被按下
|
|
{
|
|
key = 0; // 重置按键状态，防止重复触发
|
|
|
|
if (isReversed)
|
|
{
|
|
// 当前为反转，切换为正转
|
|
Spin(MOTOR_SPEED); // 正转
|
|
isReversed = false;
|
|
}
|
|
else
|
|
{
|
|
// 当前为正转，切换为反转
|
|
Spin(-MOTOR_SPEED); // 反转
|
|
isReversed = true;
|
|
}
|
|
}
|
|
}
|