Swerve drive help

[AaronHuang]

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

[bruingineer]

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.

[Skyehawk]

@bruingineer

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.

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)

Weird Swerve Allignment Problem

My team’s been building a swervedrive during the offseason and we’re having trouble figuring out the wheel alignment. We’re able to drive the robot in the robot oriented way perfectly (meaning no on-place rotation, just movement up-down-left-right). The wheels align when we do that. When we try to make the robot rotate in place, two of the wheels go in one direction, and the other two go in the other. My code is similar to this one: What can I do to make both swivelling and normal driving wor…

[Fletch1373]

@bruingineer

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

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

[AaronHuang]

Didn’t realize that

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

[bruingineer]

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

[AaronHuang]

I solved it by just adding a multiplier to x2

Turning is just slow with my code

[SLAB-Mr.Thomas]

@AaronHuang

multiplier to x2

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.