#include "Sub.h"
#include <AP_UAVCAN/AP_UAVCAN.h>


//履带PID类，包含左右履带两个
TrackPidClass::TrackPidClass(float _p1,float _i1,float _d1,float _p2,float _i2,float _d2){
	 motor1_head_target  = 0;
	 motor1_head_measure = 0;
	 error_head1         = 0;
	 error_last_head1    = 0;
	 error_last2_head1   = 0;
	 p1                  = _p1;
	 i1                  = _i1;
	 d1                  = _d1;
	 sum1 = 0.0;
	 
	 motor1_out          = 0;
	 motor2_head_target  = 0;
	 motor2_head_measure = 0;
	 error_head2         = 0;
	 error_last_head2    = 0;
	 error_last2_head2    = 0;
	 p2                  = _p2;
	 i2                  = _i2;
	 d2                  = _d2;
	 sum2 =0.0;
	 motor2_out          = 0;

}

int16_t TrackPidClass::updatePID1(float target,float measure,float _error,float MAX){
	float out=0.0;
	//未解锁复位
	 if (!sub.motors.armed()) {
       sum1=0.0;
       
    }
	 //误差防阶跃
	error_head1 = -measure;//-target+measure;
	 if (error_head1<-180.0)
	{
		 error_head1 +=360.0;
	}
	 else if(error_head1>180.0){
	 	error_head1 -=360.0;
	 }

	sum1+=error_head1;
	sum1 = constrain_float(sum1,-(MAX-31),MAX-31);
	out = p1*error_head1 +i1*sum1 +d1*(error_head1-error_last_head1);
	motor1_out =(int16_t)constrain_float(out,-(MAX-31),MAX-31);
	error_last_head1= error_head1;

return motor1_out;
}
//履带PID 
//IN:目标值 和测量值
//out：PWM
int16_t TrackPidClass::updatePID2(float target,float measure,float _error,float MAX){
	//未解锁复位

	float out=0.0;
	 if (!sub.motors.armed()) {
       sum2=0.0;
       
    }
	 //误差防阶跃

	error_head2 = _error;//target-measure;
	 
	 if (error_head2<-180.0)
	{
		 error_head2 +=360.0;
	}
	 else if(error_head2>180.0){
	 	error_head2 -=360.0;
	 }
 
	sum2+=error_head2;
	sum2 = constrain_float(sum2,-(MAX-31),MAX-31);
	out = p2*error_head2 +i2*sum2 + d2*(error_head2-error_last_head2);
	motor2_out =(int16_t)constrain_float(out,-(MAX-31),MAX-31);
	error_last_head2 = error_head2;

return motor2_out;
}