protected override void Perform(DynamixelSDKSharp.Servo servo, Action <int> goToPosition, int startPosition)
{
if (useMidvalue)
{
//go to middle between limits
var min = servo.ReadValue(RegisterType.MinPositionLimit);
var max = servo.ReadValue(RegisterType.MaxPositionLimit);
goToPosition((max + min) / 2);
}
else
{
//go to middle of encoder values
goToPosition(servo.ProductSpecification.EncoderResolution / 2);
}
}
public object Perform()
{
var results = new Dictionary <int, object>();
using (var speechSynthesizer = new SpeechSynthesizer())
{
speechSynthesizer.SelectVoiceByHints(VoiceGender.Female, VoiceAge.Adult);
DynamixelSDKSharp.Servo servo = null;
DateTime startServoTime = DateTime.Now;
Action checkTimeout = () =>
{
var didTimeout = (DateTime.Now - startServoTime).TotalSeconds > this.timeout;
if (didTimeout)
{
throw(new Exception(String.Format("Timeout nuding servo {0}", servo.ID)));
}
};
var servos = PortPool.X.Servos;
foreach (var servoKeyValue in servos)
{
try
{
servo = servoKeyValue.Value;
//save time for timeout checks
startServoTime = DateTime.Now;
//speak the servo ID
Prompt speechPrompt = null;
if (this.voiceEnabled)
{
try
{
speechPrompt = speechSynthesizer.SpeakAsync("Servo " + servoKeyValue.Key.ToString());
}
catch
{
speechPrompt = null;
}
}
//store values before we started
var oldRegisters = new List <Register>();
oldRegisters.AddRange(new[] {
servo.ReadRegister(RegisterType.TorqueEnable),
servo.ReadRegister(RegisterType.ProfileVelocity),
servo.ReadRegister(RegisterType.ProfileAcceleration),
servo.ReadRegister(RegisterType.PositionIGain)
});
//start position
var startPosition = servo.ReadValue(RegisterType.PresentPosition);
//turn on LED
servo.WriteValue(RegisterType.LED, 1);
//set movement properties
servo.WriteValue(RegisterType.TorqueEnable, 1);
servo.WriteValue(RegisterType.ProfileVelocity, 20);
servo.WriteValue(RegisterType.ProfileAcceleration, 2);
servo.WriteValue(RegisterType.PositionIGain, 100);
//set any custom registers
if (this.registers != null)
{
foreach (var register in this.registers.Values)
{
//store old value
//Note : these will enter the FIFO list after the registers above, so will overwrite on the way outs
oldRegisters.Add(register.Clone() as Register);
//set new value
servo.WriteValue(register);
}
}
//function for moving to position
Action <int> goToPosition = (int target) =>
{
//clamp target position to valid range
if (target < 0)
{
target = 0;
}
if (target >= servo.ProductSpecification.EncoderResolution)
{
target = servo.ProductSpecification.EncoderResolution - 1;
}
servo.WriteValue(RegisterType.GoalPosition, target);
while (Math.Abs(servo.ReadValue(RegisterType.PresentPosition) - target) > this.accuracy)
{
checkTimeout();
Thread.Sleep(500);
}
};
//----
//Do the action
this.Perform(servo, goToPosition, startPosition);
//
//----
//reset registers
foreach (var register in oldRegisters)
{
servo.WriteValue(register);
}
//wait for speech to finish
if (speechPrompt != null)
{
while (!speechPrompt.IsCompleted)
{
checkTimeout();
Thread.Sleep(10);
}
}
//log success
Logger.Log(Logger.Level.Trace, String.Format("Servo {0} success", servo.ID), this.GetType());
//turn off LED if successful
servo.WriteValue(RegisterType.LED, 0);
//output success result
results.Add(servo.ID, new
{
success = true
});
}
catch (Exception e)
{
//log fail
Logger.Log <NudgeAllServos>(Logger.Level.Error, e);
//speak fail
if (this.voiceEnabled)
{
speechSynthesizer.Speak(String.Format("Issue with servo {0}", servo.ID));
}
//output fail result
results.Add(servo.ID, new
{
success = false,
exception = new Utils.ExceptionMessage(e)
});
}
}
if (voiceEnabled)
{
speechSynthesizer.SpeakAsync("END");
}
}
return(results);
}