#include "Classis_Wheels.h"
#include "can.h"
#include "Parameter.h"
#include <math.h>

PID_PositionInitTypedef Wheels_SpeedPID[4];

extern M3508_Motor Can1_M3508_MotorStatus[8];
extern M3508_Motor Can2_M3508_MotorStatus[8];
extern M6020_Motor Can1_M6020_MotorStatus[7];//GM6020电机状态数组
extern M6020_Motor Can2_M6020_MotorStatus[7];//GM6020电机状态数组
extern Remote_Data Remote_RxData;//遥控器接收数据
extern CAN_HandleTypeDef hcan1;

//-------------------测试区
#define BigYaw_MediumPosition -114//大yaw中位位置(直接读大yaw电机角度)
#define Mecanum_rx 0.24f  //底盘中心到轮子中心的距离的x轴分量(m)
#define Mecanum_ry 0.24f  //底盘中心到轮子中心的距离的y轴分量(m)
#define Mecanum_WheelRadius 0.07f  //轮子半径(m)
#define MAX_RPM 2000  // 最大转速，根据电机调整
#define L 0.2f           // 轮距（单位：米）
float K_yaw = 10.0f;       // 旋转速度的比例系数

//#define Wheel_RightFront							0x201//右前轮
//#define Wheel_LeftFront								0x202//左前轮
//#define Wheel_LeftRear								0x203//左后轮
//#define Wheel_RightRear								0x204//右后轮

void Classis_Wheels_Init(void)
{
	PID_PositionStructureInit (&Wheels_SpeedPID[0],0);
  PID_PositionSetParameter  (&Wheels_SpeedPID[0],10,0,0);
  PID_PositionSetOUTRange   (&Wheels_SpeedPID[0],-20000,20000);
  PID_PositionSetEkRange    (&Wheels_SpeedPID[0], -3.0f, 3.0f);
	
	PID_PositionStructureInit (&Wheels_SpeedPID[1],0);
  PID_PositionSetParameter  (&Wheels_SpeedPID[1],10,0,0);
  PID_PositionSetOUTRange   (&Wheels_SpeedPID[1],-20000,20000);
  PID_PositionSetEkRange    (&Wheels_SpeedPID[1], -3.0f, 3.0f);
	
	PID_PositionStructureInit (&Wheels_SpeedPID[2],0);
  PID_PositionSetParameter  (&Wheels_SpeedPID[2],10,0,0);
  PID_PositionSetOUTRange   (&Wheels_SpeedPID[2],-20000,20000);
  PID_PositionSetEkRange    (&Wheels_SpeedPID[2],-3.0f, 3.0f);
	
	PID_PositionStructureInit (&Wheels_SpeedPID[3],0);
  PID_PositionSetParameter  (&Wheels_SpeedPID[3],10,0,0);
  PID_PositionSetOUTRange   (&Wheels_SpeedPID[3],-20000,20000);
  PID_PositionSetEkRange    (&Wheels_SpeedPID[3], -3.0f, 3.0f);

//  PID_PositionStructureInit(&Mecanum_TrackPID,4096);
//	PID_PositionSetParameter(&Mecanum_TrackPID,0,0,0);
//	PID_PositionSetEkRange(&Mecanum_TrackPID,-5,5);
//	PID_PositionSetOUTRange(&Mecanum_TrackPID,-1,1);
	
}
//-------------------测试区

// void Classis_Wheels_Control(void)
// {
//     extern const RC_ctrl_t *local_rc_ctrl;

//     // 1. 获取左摇杆（ch[2]=左右, ch[3]=上下）
//     float vy_chassis = -(float)Remote_RxData.Remote_R_RL;   // 左右 → 横向
//     float vx = -(float)Remote_RxData.Remote_R_UD;   // 上下 → 纵向

//     // 3. 麦轮解算 —— 用一个缩放系数 K 代替所有几何参数
//     const float K = 4.0f;

//     int16_t fl = (int16_t)((vx - vy_chassis) * K);  // 左前
//     int16_t fr = (int16_t)((vx + vy_chassis) * K);  // 右前
//     int16_t bl = (int16_t)((vx + vy_chassis) * K);  // 左后
//     int16_t br = (int16_t)((vx - vy_chassis) * K);  // 右后

//     // 4. 发送目标转速（单位：RPM）
//     Classis_Wheels_Setspeed(fl, fr, bl, br);

