#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;
        }
    }
}