public bool IsResting(Servo servo)
{
var queryRestingServos = this.ServoRests.Where(rs => rs.Servo == servo);
if (queryRestingServos.Any())
{
//we have a rest assigned for this servo
//remove all rests which are stale
this.ServoRests.RemoveAll(rs => DateTime.Now - rs.RestTimeBegan > ServoRestManager.RestingTime);
//check if we still ahve one for this servo
var newQueryRestingServos = this.ServoRests.Where(rs => rs.Servo == servo);
if (newQueryRestingServos.Any())
{
return(true);
}
else
{
//wake it up
servo.WriteValue(RegisterType.TorqueEnable, 1);
//no longer resting
return(false);
}
}
else
{
return(false);
}
}
public void PutToRest(Servo servo)
{
servo.WriteValue(RegisterType.TorqueEnable, 0);
//check if already exists
var queryExisting = ServoRests.Where(rs => rs.Servo == servo);
if (queryExisting.Any())
{
queryExisting.First().RestTimeBegan = DateTime.Now;
}
else
{
this.ServoRests.Add(new ServoRest
{
RestTimeBegan = DateTime.Now,
Servo = servo
});
}
}
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;
}
}
