/*
* 超音波センサによる障害物検知
* @return true(障害物あり)/false(障害物無し)
*/
public bool sonar_alert(EquipmentParameters body)
{
int distance = body.DistSensors.Read();
// _logger.drl.WriteLine("distance : " + distance);
if ((distance <= EquipmentParameters.SONAR_ALERT_DISTANCE) && (distance >= 0))
{
return true; /* 障害物を検知 */
}
else
{
return false; /* 障害物無し */
}
}
/*
* 走行体完全停止用モータの角度制御
* @param angle モータ目標角度[度]
*/
public static void tailControl(EquipmentParameters body, int angle)
{
float pwm = (float)(angle - body.TailMoter.GetTachoCount()) * EquipmentParameters.P_GAIN; // 比例制御
// PWM出力飽和処理
if (pwm > EquipmentParameters.PWM_ABS_MAX)
{
pwm = EquipmentParameters.PWM_ABS_MAX;
}
else if (pwm < -EquipmentParameters.PWM_ABS_MAX)
{
pwm = -EquipmentParameters.PWM_ABS_MAX;
}
if ((sbyte)pwm == 0)
{
body.TailMoter.Brake();
}
else
{
// body.TailMoter.PowerProfile(pwm, EquipmentParameters.P_GAIN, )
body.TailMoter.SetPower((sbyte)pwm);
// body.TailMoter.SetSpeed((sbyte)pwm);
}
}
/*
* 走行体完全停止用モータの角度制御
* @param angle モータ目標角度[度]
*/
public static void tailControl(EquipmentParameters body, bool IsLight)
{
body.LightSensors.Mode = ColorMode.Reflection;
body.LightSensors.Mode = ColorMode.Ambient;
}