Struggling To Make A Swerve Drive Turn In Place

Hello from one of Team 1056’s new programmers.

This year our team is using a swerve drive. With the new Command-based framework, we managed to “borrow” some code from the Internet. Teleoperation works perfectly fine, and my autonomous “drive to location” function works too.

I, for the life of me, cannot get the swerve drive to spin with an autonomous function.

Instead of the robot rotating in place, one of two things happened:

  • The wheels turned but didn’t spin forwards/backwards, ending in a series of uncontrolled steer motor outputs, similar to a seizure. Or, more recently
  • No turning at all.

Here’s my function:

  public void pose_to(Pose2d targetPos, double targetRot){
	//while the robot isn't in the correct position
        while(getPose().equals(targetPos) != true){
                m_auto = true;
                double[] error = { (targetPos.getX() - getPose().getX()), (targetPos.getY() - getPose().getY()), };
		//get the error in X- and Y-coordinates and adjust it for the modules
                for(double i: error){
                        if(Math.abs(i) < PID_TOLERANCE){ i = 0; }
                        if(i > 1){ i = 1; }
                        if(i < -1){ i = -1; }
                }
                m_chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(error[0], error[1], 0, getGyroscopeRotation());
		//drive and update the motors
                drive(m_chassisSpeeds);
                periodic();
        }
	//while the angles are off
        while(getGyroscopeRotation().getDegrees() != targetRot){
		//get and adjust the error
                double error2 = targetRot - getGyroscopeRotation().getDegrees();
                if(Math.abs(error2) < PID_TOLERANCE){ error2 = 0; }
                if(error2 > 1){ error2 = 1; }
                if(error2 < -1){ error2 = -1; }

		//update the swerve module states, bypassing the subsystem's own periodic()
                m_chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(0, 0, error2, getGyroscopeRotation());
                SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(m_chassisSpeeds);
                SwerveDriveKinematics.desaturateWheelSpeeds(states, MAX_VELOCITY_METERS_PER_SECOND);
                m_frontLeftModule.set(states[0].speedMetersPerSecond / MAX_VELOCITY_METERS_PER_SECOND * MAX_VOLTAGE, states[0].angle.getRadians());
                m_frontRightModule.set(states[1].speedMetersPerSecond / MAX_VELOCITY_METERS_PER_SECOND * MAX_VOLTAGE, states[1].angle.getRadians());
                m_backLeftModule.set(states[2].speedMetersPerSecond / MAX_VELOCITY_METERS_PER_SECOND * MAX_VOLTAGE, states[2].angle.getRadians());
                m_backRightModule.set(states[3].speedMetersPerSecond / MAX_VELOCITY_METERS_PER_SECOND * MAX_VOLTAGE, states[3].angle.getRadians());
        }
        m_auto = false;
  }

Notes:

  • Don’t mind m_auto, it doesn’t do anything meaningful within the function.
  • We’re currently using MK3 Swerve Modules from SwerveDriveSpecialties along with its accompanying library.
  • The function used to be split in two, a “drive to location” function and “spin to angle” function. I combined them out of desperation to get the turning working, but only the driving bit works.
  • The large block of code starting with “SwerveModuleState[]…” and stopping before the ending curly bracket is taken directly from the drivetrain’s periodic function. It updates the motors, and it does that perfectly well in teleoperation. I have also tried using the periodic function itself, but to no avail. .set() is the only function that controls module steering and driving.
  • I have tried using a dedicated PID instance, but my function has failed with that and with a custom block of imitation code.

Never run a blocking while loop in the main robot thread. You should look at some of the more basic WPILib examples to get a feel for how code flow is expected to occur in a command-based robot program.

3 Likes