public void SetVelocityTurn(RobotTwoWheelCommand command, bool inclined)
{
double velocity = command.velocity;
double turn = command.turn;
double cVelocityKp = .75;
if (inclined)
{
velocity = velocity + cVelocityKp * (velocity - ForwardVelocity * 8.3333);
}
//Console.WriteLine("V: " + velocity + " FV: " + ForwardVelocity*8.3333 + " E: " + (velocity - ForwardVelocity*8.3333));
///////////////////////////////////////////////////////////////////////////////////
//DO NOT CHANGE THESE VALUES WITHOUT ASKING MARK FIRST, THIS IS FOR SAFETY PURPOSES
///////////////////////////////////////////////////////////////////////////////////
if (turn > 600)
{
turn = 600;
}
if (turn < -600)
{
turn = -600;
}
if (velocity > 3)
{
velocity = 3;
}
if (velocity < -1)
{
velocity = -1;
}
///////////////////////////////////////////////////////////////////////////////////
if (isBackwards)
{
velocity *= -1;
}
RMPControlMsg ctrl = new RMPControlMsg(velocity, turn);
byte[] send = new byte[14];
send[0] = 0x00; //send to bridge
send[1] = 0x17; //command
send[2] = 0x00; //len (hi)
send[3] = 0x0a; //len (lo)
send[4] = 0x04; // can msg hi
send[5] = 0x13; // can msg lo
send[6] = (byte)(ctrl.velocityCommand >> 8);
send[7] = (byte)(ctrl.velocityCommand & 0xFF);
send[8] = (byte)(ctrl.turningCommand >> 8);
send[9] = (byte)(ctrl.turningCommand & 0xFF);
send[10] = (byte)((short)ctrl.configurationCommand >> 8);
send[11] = (byte)((short)ctrl.configurationCommand & 0xFF);
send[12] = (byte)(ctrl.configurationParameter >> 8);
send[13] = (byte)(ctrl.configurationParameter & 0xFF);
if (simMode)
{
simRobotCommandServer.SendUnreliably(new SimMessage <RobotTwoWheelCommand>(NetworkAddress.GetSimRobotID("behavioral"), command, wheelPosTs));
}
else
{
udpClient.Send(send, 14, new IPEndPoint(ipUnicast, controlPort));
}
}
