MC100/Applications/foc/core/controller.h

#ifndef _CONTROLLER_H__
#define _CONTROLLER_H__
#include "math/fix_math.h"
#include "foc/core/PI_Controller.h"
#include "foc/core/foc.h"
#include "foc/core/etcs.h"
#include "foc/limit.h"

#define CTRL_MODE_OPEN                 ((u8)0U)
#define CTRL_MODE_SPD                  ((u8)1U)
#define CTRL_MODE_TRQ                  ((u8)2U)
#define CTRL_MODE_CURRENT              ((u8)3U)
#define CTRL_MODE_EBRAKE               ((u8)4U)

#define FOC_LIM_NO_CHANGE  0
#define FOC_LIM_CHANGE_H   1
#define FOC_LIM_CHANGE_L   2

#define SECTOR_1  0u
#define SECTOR_2  1u
#define SECTOR_3  2u
#define SECTOR_4  3u
#define SECTOR_5  4u
#define SECTOR_6  5u
#define SECTOR_UKNOW 0xFF

typedef enum {
	EPM_Dir_None,
	EPM_Dir_Back,
	EPM_Dir_Forward,
}epm_dir_t;

typedef enum {
	FOC_Success = 0,
	FOC_NotAllowed = 1,
	FOC_Have_CritiCal_Err,
	FOC_Throttle_Err, //ready的时候检测到转把信号
	FOC_NowAllowed_With_Speed,
	FOC_Speed_TooLow,
	FOC_NotCruiseMode,
	FOC_Param_Err,
	FOC_MEM_Err,
	FOC_CRC_Err,
	FOC_Unknow_Cmd,
}error_code_t;

typedef enum {
	FOC_CRIT_OV_Vol_Err,
	FOC_CRIT_UN_Vol_Err,
	FOC_CRIT_ACC_OV_Err,
	FOC_CRIT_ACC_Un_Err,
	FOC_CRIT_Phase_Err,
	FOC_CRIT_Encoder_Err, /* 编码器错误，可能还是可以骑行，取决无感是否稳定 */
	FOC_CRIT_Angle_Err, /* FOC 角度错误，一般发生在编码器错误，同时无感没有稳定的情况下，必须要停机 */
	FOC_CRIT_CURR_OFF_Err,
	FOC_CRIT_H_MOS_Err,
	FOC_CRIT_L_MOS_Err,
	FOC_CRIT_Phase_Conn_Err,
	FOC_CRIT_MOTOR_TEMP_Lim,
	FOC_CRIT_MOS_TEMP_Lim,
	FOC_CRIT_Fan_Err,
	FOC_CRIT_IDC_OV,
	FOC_CRIT_THRO_Err,
	FOC_CRIT_ENC_AB_Err,
	FOC_CRIT_Vol_HW_Err, //17
	FOC_CRIT_PHASE_UNBalance_Err, /* 三相不平衡错误，比如相线螺丝松了 */
	FOC_CRIT_THRO2_Err,
	FOC_CRIT_MOT_TEMP_Sensor,
	FOC_CRIT_MOS_TEMP_Sensor,  //21
	FOC_CRIT_BRK_Err,
	FOC_CRIT_Err_Max = 32,
}mctrl_critical_ebit_t;

#define FOC_Cri_Err_Mask(err) (1<<(err))

typedef enum {
	FOC_EV_MOT_Limit_L=FOC_CRIT_Err_Max + 1,
	FOC_EV_MOS_Limit_L,
	FOC_EV_THRO_START_V,
	FOC_START_Err_Code,
}ctrl_event_r_t;


typedef struct {
	float mot_vel;
	float torque;
	float dc_curr;
	float phase_curr; //最大相电流
	float ebrk_dc_curr; //最大母线回收电流
	float ebrk_torque;
	float dc_vol_min;
	float dc_vol_max;
}user_limit;

typedef struct {
	float target;
	float interpolation;
	float step;
	float time;
	float time_dec;
	bool  b_force_step;
}lineramp_t;

typedef struct {
	lineramp_t vel;
	lineramp_t torque;
	lineramp_t dc_curr;
}rt_lim_ramper;

typedef struct {
	float mot_vel;
	float phase_curr;
	float phase_vol;
	float fw_id; //D轴最大退磁电流
	float dc_curr;
	float dc_vol;
	float torque;
}hw_limit;

typedef struct {
	float dc_curr;
	float torque;
}prot_limit;

