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


}