/* * 超音波センサによる障害物検知 * @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; }