// 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);
}
// 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);
}
}
// 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);
}
public void AddServoPosition(ServoPosition servoPosition)
{
servoPositionList.Add(servoPosition);
}
// 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);
}
// 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);
}
}
public void AddServoPosition(ServoPosition servoPosition)
{
servoPositionList.Add(servoPosition);
}
// 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);
}
