public void AddServos(IEnumerable <byte> servoIDs)
{
foreach (var servoID in servoIDs)
{
if (!this.FServos.ContainsKey(servoID))
{
var modelNumber = this.GetModelNumber(servoID);
var servo = new Servo(this, servoID, modelNumber);
this.FServos.Add(servoID, servo);
}
}
}
protected override void Perform(DynamixelSDKSharp.Servo servo, Action <int> goToPosition, int startPosition)
{
//move by wave with amplitude 1/8 of a revolution (+/- 45 degrees)
var movementAmount = (int)((float)servo.ProductSpecification.EncoderResolution / 8);
//move +
goToPosition(startPosition + (int)movementAmount);
//move -
goToPosition(startPosition - (int)movementAmount);
//go back to start
goToPosition(startPosition);
}
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 void Refresh()
{
List <byte> foundServoIDs = new List <byte>();
this.FWorkerThread.DoSync(() =>
{
//send ping
NativeFunctions.broadcastPing(this.FPortNumber, (int)this.ProtocolVersion);
var result = NativeFunctions.getLastTxRxResult(this.FPortNumber, (int)this.ProtocolVersion);
if (result != NativeFunctions.COMM_SUCCESS)
{
var exceptionString = Marshal.PtrToStringAnsi(NativeFunctions.getTxRxResult((int)this.ProtocolVersion, result));
throw (new Exception(exceptionString));
}
//get ping results
for (byte id = 0; id < NativeFunctions.MAX_ID; id++)
{
if (NativeFunctions.getBroadcastPingResult(this.FPortNumber, (int)this.ProtocolVersion, (int)id))
{
foundServoIDs.Add(id);
}
}
});
//add new found servos
foreach (var servoID in foundServoIDs)
{
if (!this.FServos.ContainsKey(servoID))
{
var modelNumber = this.Read(servoID, 0, 2);
var servo = new Servo(this, servoID, modelNumber);
this.FServos.Add(servoID, servo);
}
}
//remove any servos not seen any more
{
var toRemove = this.FServos.Where(pair => !foundServoIDs.Contains((byte)pair.Key))
.Select(pair => pair.Key)
.ToList();
foreach (var key in toRemove)
{
this.FServos.Remove(key);
}
}
}
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);
}
protected abstract void Perform(DynamixelSDKSharp.Servo servo, Action <int> goToPosition, int startPosition);
public object Perform()
{
//Get ports
var ports = PortPool.X.Ports;
//Get database collection
var collection = Database.Connection.X.GetCollection <Database.Registers>();
//We get the register objects from the first servo we find in order to know addresses and sizes
DynamixelSDKSharp.Servo firstServo = null;
var report = new Dictionary <string, List <byte> >();
Parallel.ForEach(ports, (portIterator) =>
{
var listOfServosToLog = new List <byte>();
var port = portIterator.Value;
// check which servos need to be logged for either high frequency or because they've never been logged
foreach (var servoIterator in port.Servos)
{
var servoID = servoIterator.Key;
var servo = servoIterator.Value;
// store the first servo
if (firstServo == null)
{
firstServo = servoIterator.Value;
}
else
{
//check all servos are the same model number
if (servoIterator.Value.ProductSpecification.ModelNumber != firstServo.ProductSpecification.ModelNumber)
{
throw (new Exception("Mixed model numbers are not supported"));
}
}
// get data log entries for this servo
var documents = from document in collection.AsQueryable <Database.Registers>()
where document.ServoID == servoID
orderby document.TimeStamp descending
select document;
if (Database.HighFrequencyRequests.X.IsHighFrequencyNow(servoID))
{
// It's on high frequency list
listOfServosToLog.Add(servoID);
}
else if (documents.Count() == 0)
{
// It has never been logged
listOfServosToLog.Add(servoID);
}
}
if (port.Servos.Count == 0)
{
//quit early if this port has no servos
return;
}
//check regular logging (choose oldest)
{
var staleTimeForRegularUpdates = DateTime.Now - TimeSpan.FromSeconds(Logger.FSettings.Period);
var documents = collection.AsQueryable()
.OrderByDescending(row => row.TimeStamp)
.GroupBy(row => row.ServoID)
.Where(group => group.First().TimeStamp < staleTimeForRegularUpdates.ToUniversalTime())
.Select(group => group.Key);
//Ideally we don't want to call ToList first as this means we perform the following operation locally
var servosWithOldData = documents.Take(Logger.FSettings.MaxServoCount).ToList();
foreach (var servoWithOldData in servosWithOldData)
{
listOfServosToLog.Add((byte)servoWithOldData);
}
}
// trim any servos which aren't available on this port
listOfServosToLog.RemoveAll(servoID =>
{
return(!port.Servos.ContainsKey(servoID));
});
// accumulate data logs
var recordedValues = new Dictionary <RegisterType, Dictionary <byte, int> >();
if (listOfServosToLog.Count > 0)
{
foreach (var registerType in Logger.FSettings.Registers)
{
var registerInfo = firstServo.Registers[registerType];
if (this.UseGroupSyncRead)
{
recordedValues.Add(registerType
, portIterator.Value.GroupSyncRead(listOfServosToLog
, registerInfo.Address
, registerInfo.Size));
}
else
{
var values = new Dictionary <byte, int>();
foreach (var servoID in listOfServosToLog)
{
values.Add(servoID, port.Read(servoID
, registerInfo.Address
, registerInfo.Size));
}
recordedValues.Add(registerType, values);
}
}
}
//save data into database per servo
foreach (var servoID in listOfServosToLog)
{
var valuesForServo = new Dictionary <string, int>();
foreach (var registerValuesIterator in recordedValues)
{
valuesForServo.Add(registerValuesIterator.Key.ToString()
, registerValuesIterator.Value[servoID]);
}
var row = new Database.Registers
{
ServoID = servoID,
TimeStamp = DateTime.Now,
RegisterValues = valuesForServo
};
collection.InsertOne(row);
}
//accumulate report log
lock (report)
{
report.Add(String.Format("{0} ({1})", port.Name, port.Address), listOfServosToLog);
}
});
return(report);
}
