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)); } }