#ifndef _DRIVE_RS04_H_
#define _DRIVE_RS04_H_


#define   MASTER_CANID             0xFD  
  
#define   PI                  3.1415926

#define   MOTOR_RxCAN_Mask         0x1F00FFFF  //电机CAN接收掩码 移除故障位

// 最大允许的单步变化量 (A)
#define   MAX_STEP     1.0f   // 单步最大变化量 (A)，从5A开始
#define   MAX_DI_DT    1000.0f // 最大变化率 (A/s)，从5000A/s开始


#define   LIMIT_SPEED_INDEX        0x7017//CSP的速度
#define   LOC_REF_INDEX            0x7016//CSP的位置 
#define   IQ_REF_INDEX             0x7006//电流模式电流索引 
#define   SPD_REF                  0x700A//速度模式 速度值

#define   LIMIT_CUR                0X7018//速度位置模式电流限制


  
#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)  

// 角度量程定义 (对应Byte0~1: 当前角度)
#define   ANGLE_RANGE_MIN           (-4.0f * PI)             // 最小角度: -4π 弧度
#define   ANGLE_RANGE_MAX           (4.0f * PI)              // 最大角度: 4π 弧度
  
// 角速度量程定义 (对应Byte2~3: 当前角速度)  
#define   RS02_ANGULAR_VELOCITY_MAX      20.0f               // RS02型号最大角速度: 20 rad/s
#define   RS04_ANGULAR_VELOCITY_MAX      15.0f               // RS04型号最大角速度: 15 rad/s



 

#define   MOTOR_VELOCITY_DEADZONE    0.1f// 死区
#define   MOTOR_ANGLE_DEADZONE       0.01f// 死区

#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)) 



float constrain(float value, float min_val, float max_val);
float convertPhysical(uint16_t raw_value, float min, float max, float deadzone);
uint32_t floatToUint32(float num);
int8_t motorDisable(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata);
int8_t motorEnable(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata);
int8_t setMotorMode(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata, uint8_t mode);
int8_t setMotorWrite(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata, uint16_t index, float ref);
float dynamic_current_limit(float *last_command, float target_current, float delta_time);




#ifdef __cplusplus
}
#endif

#endif // _DRIVE_RS04_H_