public void Perform(Servo servo)
{
var value = servo.ReadValue(this.RegisterType);
bool isTooLow = value < this.MinValue;
bool isTooHigh = value > this.MaxValue;
if (isTooLow || isTooHigh)
{
//value is outside of range
//always log
Logger.Log <Constraints>(this.LogLevel, String.Format("Servo {0} register {1} has value {2} which is out of range ({3}->{4}). Performing action {5}."
, servo.ID
, this.RegisterType.ToString()
, value
, this.MinValue
, this.MaxValue
, this.Action.ToString()));
switch (this.Action)
{
case ActionType.ShutdownOne:
servo.WriteValue(RegisterType.TorqueEnable, 0);
break;
case ActionType.ShutdownAll:
PortPool.X.ShutdownAll();
break;
case ActionType.ClampValue:
if (isTooLow)
{
servo.WriteValue(this.RegisterType, this.MinValue);
}
else
{
servo.WriteValue(this.RegisterType, this.MaxValue);
}
break;
case ActionType.MiddleValue:
{
var middleValue = (this.MaxValue - this.MinValue) / 2 + this.MinValue;
servo.WriteValue(this.RegisterType, middleValue);
}
break;
case ActionType.TakeARest:
{
ServoRestManager.X.PutToRest(servo);
}
break;
case ActionType.LimitCurrent:
{
var torqueWasEnabled = servo.ReadValue(RegisterType.TorqueEnable);
var previousGoalPosition = servo.ReadValue(RegisterType.GoalPosition);
servo.WriteValue(RegisterType.TorqueEnable, 0);
{
servo.WriteValue(RegisterType.CurrentLimit, 100);
}
servo.WriteValue(RegisterType.TorqueEnable, torqueWasEnabled);
servo.WriteValue(RegisterType.GoalPosition, previousGoalPosition);
}
break;
default:
break;
}
}
if (!this.LastPerformTime.ContainsKey(servo.ID))
{
this.LastPerformTime.Add(servo.ID, DateTime.Now);
}
else
{
this.LastPerformTime[servo.ID] = DateTime.Now;
}
}