public void addCommandToQueue(MotorCommand command)
{
MotorPWM pwm = new MotorPWM();
pwm.header = command.header;
pwm.pwm = new byte[command.voltage.Length];
for (int i = 0; i < command.voltage.Length; ++i)
{
byte temp = (byte)Math.Round(command.voltage [i] / supply_.voltage [0] * 255.0);
if (temp < 0)
{
pwm.pwm [i] = 0;
}
else
if (temp > 255)
{
pwm.pwm [i] = 255;
}
else
{
pwm.pwm [i] = temp;
}
}
addPWMToQueue(pwm);
}
public void SetMotorCommand(MotorCommand command)
{
engaged = true;
SetForwardSpeed(command.forwardSpeed);
SetWheelDirection(command.turningAngle);
LastCommand = command;
}
public override MotorState execute(MotorCommand command)
{
try
{
return proxy.execute(command);
}
catch (Exception e)
{
// todo: allow this to propogate to the caller
Console.Error.WriteLine("ERROR: Error while executing motor command: {0}", e.StackTrace);
}
return null;
}
public override MotorState execute(MotorCommand command)
{
try
{
return(proxy.execute(command));
}
catch (Exception e)
{
// todo: allow this to propogate to the caller
Console.Error.WriteLine("ERROR: Error while executing motor command: {0}", e.StackTrace);
}
return(null);
}
public bool Send(MotorCommand data)
{
if (data == null)
{
return(false);
}
byte[] bytes = new byte[22];
bytes[0] = (byte)data.Address;
bytes[1] = (byte)data.Command;
bytes[2] = (byte)(data.Speed / 256);
bytes[3] = (byte)(data.Speed % 256);
saveInt(bytes, 4, data.Line);
saveInt(bytes, 8, data.X);
saveInt(bytes, 12, data.Y);
saveInt(bytes, 16, data.Z);
bytes[20] = data.ParamA;
bytes[21] = data.ParamB;
Send(bytes);
return(true);
}
public Task<object> Invoke(IDictionary<string, object> input)
{
if (input.ContainsKey("action"))
{
var action = input["action"].ToString();
if (action == "init")
{
return Task.Run<object>(() => { return Init(input); });
}
if (action == "list")
{
return Task.Run<object>(() => { return Device.GetPorts(); });
}
if (device != null)
{
if (action == "write")
{
Task.Run<object>(() => { return device.Send((byte[])input["data"]); });
}
if (action == "command")
{
var obj = new MotorCommand();
if (input.ContainsKey("line"))
{
obj.Line = Convert.ToInt32(input["line"]);
}
if (input.ContainsKey("address"))
{
obj.Address = Convert.ToByte(input["address"]);
}
if (input.ContainsKey("command"))
{
obj.Command = Convert.ToByte(input["command"]);
}
if (input.ContainsKey("speed"))
{
obj.Speed = Convert.ToUInt16(input["speed"]);
}
if (input.ContainsKey("x"))
{
obj.X = Convert.ToInt32(input["x"]);
}
if (input.ContainsKey("y"))
{
obj.Y = Convert.ToInt32(input["y"]);
}
if (input.ContainsKey("z"))
{
obj.Z = Convert.ToInt32(input["z"]);
}
if (input.ContainsKey("paramA"))
{
obj.ParamA = Convert.ToByte(input["paramA"]);
}
if (input.ContainsKey("paramB"))
{
obj.ParamB = Convert.ToByte(input["paramB"]);
}
return Task.Run<object>(() => { return device.Send(obj); });
}
if (action == "close")
{
device.Close();
Task.Run<object>(() => { return (object)null; });
}
if (action == "read")
{
return Task.Run<object>(() => {
try{
var result = device.Read();
return result;
}
catch(Exception err){
//return err.StackTrace;
throw err;
}
});
}
if (action == "state")
{
return Task.Run<object>(() => { return device.GetState(); });
}
}
}
else
{
throw new InvalidOperationException("Unsupported type of command.");
}
return null;
}
public bool Send(MotorCommand data)
{
if (data == null)
{
return false;
}
byte[] bytes = new byte[22];
bytes[0] = (byte)data.Address;
bytes[1] = (byte)data.Command;
bytes[2] = (byte)(data.Speed / 256);
bytes[3] = (byte)(data.Speed % 256);
saveInt(bytes, 4, data.Line);
saveInt(bytes, 8, data.X);
saveInt(bytes, 12, data.Y);
saveInt(bytes, 16, data.Z);
bytes[20] = data.ParamA;
bytes[21] = data.ParamB;
Send(bytes);
return true;
}
/// <summary>
/// Control SRV-1 robot's motors using predefined commands.
/// </summary>
///
/// <param name="command">Motor command to send to the SRV-1 Blackfin robot.</param>
///
/// <remarks><para><note>Controlling SRV-1 motors with this method is only available
/// after at least one direct motor command is sent, which is done using <see cref="StopMotors"/> or
/// <see cref="RunMotors"/> methods.</note></para></remarks>
///
public void ControlMotors(MotorCommand command)
{
Send(new byte[] { (byte)command });
}
/// <summary>
/// Control SRV-1 robot's motors using predefined commands.
/// </summary>
///
/// <param name="command">Motor command to send to the SRV-1 Blackfin robot.</param>
///
/// <remarks><para><note>Controlling SRV-1 motors with this method is only available
/// after at least one direct motor command is sent, which is done using <see cref="StopMotors"/> or
/// <see cref="RunMotors"/> methods.</note></para></remarks>
///
public void ControlMotors( MotorCommand command )
{
Send( new byte[] { (byte) command } );
}
public abstract MotorState execute(MotorCommand command);
public virtual IEnumerator <ITask> SetMotorSpeedHandler(SetMotorSpeed update)
{
if (trace)
{
LogInfo("SetMotorSpeed Received");
}
if (this.qwerkController != null)
{
try
{
MotorCommand motorCommand = new MotorCommand();
motorCommand.motorMask = new bool[]
{
update.Body.Motor0 != null,
update.Body.Motor1 != null,
update.Body.Motor2 != null,
update.Body.Motor3 != null
};
motorCommand.motorVelocities = new int[]
{
(update.Body.Motor0 == null) ? 0 : (int)update.Body.Motor0,
(update.Body.Motor1 == null) ? 0 : (int)update.Body.Motor1,
(update.Body.Motor2 == null) ? 0 : (int)update.Body.Motor2,
(update.Body.Motor3 == null) ? 0 : (int)update.Body.Motor3
};
motorCommand.motorModes = new MotorMode[]
{
MotorMode.MotorSpeedControl,
MotorMode.MotorSpeedControl,
MotorMode.MotorSpeedControl,
MotorMode.MotorSpeedControl
};
motorCommand.motorAccelerations = DEFAULT_MOTOR_ACCELERATIONS;
motorCommand.motorPositions = DEFAULT_MOTOR_POSITIONS;
if (detailedTrace)
{
LogInfo("Calling execute() on MotorService with this MotorCommand");
LogInfo(" Mask: " + ArrayUtils.arrayToString(motorCommand.motorMask));
LogInfo(" Modes: " + arrayToString <MotorMode>(motorCommand.motorModes));
LogInfo(" Velocities: " + ArrayUtils.arrayToString(motorCommand.motorVelocities));
LogInfo(" Accelerations: " + ArrayUtils.arrayToString(motorCommand.motorAccelerations));
LogInfo(" Positions: " + ArrayUtils.arrayToString(motorCommand.motorPositions));
}
this.qwerkController.getMotorService().execute(motorCommand);
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 abstract MotorState execute(MotorCommand command);
public Task <object> Invoke(IDictionary <string, object> input)
{
if (input.ContainsKey("action"))
{
var action = input["action"].ToString();
if (action == "init")
{
return(Task.Run <object>(() => { return Init(input); }));
}
if (action == "list")
{
return(Task.Run <object>(() => { return Device.GetPorts(); }));
}
if (device != null)
{
if (action == "write")
{
Task.Run <object>(() => { return(device.Send((byte[])input["data"])); });
}
if (action == "command")
{
var obj = new MotorCommand();
if (input.ContainsKey("line"))
{
obj.Line = Convert.ToInt32(input["line"]);
}
if (input.ContainsKey("address"))
{
obj.Address = Convert.ToByte(input["address"]);
}
if (input.ContainsKey("command"))
{
obj.Command = Convert.ToByte(input["command"]);
}
if (input.ContainsKey("speed"))
{
obj.Speed = Convert.ToUInt16(input["speed"]);
}
if (input.ContainsKey("x"))
{
obj.X = Convert.ToInt32(input["x"]);
}
if (input.ContainsKey("y"))
{
obj.Y = Convert.ToInt32(input["y"]);
}
if (input.ContainsKey("z"))
{
obj.Z = Convert.ToInt32(input["z"]);
}
if (input.ContainsKey("paramA"))
{
obj.ParamA = Convert.ToByte(input["paramA"]);
}
if (input.ContainsKey("paramB"))
{
obj.ParamB = Convert.ToByte(input["paramB"]);
}
return(Task.Run <object>(() => { return device.Send(obj); }));
}
if (action == "close")
{
device.Close();
Task.Run <object>(() => { return((object)null); });
}
if (action == "read")
{
return(Task.Run <object>(() => {
try{
var result = device.Read();
return result;
}
catch (Exception err) {
//return err.StackTrace;
throw err;
}
}));
}
if (action == "state")
{
return(Task.Run <object>(() => { return device.GetState(); }));
}
}
}
else
{
throw new InvalidOperationException("Unsupported type of command.");
}
return(null);
}
