public override bool Send(CommBuffer buffer, out bool send, out bool free)
{
float val = (expr == null) ? ugao : expr.Value();
float dir1 = (val - c.r.rot + 360) % 360;
float dir2 = (c.r.rot - (val - 360)) % 360;
val = (Math.Abs(dir1) < Math.Abs(dir2)) ? dir1 : dir2;
// Metadata
buffer.Write(0xFF);
buffer.Write(0x0A);
buffer.Write((byte)(8));
// Data
buffer.Write((byte)(val > 0 ? 0x06 : 0x07));
//buffer.Write((byte)0x00);
int Ugao = (int)(Math.Abs(val) * 3100 / 90);
buffer.Write((byte)(Ugao & 0x000F));
buffer.Write((byte)((Ugao & 0x00F0) >> 4));
buffer.Write((byte)((Ugao & 0x0F00) >> 8));
buffer.Write((byte)((Ugao & 0xF000) >> 12));
buffer.Write((byte)(ID & 0x0F));
buffer.Write((byte)((ID & 0xF0) >> 4));
return(base.Send(buffer, out send, out free));
}
public override bool Send(CommBuffer buffer, out bool send, out bool free)
{
send = false;
free = true;
return(Convert.ToBoolean(expr.Value()));
}
public override bool Send(CommBuffer buffer, out bool send, out bool free)
{
float val = (expr == null) ? ugao : expr.Value();
// Metadata
buffer.Write(0xFF);
buffer.Write(0x0A);
buffer.Write((byte)(8));
// Data
buffer.Write((byte)(val > 0 ? 0x06 : 0x07));
//buffer.Write((byte)0x00);
//int Ugao = (int)(Math.Abs(val) * 3100 / 90);
int Ugao = (int)(Math.Abs(val) * c.r.rotationConst);
buffer.Write((byte)(Ugao & 0x000F));
buffer.Write((byte)((Ugao & 0x00F0) >> 4));
buffer.Write((byte)((Ugao & 0x0F00) >> 8));
buffer.Write((byte)((Ugao & 0xF000) >> 12));
buffer.Write((byte)(ID & 0x0F));
buffer.Write((byte)((ID & 0xF0) >> 4));
return(base.Send(buffer, out send, out free));
}
public override bool Send(CommBuffer buffer, out bool send, out bool free)
{
// Metadata
buffer.Write(0xFF);
buffer.Write(0x09);
buffer.Write(5);
// Data
byte command = 0;
if (arm == Arm.Left)
{
command = 0x03;
}
if (arm == Arm.Right)
{
command = 0x07;
}
if (arm == Arm.Cup)
{
command = 0x0D;
}
//byte command = (byte)((arm == Arm.Left) ? 0x03 : 0x07); //0x0D
buffer.Write(command);
buffer.Write(0x00);
buffer.Write((byte)(ID & 0x0F));
buffer.Write((byte)((ID & 0xF0) >> 4));
return(base.Send(buffer, out send, out free));
}
public override bool Send(CommBuffer buffer, out bool send, out bool free)
{
buffer.Write(0xFF);
buffer.Write(0x07);
buffer.Write(9);
// Data
buffer.Write((byte)(0x10));
//buffer.Write((byte)(val >= 0 ? 0xF1 : 0xF2));
//buffer.Write((byte)0x00);
buffer.Write((byte)servoID);
buffer.Write((byte)(position & 0x000F));
buffer.Write((byte)((position & 0x00F0) >> 4));
buffer.Write((byte)((position & 0x0F00) >> 8));
buffer.Write((byte)((position & 0xF000) >> 12));
buffer.Write((byte)(ID & 0x0F));
buffer.Write((byte)((ID & 0xF0) >> 4));
return(base.Send(buffer, out send, out free));
// TODO: add protocol
return(base.Send(buffer, out send, out free));
}
public override bool Send(CommBuffer buffer, out bool send, out bool free)
{
float val = (expr == null) ? dist : expr.Value();
int pomeraj = (int)(val * 11.7374f);
// Metadata
buffer.Write(0xFF);
buffer.Write(0x0A);
buffer.Write(8);
// Data
buffer.Write((byte)(val >= 0 ? 0x04 : 0x05));
//buffer.Write((byte)(val >= 0 ? 0xF1 : 0xF2));
//buffer.Write((byte)0x00);
pomeraj = Math.Abs(pomeraj);
buffer.Write((byte)(pomeraj & 0x000F));
buffer.Write((byte)((pomeraj & 0x00F0) >> 4));
buffer.Write((byte)((pomeraj & 0x0F00) >> 8));
buffer.Write((byte)((pomeraj & 0xF000) >> 12));
buffer.Write((byte)(ID & 0x0F));
buffer.Write((byte)((ID & 0xF0) >> 4));
return(base.Send(buffer, out send, out free));
}
public virtual bool Send(CommBuffer buffer, out bool send, out bool free)
{
c.r.upToDate = 0;
send = true;
free = true;
return(true);
}
public override bool Send(CommBuffer buffer, out bool send, out bool free)
{
Thread.Sleep(delay);
send = false;
free = true;
return(true);
}
public override bool Send(CommBuffer buffer, out bool send, out bool free)
{
// Metadata
buffer.Write(0xFF);
buffer.Write(0x0A);
buffer.Write(2);
// Data
buffer.Write((byte)(0x11));
return(base.Send(buffer, out send, out free));
}
public override bool Send(CommBuffer buffer, out bool send, out bool free)
{
buffer.Write(0xFF);
buffer.Write(0x0A);
buffer.Write(2);
buffer.Write((byte)0x0A);
//buffer.Write((byte)0x00);
buffer.Write((byte)(ID & 0x0F));
buffer.Write((byte)((ID & 0xF0) >> 4));
return(base.Send(buffer, out send, out free));
}
public void IssueSingleCommand(Command comm)
{
SerialComm serialComm = r.comm;
CommBuffer buffer = serialComm.outputBuffer;
bool send, free;
if (comm.Send(buffer, out send, out free))
{
if (send)
{
serialComm.ForceSendMessage();
}
}
}
public override State DoStuff()
{
Command_StopAll comm = new Command_StopAll(c);
Robot r = c.r;
SerialComm serialComm = r.comm;
CommBuffer buffer = serialComm.outputBuffer;
bool send, free;
comm.Send(buffer, out send, out free);
serialComm.ForceSendMessage();
return(this);
}
public override bool Send(CommBuffer buffer, out bool send, out bool free)
{
int data = (int)prescaler;
// Metadata
buffer.Write(0xFF);
buffer.Write(0x0A);
buffer.Write(6);
// Data
buffer.Write((byte)(0xFB));
//buffer.Write((byte)(val >= 0 ? 0xF1 : 0xF2));
//buffer.Write((byte)0x00);
buffer.Write((byte)(data & 0x000F));
buffer.Write((byte)((data & 0x00F0) >> 4));
buffer.Write((byte)((data & 0x0F00) >> 8));
buffer.Write((byte)((data & 0xF000) >> 12));
return(base.Send(buffer, out send, out free));
}
public override State DoStuff()
{
Robot r = c.r;
if (r.Ready)
{
SerialComm serialComm = r.comm;
CommBuffer buffer = serialComm.outputBuffer;
Command comm = commands.Peek();
bool send, free;
if (comm.Send(buffer, out send, out free))
{
if (send)
{
serialComm.ForceSendMessage();
}
//if (send) serialComm.SendMessage();
if (free)
{
commands.Dequeue();
if (commands.Count == 0)
{
return(c.GetNextState());
}
}
}
Thread.Sleep(100);
}
return(this);
}
public override bool Send(CommBuffer buffer, out bool send, out bool free)
{
Command comm = null;
if (!rotationSent)
{
comm = new Command_Rotate(c, angle);
free = false;
}
else
{
comm = new Command_Move(c, distance);
free = true;
}
bool dummySend, dummyFree;
comm.Send(buffer, out dummySend, out dummyFree);
send = true;
return(true);
}
