private void HandleControl(BinaryReader br)
{
RMPControlMsg msg = new RMPControlMsg(br);
//this should not happen
}
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));
}
}
public void SetVelocityTurn(RobotTwoWheelCommand command)
{
double velocity = command.velocity;
double turn = command.turn;
byte direction = 0x3b;//moving forward
///////////////////////////////////////////////////////////////////////////////////
//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;
direction = 0x1b;
}
double rotational_term = Math.PI/180*turn/7.8;
double leftvel = (command.velocity - rotational_term);
double rightvel = (command.velocity + rotational_term);
if (leftvel > 0x50)
leftvel = 0x50;
if (rightvel > 0x50)
rightvel = 0x50;
int pioneerVelocity = (int)(command.velocity*3.04108844*15);//convert counts to mm/sec
RMPControlMsg ctrl = new RMPControlMsg(velocity, turn);
byte[] send = new byte[14];
send[0] = 0x01; //send to pioneer
send[1] = 0x00; //blank
send[2] = 0x00; //len (hi)
send[3] = 0x3; //len (lo) for bridge
send[4] = 0xfa; // Pioneer start byte 1
send[5] = 0xfb; // Pioneer start byte 2
send[6] = 0x6; // pioneer packet length
send[7] = 32; // pioneer command
send[8] = 0x3b; // args type
send[9] = (byte)rightvel; // high byte of vel
send[10] = (byte)leftvel;
send[11] = (byte)((checkSum(send)) >> 8);
send[12] = (byte)((checkSum(send)) & 0xff);// checksum (excluding startbytes+command)
SendData(send, 13);
System.Threading.Thread.Sleep(1000);
/* 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);
send[0] = 0x01; //send to pioneer
send[1] = 0x00; //command
send[2] = 0x00; //len (hi)
send[3] = 0x3; //len (lo)
send[4] = 0xfa; // can msg hi
send[5] = 0xfb; // can msg lo
send[6] = 0x5; // can msg lo
send[7] = 0xb; // can msg lo
send[8] = 0xa; // can msg lo
send[9] = 0xa; // can msg lo
send[10] = (byte)((checkSum(send)) >> 8);
send[11] = (byte)((checkSum(send)) & 0xff);// checksum (excluding startbytes+command)*/
/* MoveMotors(send);
if (simMode)
simRobotCommandServer.SendUnreliably(new SimMessage<RobotTwoWheelCommand>(NetworkAddress.GetSimRobotID("behavioral"), command, wheelPosTs));
else
udpClient.Send(send, 12, new IPEndPoint(ipUnicast, controlPort));*/
}
private void HandleControl(BinaryReader br)
{
RMPControlMsg msg = new RMPControlMsg(br);
//this should not happen
}
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));
}
