I am having a problem while trying to rotate in a spot as shown above.

Everything else is working.

```
public void processInput(double x1, double y1, double x2, boolean deadStick, boolean driveCorrect) {
this._driveCorrect = driveCorrect;
SmartDashboard.putNumber("Forwrad", x1);
SmartDashboard.putNumber("Strafe", y1);
double A = x1 - x2;
double B = x1 + x2;
double C = y1 - x2;
double D = y1 + x2;
double speedFL = speed(B, C);
double speedBL = speed(A, C);
double speedFR = speed(B, D);
double speedBR = speed(A, D);
double lfOffset = lfSetAngle.getDouble(0.0);
double lbOffset = lbSetAngle.getDouble(0.0);
double rfOffset = rfSetAngle.getDouble(0.0);
double rbOffset = rbSetAngle.getDouble(0.0);
double angleFL = angle(B, C);// + Constants.FL_STEER_OFFSET;//+ lfOffset;
double angleBL = angle(A, C);// + Constants.BL_STEER_OFFSET;// + lbOffset;
double angleFR = angle(B, D);// + Constants.FR_STEER_OFFSET;// + rfOffset;
double angleBR = angle(A, D);// + Constants.BR_STEER_OFFSET;// + rbOffset;
double maxSpeed = Collections.max(Arrays.asList(speedFL, speedBL, speedFR, speedBR, 1.0));
SmartDashboard.putNumber("angleLF", angleFL);
SmartDashboard.putNumber("speedLF", speedFL);
SmartDashboard.putNumber("CurAngle FL", swerve1.getSteerEncDeg());
if (deadStick) {
swerve1.setDriveSpeed(0);
swerve2.setDriveSpeed(0);
swerve3.setDriveSpeed(0);
swerve4.setDriveSpeed(0);
} else {
swerve1.setSwerve(angleFL, speedFL / maxSpeed, this._driveCorrect);
swerve2.setSwerve(angleBL, speedBL / maxSpeed, this._driveCorrect);
swerve3.setSwerve(angleFR, speedFR / maxSpeed, this._driveCorrect);
swerve4.setSwerve(angleBR, speedBR / maxSpeed, this._driveCorrect);
}
}
private double speed(double val1, double val2) {
return Math.sqrt ((val1 * val1) + (val2 * val2));
}
private double angle(double val1, double val2) {
return (Math.atan2(val1, val2)/Math.PI + 1)/2;
}
```