62 lines
1.4 KiB
C
62 lines
1.4 KiB
C
|
#ifndef _MOTOR_CONTROL_H_
|
||
|
|
#define _MOTOR_CONTROL_H_
|
||
|
|
|
||
|
|
#include "struct_typedef.h"
|
||
|
|
#include "pid.h"
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
typedef struct
|
||
|
|
{
|
||
|
|
fp32 kp;
|
||
|
|
fp32 ki;
|
||
|
|
fp32 kd;
|
||
|
|
|
||
|
|
fp32 set;
|
||
|
|
fp32 get;
|
||
|
|
fp32 err;
|
||
|
|
|
||
|
|
fp32 max_out;
|
||
|
|
fp32 max_iout;
|
||
|
|
|
||
|
|
fp32 Pout;
|
||
|
|
fp32 Iout;
|
||
|
|
fp32 Dout;
|
||
|
|
|
||
|
|
fp32 out;
|
||
|
|
} gimbal_PID_t;
|
||
|
|
|
||
|
|
typedef struct
|
||
|
|
{
|
||
|
|
gimbal_PID_t gimbal_motor_relative_angle_pid;
|
||
|
|
pid_type_def gimbal_motor_gyro_pid;
|
||
|
|
fp32 angel_set;
|
||
|
|
fp32 angel_get;
|
||
|
|
fp32 gyro_set;
|
||
|
|
fp32 gyro_get;
|
||
|
|
fp32 current_set;
|
||
|
|
fp32 current_given;
|
||
|
|
}gimbal_pid_control_t;
|
||
|
|
|
||
|
|
|
||
|
|
#define FILTER_BUF_LEN 5
|
||
|
|
|
||
|
|
//#define MOTOR_ECD_TO_ANGLE 0.00231311936
|
||
|
|
#define MOTOR_ECD_TO_ANGLE (360.0 / 8191.0 / (3591.0/187.0))
|
||
|
|
#define MOTOR_ECD_TO_RAD 0.000766990394f
|
||
|
|
#define rad_format(Ang) loop_fp32_constrain((Ang), (-4096*MOTOR_ECD_TO_RAD), (4095*MOTOR_ECD_TO_RAD))
|
||
|
|
#define rad(code) ((code)*MOTOR_ECD_TO_RAD)
|
||
|
|
|
||
|
|
|
||
|
|
void M3508_motor_init(int mode);
|
||
|
|
void M3508_pos_loop(fp32 angle1,fp32 angle2,fp32 angle3,fp32 angle4);
|
||
|
|
void GM6020_position_loop_init(void);
|
||
|
|
void GM6020_pos_loop(int32_t ecd);
|
||
|
|
void M3508_velocity_loop(int16_t speed1,int16_t speed2,int16_t speed3,int16_t speed4);
|
||
|
|
fp32 gimbal_PID_calc(gimbal_PID_t *pid, fp32 get, fp32 set, fp32 error_delta);
|
||
|
|
fp32 motor_ecd_to_angle_change(uint32_t ecd, uint16_t offset_ecd);
|
||
|
|
void gimbal_PID_init(gimbal_PID_t *pid, fp32 maxout, fp32 max_iout, fp32 kp, fp32 ki, fp32 kd);
|
||
|
|
void motor_speed_change_low_to_high(int rag);
|
||
|
|
#endif
|
||
|
|
|