typedef struct{
	bool 			b_start;
	bool			b_ebrk_running;
	bool			b_hw_braker;
	bool 			b_mtpa_calibrate;
	bool    		b_cruiseEna;
	bool    		b_AutoHold;
	float 			adv_angle; //dq 分配超前角
	float 			force_angle;
	float 			angle_last;
	float   		target_torque;
	float           target_torque_raw; //扭矩过零点处理前的扭矩
	dq_t			target_idq;
	dq_t            out_idq_filterd;
	float 			out_current_vec;
	u32				ebrk_ramp_time;
	u32				torque_acc_time;
	u32				torque_dec_time;
	float   		dc_curr_filted;
	float			dc_curr_calc;
	float   		phase_curr_filted[3];
	float           autohold_torque;
	u8 				mode_req;
	u8 				mode_running;
	u8      		error;
	foc_t 			foc;
	etcs_t          etcs;
	PI_Controller 	pi_lock;
	PI_Controller 	pi_power;
	PI_Controller 	pi_vel_lim;
	PI_Controller 	pi_vel;
	user_limit 		userlim;
	hw_limit   		hwlim;
	prot_limit 		protlim;
	lineramp_t   	target_current;
	lineramp_t 		cruise_vel;
	lineramp_t 		target_vd;
	lineramp_t 		target_vq;
	lineramp_t		target_vel;
	lineramp_t 		vel_lim;
	lineramp_t 		torque_lim;
	lineramp_t 		dc_curr_lim;
	lineramp_t		input_torque;
	float 			phase_a_max;
	float 			phase_b_max;
	float 			phase_c_max;
	u32 			phase_unbalance_cnt;
}mot_contrl_t;

void mot_contrl_init(mot_contrl_t *ctrl);
bool mot_contrl_enable(mot_contrl_t *ctrl, bool start);
bool mot_contrl_update(mot_contrl_t *ctrl);
u8   mot_contrl_mode(mot_contrl_t *ctrl);
bool mot_contrl_request_mode(mot_contrl_t *ctrl, u8 mode);
void mot_contrl_slow_task(mot_contrl_t *ctrl);
u8   mot_contrl_protect(mot_contrl_t *ctrl);
float mot_contrl_get_speed(mot_contrl_t *ctrl);
void mot_contrl_velloop_params(mot_contrl_t *ctrl, float wcv, float b0);
void mot_contrl_trqloop_params(mot_contrl_t *ctrl, float wcv, float b0);
void mot_contrl_set_dccurr_limit(mot_contrl_t *ctrl, float ibusLimit);
void mot_contrl_set_vel_limit(mot_contrl_t *ctrl, float vel);
void mot_contrl_set_torque_limit(mot_contrl_t *ctrl, float torque);
float mot_contrl_get_ebrk_torque(mot_contrl_t *ctrl);
void mot_contrl_set_ebrk_time(mot_contrl_t *ctrl, u32 time);
void mot_contrl_set_vdq(mot_contrl_t *ctrl, float vd, float vq);
void mot_contrl_set_vdq_immediate(mot_contrl_t *ctrl, float vd, float vq);
bool mot_contrl_set_cruise(mot_contrl_t *ctrl, bool enable);
bool mot_contrl_resume_cruise(mot_contrl_t *ctrl);
bool mot_contrl_set_cruise_speed(mot_contrl_t *ctrl, float rpm);
bool mot_contrl_set_current(mot_contrl_t *ctrl, float is);
void mot_contrl_set_torque_ramp_time(mot_contrl_t *ctrl, u32 acc, u32 dec);
void mot_contrl_set_torque_acc_time(mot_contrl_t *ctrl, u32 acc);
bool mot_contrl_set_torque(mot_contrl_t *ctrl, float torque);
void mot_contrl_mtpa_calibrate(mot_contrl_t *ctrl, bool enable);
void mot_contrl_set_autohold(mot_contrl_t *ctrl, bool lock);
bool mot_contrl_energy_recovery(mot_contrl_t *ctrl, bool start);
void mot_contrl_set_hw_brake(mot_contrl_t *ctrl, bool hw_brake);
void mot_contrl_calc_current(mot_contrl_t *ctrl);
void mot_contrl_get_pid(mot_contrl_t *ctrl, u8 id, float *kp, float *ki, float *kd);
void mot_contrl_set_pid(mot_contrl_t *ctrl, u8 id, float kp, float ki, float kd);

static __INLINE void line_ramp_set_target(lineramp_t *line, float target) {
	if (line->target != target) {
		line->target = target;
		if (!line->b_force_step) {
			float delta = (line->target - line->interpolation);
			if (delta >= 0) {
				line->step = delta / line->time;
			}else {
				line->step = -delta / line->time_dec;
			}
		}
	}
}

static __INLINE bool mot_contrl_start(mot_contrl_t *ctrl, u8 mode) {
	mot_contrl_enable(ctrl, true);
	return mot_contrl_request_mode(ctrl, mode);
}

