public void OnServoTimed(ServoComponent servo, float startValue, float endValue, int delayMillis)
{
ServoCommand command1 = new ServoCommand(servo.BoardID, startValue);
ServoCommand command2 = new ServoCommand(servo.BoardID, endValue);
_listener.OnTimedCommand(command1, command2, delayMillis);
}
public override void execute(ServoCommand cmd)
{
try
{
proxy.execute(cmd);
}
catch (Exception e)
{
// todo: allow this to propogate to the caller
Console.Error.WriteLine("ERROR: Error in executing servo command: {0}", e.StackTrace);
}
}
public void Verify_Timed_Servo_Command()
{
TestStandMapping mapping = new TestStandMapping();
Session session = new Session(mapping);
DataStore dataStore = new DataStore(session);
Mock <IUserInterface> ui = new Mock <IUserInterface>();
Mock <LogThread> logThread = new Mock <LogThread>(dataStore);
IOThread ioThread = new IOThread(dataStore, ref session);
Program p = new Program(dataStore, logThread.Object, ioThread, ui.Object);
ServoCommand cmd1 = new ServoCommand(6, 69.0f);
ServoCommand cmd2 = new ServoCommand(6, 0.0f);
byte[] actual = {};
Mock <ISerialPort> serial = new Mock <ISerialPort>();
serial.Setup(x => x.IsOpen).Returns(true);
serial.Setup(x => x.Write(It.IsAny <byte[]>(), It.IsAny <int>(), It.IsAny <int>()))
.Callback((byte[] b, int o, int c) =>
{
actual = b;
});
ioThread.StartConnection(serial.Object, null);
p.OnTimedCommand(cmd1, cmd2, 2000);
Thread.Sleep(IOThread.AckWaitMillis - 10);
byte[] expected1 =
{
0xFD, 0xFF, 0xFF, 0xFF, 0xFF,
0x00, 0x01, 0x00, 0x03,
0x06, 0xB0, 0xA3,
0xFE, 0xFF, 0xFF, 0xFF, 0xFF
};
Assert.AreEqual(expected1, actual);
Thread.Sleep(2000 - IOThread.AckWaitMillis - 10);
byte[] expected2 =
{
0xFD, 0xFF, 0xFF, 0xFF, 0xFF,
0x00, 0x01, 0x00, 0x03,
0x06, 0x00, 0x00,
0xFE, 0xFF, 0xFF, 0xFF, 0xFF
};
Assert.AreEqual(expected2, actual);
ioThread.StopConnection();
}
public virtual IEnumerator <ITask> SetServoPositionsHandler(SetServoPositions update)
{
if (trace)
{
LogInfo("SetServoPositions Received");
}
if (this.qwerkController != null)
{
try
{
TeRK.ServoCommand servoCommand = new ServoCommand();
servoCommand.servoMask = new bool[16];
servoCommand.servoPositions = new int[16];
servoCommand.servoSpeeds = new int[16];
servoCommand.servoModes = new ServoMode[16];
for (int i = 0; i < 16; i++)
{
servoCommand.servoMask[i] = (update.Body.ServoPosArray[i] != null);
servoCommand.servoPositions[i] = (update.Body.ServoPosArray[i] == null) ? 0 : (int)update.Body.ServoPosArray[i];
servoCommand.servoSpeeds[i] = (update.Body.ServoVelArray[i] == null) ? 1000 : (int)update.Body.ServoVelArray[i];
servoCommand.servoModes[i] = (update.Body.ServoVelArray[i] == null) ? ServoMode.ServoMotorPositionControl : ServoMode.ServoMotorSpeedControl;
}
if (detailedTrace)
{
LogInfo("Calling execute() on ServoService with this ServoCommand");
LogInfo(" Mask: " + ArrayUtils.arrayToString(servoCommand.servoMask));
LogInfo(" Modes: " + arrayToString <ServoMode>(servoCommand.servoModes));
LogInfo(" Speeds: " + ArrayUtils.arrayToString(servoCommand.servoSpeeds));
LogInfo(" Positions: " + ArrayUtils.arrayToString(servoCommand.servoPositions));
}
this.qwerkController.getServoService().execute(servoCommand);
update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
}
catch (Exception x)
{
update.ResponsePort.Post(generateFault(x.ToString()));
}
}
else
{
update.ResponsePort.Post(generateFault("Not Connected To Qwerk"));
}
yield break;
}
public void OnServoPressed(ServoComponent servo, float value)
{
ServoCommand command = new ServoCommand(servo.BoardID, value);
_listener.OnCommand(command);
}
public abstract void execute(ServoCommand cmd);
public override void execute(ServoCommand cmd)
{
try
{
proxy.execute(cmd);
}
catch (Exception e)
{
// todo: allow this to propogate to the caller
Console.Error.WriteLine("ERROR: Error in executing servo command: {0}", e.StackTrace);
}
}
