43 lines
1.0 KiB
C++
43 lines
1.0 KiB
C++
#include "TopDefine.h"
|
|
#include "motor.hpp"
|
|
#include "remote_control.h"
|
|
#include "calc_lib.h"
|
|
extern RC_ctrl_t rc_ctrl;
|
|
const float Trigger::Trigger_speed_PID[3]={10, 0 ,0}; //3508P,I,D(速度环)
|
|
const float Trigger::Trigger_angle_PID[3]={10.0, 0 ,0}; //3508P,I,D(角度环)
|
|
|
|
Trigger::Trigger()
|
|
{
|
|
T_Motor = get_motor_point(1);//获取电机数据指针
|
|
T_Motor->type = M2006;//设置电机类型
|
|
PID_init(&speed_pid,PID_POSITION,Trigger_speed_PID,7000,2000);//pid初始化
|
|
PID_init(&angle_pid,PID_POSITION,Trigger_angle_PID,700,0);//pid初始化
|
|
|
|
result = 0;
|
|
angleSet = 0;
|
|
}
|
|
|
|
void Trigger::triggerZero()
|
|
{
|
|
int16_t delta[1];
|
|
|
|
angleSet = ZER0;
|
|
|
|
delta[0] = PID_calc(&angle_pid,T_Motor->total_angle,angleSet);
|
|
result = PID_calc(&speed_pid, T_Motor->speed_rpm, delta[0]);
|
|
|
|
|
|
}
|
|
|
|
void Trigger::triggerFlow(float angle)
|
|
{
|
|
int16_t delta[1];
|
|
//下降
|
|
angleSet = angle;
|
|
|
|
delta[0] = PID_calc(&angle_pid,T_Motor->total_angle,angleSet);
|
|
result = PID_calc(&speed_pid, T_Motor->speed_rpm, delta[0]);
|
|
|
|
|
|
}
|