void Loop10Ms() { /* get all the buttons */ FillBtns(ref _btns); /* scale the x-axis to go from 0 to sensorRange, left to right */ float leftAxisX = (((_sensorRange / 2) * _gamepad.GetAxis(0)) + (_sensorRange / 2)); float rightAxisX = kJoystickScaler * _gamepad.GetAxis(2); Deadband(ref rightAxisX); if (rightAxisX != 0) { _talon.SetControlMode(CTRE.TalonSrx.ControlMode.kPercentVbus); _talon.Set(rightAxisX); } else if (_talon.GetControlMode() == CTRE.TalonSrx.ControlMode.kPercentVbus) { _targetPosition = _talon.GetPosition(); /* user has let go of the stick, lets closed-loop whereever we happen to be */ EnableClosedLoop(); } /* When you press the 'A' button on a Logitech Gamepad * and the enable button is pressed */ if (_btns[2] && !_btnsLast[2] && _gamepad.GetButton(kEnableButton)) { _targetPosition = servo(leftAxisX, _talon.GetPosition(), _sensorRange); EnableClosedLoop(); } }
/// <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); }