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; } }