public void SetRobotSpeed(object sender, SpeedConsigneArgs e)
{
if (robotList.ContainsKey(e.RobotId))
{
robotList[e.RobotId].Vx = e.Vx;
robotList[e.RobotId].Vy = e.Vy;
robotList[e.RobotId].Vtheta = e.Vtheta;
}
}
//Input events
public void GenerateMessageSetSpeedConsigneToRobot(object sender, SpeedConsigneArgs e)
{
byte[] payload = new byte[12];
//Int32 Vx = (Int32)(e.Vx*1000);
//Int32 Vy = (Int32)(e.Vy * 1000);
//Int32 Vtheta = (Int32)(e.Vtheta * 1000);
//payload.SetValueRange(Vx.GetBytes(), 0);
//payload.SetValueRange(Vy.GetBytes(), 4);
//payload.SetValueRange(Vtheta.GetBytes(), 8);
payload.SetValueRange((e.Vx).GetBytes(), 0);
payload.SetValueRange((e.Vy).GetBytes(), 4);
payload.SetValueRange((e.Vtheta).GetBytes(), 8);
OnMessageToRobot((Int16)Commands.SetSpeedConsigne, 12, payload);
}
public void UpdateSpeedConsigneOnGraph(object sender, SpeedConsigneArgs e)
{
oscilloX.AddPointToLine(0, currentTime, e.Vx);
oscilloY.AddPointToLine(0, currentTime, e.Vy);
oscilloTheta.AddPointToLine(0, currentTime, e.Vtheta);
}