Using SDS MK4i need help for auto command

Technical Programming
1

My team is using the SDS MK4i modules on our off-season Bot. Right now I’m trying to use “SwerveControllerCommand” everything should be working except for the Consumer.

Im using the Sds swerveLib “Mk4iSwerveModuleHelper”
and I need to use it in a static way

I truly have little experience with Swerve Drives, here is my code

1 Like
2

Simply providing code, while certainly helpful, isn’t enough for us to be able to help you. Can you explain what specifically you’re having issues with? Heres some additional questions to help guide further responses:

  1. What have you tried?
  2. What did it do?
  3. What did you expect it to do (that it didn’t do)?
  4. Do you have any errors?
5

I originally tried the WPILib Swerve example and based it on that, the original problems with that were the offsets and problems with drift in the CANCoders.

so I switched over to the SwerveLib by Sds, I was able to get the robot to drive properly and work to my standers during teleop. Now I’m trying to add in some trajectories and waypoints for auto

I haven’t been able to test the most recent iteration of my code due to hurricane Ian, when I did test with some older code it would zero my modules after a few seconds it would say the command finished.

the current error I can see is in RobotContainer with my test command with the “SwerveControllerCommand” there is an error with my “setModuleStates” & “getPose”

    public Command getAutonomousCommand() {
    //run in autonomous
    TrajectoryConfig trajectoryConfig = new TrajectoryConfig(
      Constants.AutoConstants.kMaxSpeedMetersPerSecond,
      Constants.AutoConstants.kMaxAccelerationMetersPerSecondSquared)
        .setKinematics(Constants.SwerveDrive.m_kinematics);
    Trajectory trajectory = TrajectoryGenerator.generateTrajectory(
      new Pose2d(0,0,new Rotation2d(0)),
      List.of(
        new Translation2d(1,0),
        new Translation2d(1,-1)
      ),
      new Pose2d(2,-1, Rotation2d.fromDegrees(180)), 
      trajectoryConfig
    );

    PIDController xController = new PIDController(Constants.AutoConstants.kPXController, 0, 0);
    PIDController yController = new PIDController(Constants.AutoConstants.kPYController, 0, 0);
    ProfiledPIDController thetaController = new ProfiledPIDController(
      Constants.AutoConstants.kPThetaController, 0, 0, Constants.AutoConstants.kThetaControllerConstraints);
    thetaController.enableContinuousInput(-Math.PI, Math.PI);

    SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand(
      trajectory,
      SwerveDriveTrain::getPose,
      Constants.SwerveDrive.m_kinematics,
      xController,
      yController,
      thetaController,
      SwerveDriveTrain::setModuleStates,
      s_swervedrivetrain
    );
    
    return SequentialCommandGroup(
      new InstantCommand(() -> s_swervedrivetrain.resetOdometry(trajectory.getInitialPose())),
      swerveControllerCommand,
      new InstantCommand(() -> s_swervedrivetrain.stopModules())
    );
  }

errors

    [{“resource”: “src/main/java/frc/robot/RobotContainer.java”,
“message”: “Cannot make a static reference to the non-static method getPose() from the type SwerveDriveTrain”,
    “LineNumber”: 154,
    },

    {“resource”: “src/main/java/frc/robot/RobotContainer.java”,
“message”: “Cannot make a static reference to the non-static method setModuleStates(SwerveModuleState[]) from the type SwerveDriveTrain”,
    “LineNumber”: 159,
    }]
6

It looks like what you need to do is change the SwerveDriveTrain::getPose to () -> drivetrainSubsystem.getPose(). drivetrainSubsystem should be replaced with the name of the instance of your drivetrain subsystem in whatever scope you’re making this command in. This is because the SwerveDriveTrain::getPose call must be static which I believe means it cannot interact with the state of an instance of the drivetrain class (Someone correct me if I’m wrong). What using the () -> or lambda syntax does is it wraps the getPose() method in an “anonymous” method which is passed to the SwerveControllerCommand and can interact with the state of an instance of SwerveDriveTrain
Edit: looks like something similar is happening with setModuleStates
Edit 2: Heres my teams code for this. It’s not using the standard wpilib SwerveControllerCommand but it should be similar.

new SwerveController(
path,
() → m_odometry.getPoseMeters(),
m_kinematics,
new PIDController(0.5, 0.0,0.0 ),
new PIDController(0.5, 0.0, 0.0), //was 0.0080395
new ProfiledPIDController(0.6, 0.0, 0.0, new Constraints(2, 2)), //was 0.003
(SwerveModuleState[] states) → {
m_chassisSpeeds = m_kinematics.toChassisSpeeds(states);
},
this
)

7

You’re completely right

I forgot I already defined the subsystem in the RobotContainer, so instead of SwerveDriveTrain::[insert], I needed the already defined s_swervedrivetrain::[insert]

8

passing functions around gets weird quickly, I’ve messed this up quite a few times before lol. If you need help with anything else feel free to dm me

9

thanks. I might take you up on that