- public void turnAngle(double angle) {
- angle = MathLib.constrain(angle, -360, 360);
- setGyroPID();
- setSetpoint(angle);
+ // if (type == Constants.DriveTrain.TANK) {
+ // // TODO Test this for negation and for voltage vs. [-1,1] outputs
+ // double leftSpeed = (-frontLeft.get() + -rearLeft.get()) / 2;
+ // double rightSpeed = (-frontRight.get() + -rearRight.get()) / 2;
+ // left = ((Constants.DriveTrain.kADJUST - 1) * leftSpeed + left)
+ // / Constants.DriveTrain.kADJUST;
+ // right = ((Constants.DriveTrain.kADJUST - 1) * rightSpeed + right)
+ // / Constants.DriveTrain.kADJUST;
+ // robotDrive.tankDrive(-left * k, -right * k);
+ // } else if (type == Constants.DriveTrain.ARCADE) {
+ // double speed = (-frontLeft.get() + -rearLeft.get() + -frontRight.get() +
+ // -rearRight
+ // .get()) / 2;
+ // left = ((Constants.DriveTrain.kADJUST - 1) * speed + left)
+ // / Constants.DriveTrain.kADJUST;
+ // robotDrive.arcadeDrive(left * k, right);
+ // }