Swerve drive help

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) {


        } 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;
1 Like

can you post the code where you declare the swerve modules?

have you double-checked that the motor variables in code and on the robot correspond correctly to each corner? it looks like the back left and back right may have been switched. but more info is required to verify this.


There was a thread about a week ago that had a very similar issue.

Here is another thread with a similar issue (not the one I was thinking about from a week ago)

I’ll take this one step further. Can you post a link to your code in github (or similar)? Its exceptionally difficult to troubleshoot code without the complete project

Didn’t realize that :slight_smile:

Now that I have it fixed, turning in a spot is slow. How can I fix it?

Can you please post a link to your code? It will be easier to troubleshoot with you.

I would also recommend looking at the swerve control options in WPILib

I solved it by just adding a multiplier to x2

Turning is just slow with my code

The maximum speed allowed for translation and maximum speed for rotation and how the sum of translation and rotation is proportioned to the maximum speed of the motors is arbitrary. Pick what works for you.

And that ratio of allocation can be changed dynamically depending on the speed of one or the other if you want varying performance curves such as if I’m translating fast don’t allow fast rotations.

You can see that if translation and rotation both have numbers (from the joysticks) around one then they are treated roughly equally.