91 lines
2.0 KiB
C++
91 lines
2.0 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 1000
|
|
|
|
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)
|
|
{
|
|
|
|
speedSet[MOTOR_1] = -speed;
|
|
result[MOTOR_1] = PID_calc(&speed_pid[MOTOR_1],hand_Motor[MOTOR_1]->speed_rpm,speedSet[MOTOR_1]);
|
|
|
|
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]);
|
|
|
|
}
|
|
|
|
CAN_cmd_200(result[MOTOR_1],result[MOTOR_2],result[MOTOR_3],0,&hcan1);
|
|
osDelay(1);
|
|
|
|
}
|
|
|
|
void Ball::ballHadling(void)
|
|
{
|
|
if (key > 0) // 检测按键是否被按下
|
|
{
|
|
if (key % 2 == 1) // key 为奇数，摩擦轮正转
|
|
{
|
|
Spin(5000); // 正转
|
|
}
|
|
else if (key % 2 == 0) // key 为偶数，摩擦轮反转
|
|
{
|
|
Spin(-5000); // 反转
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
// 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;
|
|
// }
|
|
// }
|
|
// }
|