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