/// <summary> /// Drives Kiwi around /// </summary> static void Drive() { if (EStop) { return; } float aSpeed, bSpeed, cSpeed; if (null == _gamepad) { _gamepad = new CTRE.Gamepad(CTRE.UsbHostDevice.GetInstance()); } float x = _gamepad.GetAxis(0); float y = -1 * _gamepad.GetAxis(1); float twist = _gamepad.GetAxis(2); float adjustmentAngle = 0; if (FieldOriented1) { TryFieldOriented(out adjustmentAngle); } else { adjustmentAngle = Orientation; } //Always using radial coordinates allows for easy orientation adjustment outside of field oriented float magnitude = (float)System.Math.Sqrt(x * x + y * y), directionRadians = (float)System.Math.Atan(x / y); x = magnitude * (float)System.Math.Cos(directionRadians - adjustmentAngle); y = magnitude * (float)System.Math.Sin(directionRadians - adjustmentAngle); Deadband(ref x, .05f); Deadband(ref y, .05f); Deadband(ref twist, .05f); aSpeed = x / 2 - (float)System.Math.Sqrt(3) / 2 * y + twist / (2); bSpeed = x / 2 + (float)System.Math.Sqrt(3) / 2 * y + twist / (2); cSpeed = x * -1 + twist / 2; Deadband(ref aSpeed, .07f); Deadband(ref bSpeed, .07f); Deadband(ref cSpeed, .07f); motorA.Set(aSpeed); motorB.Set(bSpeed); motorC.Set(cSpeed); stringBuilder.Append("\t"); stringBuilder.Append(x); stringBuilder.Append("\t"); stringBuilder.Append(y); stringBuilder.Append("\t"); stringBuilder.Append(twist); }