// }
/*-------------------测试区
        ↑y
        |
------------------>x      ↑w
        |
        |

-------------------测试区*/
void Classis_Wheels_Control(void)
{
  // 1. 通过遥控器获取车体运动的期望速度:x轴前后平动速度,y轴左右平动速度,转动角速度(前右为正,逆时针为正)
  float vy_chassis = (float)Remote_RxData.Remote_R_UD / 660.0f;//范围-1~1，归一化为-1~1的速度比例
  float vx_chassis = (float)Remote_RxData.Remote_R_RL / 660.0f;
  float sigma = sqrtf(vx_chassis*vx_chassis + vy_chassis*vy_chassis);//归一化系数
  float w_chassis = 0.0f; // 旋转速度暂时设为3
  float theta = BigYaw_MediumPosition - Can2_M6020_MotorStatus[0].ANgle; //云台底盘相对角度
  if(theta > 180.0f) theta -= 360.0f; // 将角度调整到[-180, 180]范围内
  if(theta < -180.0f) theta += 360.0f;

  vy_chassis = vy_chassis / sigma; // 归一化到最大转速
  vx_chassis = vx_chassis / sigma; // 归一化到最大转速

	float vx_=vx_chassis,vy_=vy_chassis;
	vy_chassis = vy_*cosf(theta)-vx_*sinf(theta);//根据底盘云台相对角度修正xy轴速度
	vx_chassis = vx_*cosf(theta)+vy_*sinf(theta);

  //2.
	
	vy_chassis *= 0.2;
  vx_chassis *= 0.2;
	int16_t RightFrontSpeed	=(int16_t)((-vx_chassis+vy_chassis+w_chassis*(Mecanum_rx+Mecanum_ry))/Mecanum_WheelRadius*181.43663512f);//右前
	int16_t LeftFrontSpeed	=(int16_t)((+vx_chassis+vy_chassis-w_chassis*(Mecanum_rx+Mecanum_ry))/Mecanum_WheelRadius*181.43663512f);//左前
	int16_t LeftRearSpeed		=(int16_t)((-vx_chassis+vy_chassis-w_chassis*(Mecanum_rx+Mecanum_ry))/Mecanum_WheelRadius*181.43663512f);//左后
	int16_t RightRearSpeed	=(int16_t)((+vx_chassis+vy_chassis+w_chassis*(Mecanum_rx+Mecanum_ry))/Mecanum_WheelRadius*181.43663512f);//右后
//w' = v/R (rad/s) = v/(2Π×R) (r/s)(圈每秒) = 60×v/(2Π×R) (r/min)=1/19 w
//⇒ w=19 × 60 × v/(2Π×R)=19×60×v/(2Π×R)=181.43663512×v/R (圈每分),R单位m
  Classis_Wheels_Setspeed(RightFrontSpeed,LeftFrontSpeed,LeftRearSpeed,RightRearSpeed);
	
  
  



















}



void Classis_Wheels_Setspeed(int16_t LeftFrontSpeed,int16_t RightFrontSpeed,int16_t LeftRearSpeed,int16_t RightRearSpeed)
{
	
	//更改期望
	Wheels_SpeedPID[0].Need_Value	=LeftFrontSpeed;//左前轮
	Wheels_SpeedPID[1].Need_Value	=RightFrontSpeed;//右前轮;
	Wheels_SpeedPID[2].Need_Value	=LeftRearSpeed;//左后轮
	Wheels_SpeedPID[3].Need_Value	=RightRearSpeed;//右后轮
	
	
	//PID计算
	PID_PositionCalc(&Wheels_SpeedPID[0],Can1_M3508_MotorStatus[0].RotorSpeed);//右前轮
	PID_PositionCalc(&Wheels_SpeedPID[1],Can1_M3508_MotorStatus[1].RotorSpeed);//左前轮
	PID_PositionCalc(&Wheels_SpeedPID[2],Can1_M3508_MotorStatus[2].RotorSpeed);//左后轮
	PID_PositionCalc(&Wheels_SpeedPID[3],Can1_M3508_MotorStatus[3].RotorSpeed);//右后轮

//	Mecanum_Current=0;

//	for(uint8_t i=0;i<4;i++)
//		Mecanum_Current+=fabs(Wheels_SpeedPID[i].OUT);
//	if(Mecanum_Current>Mecanum_CurrentLimit)
//	{
//		Wheels_SpeedPID[0].OUT*=(Mecanum_CurrentLimit/Mecanum_Current);
//		Wheels_SpeedPID[1].OUT*=(Mecanum_CurrentLimit/Mecanum_Current);
//		Wheels_SpeedPID[2].OUT*=(Mecanum_CurrentLimit/Mecanum_Current);
//		Wheels_SpeedPID[3].OUT*=(Mecanum_CurrentLimit/Mecanum_Current);
//	}
//	
	Motor_3508_Current1((int16_t)Wheels_SpeedPID[0].OUT , (int16_t)Wheels_SpeedPID[1].OUT,
											(int16_t)Wheels_SpeedPID[2].OUT , (int16_t)Wheels_SpeedPID[3].OUT,
											&hcan1);//M3508控制
}