// Starts the specified servo move using the specified speed.
private void StartSpeedMove(ServoPosition servoPosition, long servoSpeed)
{
Servo servo = servoList.Find(x => x.index == servoPosition.index);
SetServoSpeed(servo, servoSpeed);
SetServoPosition(servo, servoPosition.position);
}
// Sets the speed of the specified servo, in the servo controller hardware. This speed
// value is first bounded within the servo's min/max speed limits, and a warning is
// logged if a speed outside of these limits was specified.
private void SetServoSpeed(Servo servo, long speed)
{
if (speed < servo.speedLimitMin)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "Requested servo " + servo.index.ToString() + " speed " + speed.ToString() + " bound to minimum limit " + servo.speedLimitMin.ToString());
// Bound to this limit.
speed = servo.speedLimitMin;
}
if (speed > servo.speedLimitMax)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "Requested servo " + servo.index.ToString() + " speed " + speed.ToString() + " bound to maximum limit " + servo.speedLimitMax.ToString());
// Bound to this limit.
speed = servo.speedLimitMax;
}
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "Setting servo " + servo.index.ToString() + " speed to " + speed.ToString());
try
{
// Send this value to the hardware.
uscDevice.setSpeed((byte)servo.index, (ushort)speed);
}
catch (System.Exception ex)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "Caught exception in SetServoSpeed(): " + ex.Message);
}
}
// Starts the specified servo move using the specified speed. StartSpeedMove is used by PersonTracking
// and RobotSpeech, which are lower priority moves than those issued through Sequences. StartSpeedMove
// motion can be interrupted by Sequence moves, and new StartSpeedMove moves are ignored if a Sequence
// is already moving this servo.
public void StartSpeedMove(ServoPosition servoPosition, long servoSpeed)
{
if (!IsEnabled())
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "StartSpeedMove failed, servos are disabled.");
return;
}
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "StartSpeedMove moving servo " + servoPosition.index.ToString() + " to position " + servoPosition.position.ToString() + " at speed " + servoSpeed.ToString() + ".");
Servo servo = servoList.Find(x => x.index == servoPosition.index);
lock (servo.movePriorityLock)
{
if (servo.isRunningHighPriorityMove)
{
// Ignore this command if a higher priority (i.e. Sequence) move is already running.
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "StartSpeedMove, isRunningHighPriorityMove is set for servo " + servo.index.ToString() + ". Aborting move.");
return;
}
if (!IsConnected())
{
return;
}
SetServoSpeed(servo, servoSpeed);
SetServoPosition(servo, servoPosition.position);
}
}
// Sets the target position of the specified servo, causing this servo to begin moving
// to the new position. This target position is first bounded within the servo's min/max
// position limits, and a warning is logged if a position outside of these limits was
// specified.
private void SetServoPosition(Servo servo, double position)
{
if (position < servo.positionLimitMin)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "Requested servo " + servo.index.ToString() + " position " + position.ToString() + " bound to minimum limit " + servo.positionLimitMin.ToString());
// Bound to this limit.
position = servo.positionLimitMin;
}
if (position > servo.positionLimitMax)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "Requested servo " + servo.index.ToString() + " position " + position.ToString() + " bound to maximum limit " + servo.positionLimitMax.ToString());
// Bound to this limit.
position = servo.positionLimitMax;
}
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "Setting servo " + servo.index.ToString() + " position to " + position.ToString());
try
{
// Send this value to the hardware.
// Note that the servo position values are handled in this class in units of μs,
// to match the convention used by Pololu's Maestro Control Center application.
// However, the servo controller hardware expects the position represented as an
// integer value in 0.25 μs. The local value must be multiplied by 4 to convert
// to these units.
uscDevice.setTarget((byte)servo.index, (ushort)(position * 4));
}
catch (System.Exception ex)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "Caught exception in SetServoPosition(): " + ex.Message);
}
}
// Starts performing all moves specified by servoPositionList, and then waits for these
// servos to finish moving.
public void MoveServos(List <ServoPosition> servoPositionList, double timeToDestination)
{
if (!IsEnabled())
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "MoveServos failed, servos are disabled.");
return;
}
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "MoveServos moving servos over " + timeToDestination + " seconds.");
foreach (ServoPosition servoPosition in servoPositionList)
{
Servo servo = servoList.Find(x => x.index == servoPosition.index);
lock (servo.movePriorityLock)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "MoveServos set isRunningHighPriorityMove for servo " + servo.index.ToString() + ".");
servo.isRunningHighPriorityMove = true;
}
}
if (!IsConnected())
{
// Simulate the move.
Thread.Sleep((int)(timeToDestination * 1000));
foreach (ServoPosition servoPosition in servoPositionList)
{
Servo servo = servoList.Find(x => x.index == servoPosition.index);
lock (servo.movePriorityLock)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "MoveServos cleared isRunningHighPriorityMove for servo " + servo.index.ToString() + ".");
servo.isRunningHighPriorityMove = false;
}
}
return;
}
foreach (ServoPosition servoPosition in servoPositionList)
{
StartTimedMove(servoPosition, timeToDestination);
}
WaitForMoveComplete(servoPositionList, timeToDestination);
foreach (ServoPosition servoPosition in servoPositionList)
{
Servo servo = servoList.Find(x => x.index == servoPosition.index);
lock (servo.movePriorityLock)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "MoveServos cleared isRunningHighPriorityMove for servo " + servo.index.ToString() + ".");
servo.isRunningHighPriorityMove = false;
}
}
}
// Polls the servo hardware to determine if the servos are still moving. Returns when all
// servos in servoPositionList have completed their moves.
private void WaitForMoveComplete(List <ServoPosition> servoPositionList, double timeToDestination)
{
// Create a list of servos to monitor.
List <Servo> servosToMonitor = new List <Servo>();
foreach (ServoPosition servoPosition in servoPositionList)
{
Servo servo = servoList.Find(x => x.index == servoPosition.index);
if (servo == null)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "WaitForMoveComplete failed, servo " + servoPosition.index.ToString() + " not found.");
return;
}
servosToMonitor.Add(servo);
}
// Poll servo positions and wait until all servos reach their destinations.
double pollTimeout = timeToDestination + pollTimeoutAdjustment;
int pollTimeoutCount = (int)(pollTimeout * 1000 / (double)pollPeriod_ms);
int currentPollCount = 0;
while (true)
{
if (currentPollCount >= pollTimeoutCount)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "WaitForMoveComplete timeout, servos failed to reach destination in " + pollTimeout.ToString() + " seconds.");
return;
}
currentPollCount++;
UpdateServoValues();
// Determine if any servos in the list are still moving.
bool servoIsMoving = false;
foreach (Servo servo in servosToMonitor)
{
if (servo.isMoving)
{
servoIsMoving = true;
}
}
if (!servoIsMoving)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "WaitForMoveComplete succeeded, all servos reached destinations.");
return;
}
Thread.Sleep(pollPeriod_ms);
}
}
// Sets the acceleration of the specified servo, in the servo controller hardware.
private void SetServoAcceleration(Servo servo, long acceleration)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "Setting servo " + servo.index.ToString() + " acceleration to " + acceleration.ToString());
try
{
// Send this value to the hardware.
uscDevice.setAcceleration((byte)servo.index, (ushort)acceleration);
}
catch (System.Exception ex)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "Caught exception in SetServoAcceleration(): " + ex.Message);
}
}
public long GetServoIndex(string servoName)
{
Servo servo = servoList.Find(x => x.name == servoName);
if (servo != null)
{
return(servo.index);
}
else
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "GetServoIndex() failed to find servo '" + servoName + "'.");
return(-1);
}
}
// Calculates the required servo speed to reach the position specified by servoPosition,
// in the amount of time indicated by timeToDestination. Then starts this move and returns.
private void StartTimedMove(ServoPosition servoPosition, double timeToDestination)
{
Servo servo = servoList.Find(x => x.index == servoPosition.index);
ushort servoSpeed = 0;
if (servo == null)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "StartTimedMove failed, servo " + servoPosition.index.ToString() + " not found.");
return;
}
if (servo.polledPosition == 0)
{
// If the servo position is 0, then the servo is off and we have no information about its actual position.
// We don't know the actual move distance, so just use the default servo speed.
servoSpeed = (ushort)servo.defaultSpeed;
}
else
{
// Convert current position to μs (the hardware uses 0.25 μs increments).
double currentPosition = ((double)servo.polledPosition) / 4;
// Position difference in μs.
double positionDifference = Math.Abs(servoPosition.position - currentPosition);
// Required speed in (μs/second).
double calculatedSpeed;
if (timeToDestination != 0)
{
calculatedSpeed = positionDifference / timeToDestination;
}
else
{
// If the desired move time is instantaneous, use the max allowed servo speed.
calculatedSpeed = servo.speedLimitMax;
}
// Convert speed from (1 μs / second) to (0.25 μs / 10 ms), used by the hardware.
servoSpeed = (ushort)(calculatedSpeed * (4.0 / 100.0));
}
SetServoSpeed(servo, servoSpeed);
SetServoPosition(servo, servoPosition.position);
}
// Creates an example servo configuration xml file to provide a template for creating
// a complete servo configuration file.
public void GenerateExampleConfigFile(string fileName)
{
Servo exampleServo = new Servo();
exampleServo.index = 0;
exampleServo.name = "Example Servo";
exampleServo.positionLimitMax = 0;
exampleServo.positionLimitMin = 0;
exampleServo.speedLimitMax = 0;
exampleServo.speedLimitMin = 0;
List <Servo> exampleServoList = new List <Servo>();
exampleServoList.Add(exampleServo);
XmlSerializer SerializerObj = new XmlSerializer(typeof(List <Servo>));
TextWriter WriteFileStream = new StreamWriter(fileName);
SerializerObj.Serialize(WriteFileStream, exampleServoList);
WriteFileStream.Close();
}
// Moves the robot's head/eyes to track the specified skeleton joint.
public void PersonTrackingUpdate(SkeletonPoint targetPosition)
{
// Only update if tracking is allowed and if we're not already updating in a
// different thread.
lock (personTrackingLock)
{
if (!trackingEnabled)
{
return;
}
if (isAlreadyUpdatingTracking)
{
return;
}
else
{
isAlreadyUpdatingTracking = true;
}
}
// Only update once every 100ms, even if we're receiving new joint positions
// more frequently than this.
TimeSpan timeBetweenUpdates = new TimeSpan(0, 0, 0, 0, 100);
if (DateTime.Now - lastUpdateTime < timeBetweenUpdates)
{
lock (personTrackingLock)
{
isAlreadyUpdatingTracking = false;
}
return;
}
lastUpdateTime = DateTime.Now;
// Position of the robot neck and eye servos at which the robot faces straight forward.
const double servoCenterPostion_HeadHorizontal = 1550;
const double servoCenterPostion_HeadVertical = 1500;
const double servoCenterPostion_Eyes = 1600;
const double servoOffset_HeadVertical = 0;
// Number of servo position increments per radian of rotation for each of the servos.
const double servoIncrementsPerRadian_HeadHorizontal = 800;
const double servoIncrementsPerRadian_HeadVertical = 1000;
const double servoIncrementsPerRadian_Eyes = 800;
// Tracking speed of each servo.
const long servoSpeed_HeadHorizontal = 30;
const long servoSpeed_HeadVertical = 30;
const long servoSpeed_Eyes = 1000;
if (IsConnected())
{
UpdateServoValues();
}
Servo headHorizontalServo = servoList.Find(x => x.index == 10);
if (headHorizontalServo == null)
{
lock (personTrackingLock)
{
isAlreadyUpdatingTracking = false;
}
// Log Error
return;
}
// Calculate the current position of the head.
double currentPosition_HeadHorizontal = headHorizontalServo.polledPosition / 4.0;
double currentAngle_HeadHorizontal = (servoCenterPostion_HeadHorizontal - currentPosition_HeadHorizontal) / servoIncrementsPerRadian_HeadHorizontal;
// Calculate the angle to the target joint.
double targetAngle_Horizontal = Math.Atan(targetPosition.X / targetPosition.Z);
double targetAngle_Vertical = Math.Atan(targetPosition.Y / targetPosition.Z);
// Calculate the new head position to face this target joint.
double newServoPosition_HeadHorizontal = servoCenterPostion_HeadHorizontal - targetAngle_Horizontal * servoIncrementsPerRadian_HeadHorizontal;
double newServoPosition_HeadVertical = servoCenterPostion_HeadVertical + servoOffset_HeadVertical + targetAngle_Vertical * servoIncrementsPerRadian_HeadVertical;
// Eye position with head at center.
double newServoPosition_Eyes = servoCenterPostion_Eyes + targetAngle_Horizontal * servoIncrementsPerRadian_Eyes;
// Eye position based on current head position.
//double newServoPosition_Eyes = servoCenterPostion_Eyes + (targetAngle_Horizontal - currentAngle_HeadHorizontal) * servoIncrementsPerRadian_Eyes;
//ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "PersonTrackingUpdate: targetAngle_Horizontal = " + targetAngle_Horizontal + ", newServoPosition_HeadHorizontal = " + newServoPosition_HeadHorizontal + ", newServoPosition_Eyes = " + newServoPosition_Eyes);
if (IsConnected())
{
// Update with head motion.
StartSpeedMove(new ServoPosition(10, newServoPosition_HeadHorizontal), servoSpeed_HeadHorizontal);
StartSpeedMove(new ServoPosition(9, newServoPosition_HeadVertical), servoSpeed_HeadVertical);
// Update with only eye motion.
//StartSpeedMove(new ServoPosition(10, servoCenterPostion_HeadHorizontal), servoSpeed_HeadHorizontal);
//StartSpeedMove(new ServoPosition(9, servoCenterPostion_HeadVertical), servoSpeed_HeadVertical);
//StartSpeedMove(new ServoPosition(8, newServoPosition_Eyes), servoSpeed_Eyes);
}
lock (personTrackingLock)
{
isAlreadyUpdatingTracking = false;
}
}
// Sets the speed of the specified servo, in the servo controller hardware. This speed
// value is first bounded within the servo's min/max speed limits, and a warning is
// logged if a speed outside of these limits was specified.
private void SetServoSpeed(Servo servo, long speed)
{
if (speed < servo.speedLimitMin)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "Requested servo " + servo.index.ToString() + " speed " + speed.ToString() + " bound to minimum limit " + servo.speedLimitMin.ToString());
// Bound to this limit.
speed = servo.speedLimitMin;
}
if (speed > servo.speedLimitMax)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "Requested servo " + servo.index.ToString() + " speed " + speed.ToString() + " bound to maximum limit " + servo.speedLimitMax.ToString());
// Bound to this limit.
speed = servo.speedLimitMax;
}
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "Setting servo " + servo.index.ToString() + " speed to " + speed.ToString());
try
{
lock (uscLock)
{
if (uscDevice == null)
{
throw new System.Exception("uscDevice is null");
}
// Send this value to the hardware.
uscDevice.setSpeed((byte)servo.index, (ushort)speed);
}
}
catch (System.Exception ex)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "Caught exception in SetServoSpeed(): " + ex.Message);
}
}
// Sets the target position of the specified servo, causing this servo to begin moving
// to the new position. This target position is first bounded within the servo's min/max
// position limits, and a warning is logged if a position outside of these limits was
// specified.
private void SetServoPosition(Servo servo, double position)
{
if (position < servo.positionLimitMin)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "Requested servo " + servo.index.ToString() + " position " + position.ToString() + " bound to minimum limit " + servo.positionLimitMin.ToString());
// Bound to this limit.
position = servo.positionLimitMin;
}
if (position > servo.positionLimitMax)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "Requested servo " + servo.index.ToString() + " position " + position.ToString() + " bound to maximum limit " + servo.positionLimitMax.ToString());
// Bound to this limit.
position = servo.positionLimitMax;
}
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "Setting servo " + servo.index.ToString() + " position to " + position.ToString());
try
{
lock (uscLock)
{
if (!isEnabled)
{
return;
}
if (uscDevice == null)
{
throw new System.Exception("uscDevice is null");
}
// Send this value to the hardware.
// Note that the servo position values are handled in this class in units of μs,
// to match the convention used by Pololu's Maestro Control Center application.
// However, the servo controller hardware expects the position represented as an
// integer value in 0.25 μs. The local value must be multiplied by 4 to convert
// to these units.
uscDevice.setTarget((byte)servo.index, (ushort)(position * 4));
}
}
catch (System.Exception ex)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "Caught exception in SetServoPosition(): " + ex.Message);
}
}
// Sets the acceleration of the specified servo, in the servo controller hardware.
private void SetServoAcceleration(Servo servo, long acceleration)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "Setting servo " + servo.index.ToString() + " acceleration to " + acceleration.ToString());
try
{
lock (uscLock)
{
if (uscDevice == null)
{
throw new System.Exception("uscDevice is null");
}
// Send this value to the hardware.
uscDevice.setAcceleration((byte)servo.index, (ushort)acceleration);
}
}
catch (System.Exception ex)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "Caught exception in SetServoAcceleration(): " + ex.Message);
}
}
// Creates an example servo configuration xml file to provide a template for creating
// a complete servo configuration file.
public void GenerateExampleConfigFile(string fileName)
{
Servo exampleServo = new Servo();
exampleServo.index = 0;
exampleServo.name = "Example Servo";
exampleServo.positionLimitMax = 0;
exampleServo.positionLimitMin = 0;
exampleServo.speedLimitMax = 0;
exampleServo.speedLimitMin = 0;
List<Servo> exampleServoList = new List<Servo>();
exampleServoList.Add(exampleServo);
XmlSerializer SerializerObj = new XmlSerializer(typeof(List<Servo>));
TextWriter WriteFileStream = new StreamWriter(fileName);
SerializerObj.Serialize(WriteFileStream, exampleServoList);
WriteFileStream.Close();
}