static __INLINE bool mot_contrl_stop(mot_contrl_t *ctrl) {
	return mot_contrl_enable(ctrl, false);
}

static __INLINE bool mot_contrl_is_start(mot_contrl_t *ctrl) {
	return ctrl->b_start;
}
static __INLINE bool mot_contrl_dccurr_is_protected(mot_contrl_t *ctrl) {
	return (ctrl->protlim.dc_curr != HW_LIMIT_NONE);
}

static __INLINE bool mot_contrl_torque_is_protected(mot_contrl_t *ctrl) {
	return (ctrl->protlim.torque != HW_LIMIT_NONE);
}

static __INLINE void mot_contrl_set_ebrk_torquer(mot_contrl_t *ctrl, s16 torque) {
	ctrl->userlim.ebrk_torque = torque;
}

static __INLINE void mot_contrl_set_error(mot_contrl_t *ctrl, u8 error) {
	ctrl->error = error;
}

static __INLINE u8 mot_contrl_get_errcode(mot_contrl_t *ctrl) {
	return ctrl->error;
}

static __INLINE void mot_contrl_pause_cruise(mot_contrl_t *ctrl) {
	ctrl->b_cruiseEna = false;
}

static __INLINE bool mot_contrl_is_cruise_enabled(mot_contrl_t *ctrl) {
	return (ctrl->b_cruiseEna && (ctrl->mode_running == CTRL_MODE_SPD));
}

static __INLINE bool mot_contrl_set_target_vel(mot_contrl_t *ctrl, float vel) {
	if (ctrl->b_cruiseEna) {
		return false;
	}
	line_ramp_set_target(&ctrl->target_vel ,min(ABS(vel), ctrl->userlim.mot_vel)*SIGN(vel));
	return true;
}

static __INLINE void mot_contrl_set_angle(mot_contrl_t *ctrl, float angle) {
	ctrl->force_angle = (angle);
}

static __INLINE void mot_contrl_set_adv_angle(mot_contrl_t *ctrl, float angle) {
	ctrl->adv_angle = (angle);
}

static __INLINE bool mot_contrl_is_auto_holdding(mot_contrl_t *ctrl) {
	return ctrl->b_AutoHold;
}

static __INLINE float mot_contrl_get_current_vector(mot_contrl_t *ctrl) {
	return ctrl->out_current_vec;
} 

static __INLINE float mot_contrl_get_dc_current(mot_contrl_t *ctrl) {
	return ctrl->dc_curr_filted;
} 

static __INLINE bool mot_contrl_ebrk_is_enabled(mot_contrl_t *ctrl) {
	return mot_contrl_get_ebrk_torque(ctrl) != 0;
}

static __INLINE bool mot_contrl_ebrk_is_running(mot_contrl_t *ctrl) {
	return ctrl->mode_running == CTRL_MODE_EBRAKE;
}

static __INLINE float mot_contrl_get_final_torque(mot_contrl_t *ctrl) {
	return ctrl->input_torque.target;
}

static __INLINE u32 mot_contrl_get_torque_acc_time(mot_contrl_t *ctrl) {
	return ctrl->input_torque.time;
}

static __INLINE void line_ramp_init(lineramp_t *line, float time) {
	line->target = 0;
	line->interpolation = 0;
	line->step = 0;
	line->b_force_step = false;
	line->time_dec = line->time = time;
}

static __INLINE void line_ramp_reset(lineramp_t *line, float target) {
	line->target = target;
	line->interpolation = target;
}


static __INLINE void line_ramp_set_time(lineramp_t *line, float time) {
	line->time_dec = line->time = time;
	line->b_force_step = false;
}

static __INLINE void line_ramp_set_dectime(lineramp_t *line, float time) {
	line->time_dec = time;
	line->b_force_step = false;
}

static __INLINE void line_ramp_set_acctime(lineramp_t *line, float time) {
	line->time = time;
	line->b_force_step = false;
}

static __INLINE void line_ramp_set_step(lineramp_t *line, float step) {
	line->step = step;
	line->b_force_step = true;
}

static __INLINE float line_ramp_get_target(lineramp_t *line) {
	return line->target;
}

static __INLINE float line_ramp_get_interp(lineramp_t *line) {
	return line->interpolation;
}


static __INLINE void line_ramp_update(lineramp_t *line) {
	if (!line->b_force_step) {
		float delta = (line->target - line->interpolation);
		if (delta >= 0) {
			line->step = delta / line->time;
		}else {
			line->step = -delta / line->time_dec;
		}
	}
}

static __INLINE float line_ramp_step(lineramp_t *line) {
	step_towards(&line->interpolation, line->target, line->step);
	return line->interpolation;
}

#endif /* _CONTROLLER_H__ */