#ifndef TURNTAABLE_H
#define TURNTAABLE_H


#include "app_power.h"

#ifdef __cplusplus
extern "C" {
#endif

#define   MASTER_CANID             0xFD    
  
#define   PITCH_MOTOR_CANID        0x7D 
#define   RIGHT_MOTOR_CANID        0x7E
#define   TURN_MOTOR_CANID         0x7F   
  
#define   PITCH_MOTOR_RxCANID      (0x20000FD + (PITCH_MOTOR_CANID << 8))  // 0x2007DFD 
#define   RIGHT_MOTOR_RxCANID      (0x20000FD + (RIGHT_MOTOR_CANID << 8))  // 0x2007EFD
#define   TURN_MOTOR_RxCANID       (0x20000FD + (TURN_MOTOR_CANID << 8))   // 0x2007FFD
 
#define   MOTOR_RxCAN_Mask         0x1F00FFFF  //电机CAN接收掩码 移除故障位

#define   LIMIT_SPEED_INDEX        0x7017//CSP的速度
#define   LOC_REF_INDEX            0x7016//CSP的位置 
#define   IQ_REF_INDEX             0x7006//电流模式电流索引 
  
#define   RUM_MODE                 0x7005//mode模式
  
#define   OPERATION_MODE           0   // 运控模式
#define   POSITION_MODE_PP         1   // 位置模式 (PP - Profile Position)
#define   VELOCITY_MODE            2   // 速度模式
#define   CURRENT_MODE             3   // 电流模式
#define   POSITION_MODE_CSP        5   // 位置模式 (CSP - Cyclic Synchronous Position)  
  
  
// 最大允许的单步变化量 (A)
#define MAX_STEP     1.0f   // 单步最大变化量 (A)，从5A开始
#define MAX_DI_DT    1000.0f // 最大变化率 (A/s)，从5000A/s开始
  
  
// 数学常数定义
#define   PI                         3.14159f  // 高精度π值

// 角度量程定义 (对应Byte0~1: 当前角度)
#define   ANGLE_RANGE_MIN           (-4.0f * PI)             // 最小角度: -4π 弧度
#define   ANGLE_RANGE_MAX           (4.0f * PI)              // 最大角度: 4π 弧度
  
// 角速度量程定义 (对应Byte2~3: 当前角速度)  
#define   ANGULAR_VELOCITY_MIN      -15.0f                   // 最小角速度: -15 rad/s
#define   ANGULAR_VELOCITY_MAX      15.0f                    // 最大角速度: 15 rad/s
 

#define   MOTOR_VELOCITY_DEADZONE    1.0f// 最大力矩: 120 Nm      
  
// 力矩量程定义 (对应Byte4~5: 当前力矩)
#define   TORQUE_MIN                -120.0f                  // 最小力矩: -120 Nm
#define   TORQUE_MAX                120.0f                   // 最大力矩: 120 Nm  
          
#define   ZERO_VAULE                0x0080                   // 32768,需要高位在前


  
  
#define SWAP_ENDIAN_16(x) ((((x) & 0xFF) << 8) | (((x) >> 8) & 0xFF))
#define SWAP_ENDIAN_32(x) (((x) << 24) | (((x) & 0xFF00) << 8) | (((x) >> 8) & 0xFF00) | ((x) >> 24))  
  
  
  
typedef struct TurnableData
{
    uint8_t turnable_state;
    PowerState  current_state;         // 当前电源状态       

    float position_x;                  //转台相对位置x
    float position_y;                  //转台相对位置y
    float position_z;                  //转台相对位置z
    
    float desired_speed;               // 期望转台速度
    float desired_pitch_position;      // 期望俯仰位置
    float desired_horizontal_position; // 期望水平位置    
    
    float left_motor_speed;            // 当前左电机速度
    float right_motor_speed;           // 当前右电机速度     
    float speed;                       // 当前转盘速度
    float pitch_position;              // 当前俯仰位置
    float horizontal_position;         // 当前水平位置     
                                      
    float max_speed;                   // 最大速度
    float out_left_motor_ampere;       // 输出左电机电流
    float out_right_motor_ampere;      // 输出右电机电流
    float out_pitch_motor_ampere;      // 输出右电机电流 
    
    float out_left_motor_ampere_last;  // 输出左电机电流
    float out_right_motor_ampere_last; // 输出右电机电流
    float out_pitch_motor_ampere_last; // 输出右电机电流 

    float out_left_motor_ampere_limit;  // 输出左电机电流限制值
    float out_right_motor_ampere_limit; // 输出右电机电流限制值
    float out_pitch_motor_ampere_limit; // 输出右电机电流限制值    
    
    
    Timer turnable_timer;              // 定时器
    Timer turnable_timer1;             // 定时器      
    Timer turnable_timer2;             // 定时器    
    uint8_t turnable_cnt;    
                                      
    float max_ampere;                  // 最大电流限制       
} TurnableData;  
  


void turnableInit();
    

#ifdef __cplusplus
}
#endif

#endif // TURNTAABLE_H