//
// TailControl.cpp
//

#include "TailControl.h"
#include "math.h"
//=============================================================================
TailControl::TailControl(Motor* pMotorT)
	: m_pMotorT(pMotorT)
{
}

TailControl::~TailControl(){
}

//*****************************************************************************
// 関数名 : tail_control
// 引数  : angle (モータ目標角度[度])
// 返り値 : 無し
// 概要 : 走行体完全停止用モータの角度制御
//*****************************************************************************
void TailControl::tail_control(signed int angle)
{
	float pwm = (float)(angle - nxt_motor_get_count(NXT_PORT_A))*P_GAIN; /* 比例制御 */
	/* PWM出力飽和処理 */
	if (pwm > PWM_ABS_MAX)
	{
		pwm = PWM_ABS_MAX;
	}
	else if (pwm < -PWM_ABS_MAX)
	{
		pwm = -PWM_ABS_MAX;
	}

	nxt_motor_set_speed(NXT_PORT_A, (signed char)pwm, 1);
}

void TailControl::stand(float pwm)
{

	nxt_motor_set_speed(NXT_PORT_A, (signed char)pwm, 1);
}

bool TailControl::isAngle(signed int angle)
{
	float pwm = (float)(angle - nxt_motor_get_count(NXT_PORT_A))*P_GAIN; /* 比例制御 */

	if(fabs(pwm) < 3){
		return true;
	}
	return false;
}
