#pragma once

#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_Param/AP_Param.h>
#include <AP_Math/AP_Math.h>



//履带pid类
class TrackPidClass{
public:
	TrackPidClass(float _p1,float _i1,float _d1,float _p2,float _i2,float _d2);
	//履带电机1
	float motor1_head_target;//目标值
	float motor1_head_measure;//测量值
	float error_head1;//误差
	float error_last_head1;//上一次误差
	float error_last2_head1;//上2次误差
	float p1;//P
	float i1;//I
	float d1;//D
	float sum1;//误差和
	int16_t motor1_out;//输出
	//履带电机2
	float motor2_head_target;//目标值
	float motor2_head_measure;//测量值
	float error_head2;//误差
	float error_last_head2;//上一次误差
	float error_last2_head2;//上2次误差	
	float p2;//P
	float i2;//I
	float d2;//D
	float sum2;//误差和
	int16_t motor2_out;//输出

	int16_t updatePID1(float target,float measure,float _error,float MAX);//履带1PID控制
	int16_t updatePID2(float target,float measure,float _error,float MAX);//履带1PID控制

};