We are using the SDS example code to run a Swerve Drivebase for FRC we are using SDS MK4i swerve modules and Neos for both our turn and drive motors using Cancoders as the turning encoders and a NavX over USB as our Gyro. However most of the code is out dated this being said most of the Code is fully working. Our issue is the wheels turn 45 instead of 90 when you move the joystick 90 also sometimes the wheels will go from 0-180 randomly and the same applies for 90-270 as I am still very new to programming I don't know a good process to debug so all the code is below:
DefaultDriveCommand.java:
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.subsystems.DrivetrainSubsystem;
import java.util.function.DoubleSupplier;
public class DefaultDriveCommand extends CommandBase { private final DrivetrainSubsystem m_drivetrainSubsystem; private final DoubleSupplier m_translationXSupplier; private final DoubleSupplier m_translationYSupplier; private final DoubleSupplier m_rotationSupplier; public DefaultDriveCommand(DrivetrainSubsystem drivetrain,DoubleSupplier translationXSupplier,DoubleSupplier translationYSupplier,DoubleSupplier rotationSupplier) { this.m_drivetrainSubsystem = drivetrain; this.m_translationXSupplier = translationXSupplier; this.m_translationYSupplier = translationYSupplier; this.m_rotationSupplier = rotationSupplier; addRequirements(drivetrain); } @Override public void execute() { // You can use `new ChassisSpeeds(...)` for robot-oriented movement instead of field-oriented movement m_drivetrainSubsystem.drive( ChassisSpeeds.fromFieldRelativeSpeeds( m_translationXSupplier.getAsDouble(), m_translationYSupplier.getAsDouble(), m_rotationSupplier.getAsDouble(), m_drivetrainSubsystem.getGyroscopeRotation() ) ); } @Override public void end(boolean interrupted) { m_drivetrainSubsystem.drive(new ChassisSpeeds(0.0, 0.0, 0.0)); }
}DrivetrainSubsystem.java:
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot.subsystems;
import com.kauailabs.navx.frc.AHRS;
import com.swervedrivespecialties.swervelib.Mk4SwerveModuleHelper;
import com.swervedrivespecialties.swervelib.SdsModuleConfigurations;
import com.swervedrivespecialties.swervelib.SwerveModule;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj.SerialPort.Port;
import static frc.robot.Constants.*;
public class DrivetrainSubsystem extends SubsystemBase { /** * The maximum voltage that will be delivered to the drive motors. * <p> * This can be reduced to cap the robot's maximum speed. Typically, this is useful during initial testing of the robot. */ public static final double MAX_VOLTAGE = 2.5; // FIXME Measure the drivetrain's maximum velocity or calculate the theoretical. // The formula for calculating the theoretical maximum velocity is: // <Motor free speed RPM> / 60 * <Drive reduction> * <Wheel diameter meters> * pi // By default this value is setup for a Mk3 standard module using Falcon500s to drive. // An example of this constant for a Mk4 L2 module with NEOs to drive is: // 5880.0 / 60.0 / SdsModuleConfigurations.MK4_L2.getDriveReduction() * SdsModuleConfigurations.MK4_L2.getWheelDiameter() * Math.PI /** * The maximum velocity of the robot in meters per second. * <p> * This is a measure of how fast the robot should be able to drive in a straight line. */ public static final double MAX_VELOCITY_METERS_PER_SECOND = 6380.0 / 60.0 * SdsModuleConfigurations.MK4I_L2.getDriveReduction() * SdsModuleConfigurations.MK4I_L2.getWheelDiameter() * Math.PI; /** * The maximum angular velocity of the robot in radians per second. * <p> * This is a measure of how fast the robot can rotate in place. */ // Here we calculate the theoretical maximum angular velocity. You can also replace this with a measured amount. public static final double MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND = MAX_VELOCITY_METERS_PER_SECOND / Math.hypot(DRIVETRAIN_TRACKWIDTH_METERS / 2.0, DRIVETRAIN_WHEELBASE_METERS / 2.0); private final SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics( // Front left new Translation2d(DRIVETRAIN_TRACKWIDTH_METERS / 2.0, DRIVETRAIN_WHEELBASE_METERS / 2.0), // Front right new Translation2d(DRIVETRAIN_TRACKWIDTH_METERS / 2.0, -DRIVETRAIN_WHEELBASE_METERS / 2.0), // Back left new Translation2d(-DRIVETRAIN_TRACKWIDTH_METERS / 2.0, DRIVETRAIN_WHEELBASE_METERS / 2.0), // Back right new Translation2d(-DRIVETRAIN_TRACKWIDTH_METERS / 2.0, -DRIVETRAIN_WHEELBASE_METERS / 2.0) ); // By default we use a Pigeon for our gyroscope. But if you use another gyroscope, like a NavX, you can change this. // The important thing about how you configure your gyroscope is that rotating the robot counter-clockwise should // cause the angle reading to increase until it wraps back over to zero. // FIXME Remove if you are using a Pigeon //private final PigeonIMU m_pigeon = new PigeonIMU(DRIVETRAIN_PIGEON_ID); // FIXME Uncomment if you are using a NavX private final AHRS m_navx = new AHRS(Port.kUSB2); // NavX connected over USB // These are our modules. We initialize them in the constructor. private final SwerveModule m_frontLeftModule; private final SwerveModule m_frontRightModule; private final SwerveModule m_backLeftModule; private final SwerveModule m_backRightModule; private ChassisSpeeds m_chassisSpeeds = new ChassisSpeeds(0.0, 0.0, 0.0); public DrivetrainSubsystem() { ShuffleboardTab tab = Shuffleboard.getTab("Drivetrain"); // There are 4 methods you can call to create your swerve modules. // The method you use depends on what motors you are using. // // Mk3SwerveModuleHelper.createFalcon500(...) // Your module has two Falcon 500s on it. One for steering and one for driving. // // Mk3SwerveModuleHelper.createNeo(...) // Your module has two NEOs on it. One for steering and one for driving. // // Mk3SwerveModuleHelper.createFalcon500Neo(...) // Your module has a Falcon 500 and a NEO on it. The Falcon 500 is for driving and the NEO is for steering. // // Mk3SwerveModuleHelper.createNeoFalcon500(...) // Your module has a NEO and a Falcon 500 on it. The NEO is for driving and the Falcon 500 is for steering. // // Similar helpers also exist for Mk4 modules using the Mk4SwerveModuleHelper class. // By default we will use Falcon 500s in standard configuration. But if you use a different configuration or motors // you MUST change it. If you do not, your code will crash on startup. // FIXME Setup motor configuration m_frontLeftModule = Mk4SwerveModuleHelper.createNeo( // This parameter is optional, but will allow you to see the current state of the module on the dashboard. tab.getLayout("Front Left Module", BuiltInLayouts.kList) .withSize(2, 4) .withPosition(0, 0), // This can either be STANDARD or FAST depending on your gear configuration Mk4SwerveModuleHelper.GearRatio.L2, // This is the ID of the drive motor FRONT_LEFT_MODULE_DRIVE_MOTOR, // This is the ID of the steer motor FRONT_LEFT_MODULE_STEER_MOTOR, // This is the ID of the steer encoder FRONT_LEFT_MODULE_STEER_ENCODER, // This is how much the steer encoder is offset from true zero (In our case, zero is facing straight forward) FRONT_LEFT_MODULE_STEER_OFFSET ); // We will do the same for the other modules m_frontRightModule = Mk4SwerveModuleHelper.createNeo( tab.getLayout("Front Right Module", BuiltInLayouts.kList) .withSize(2, 4) .withPosition(2, 0), Mk4SwerveModuleHelper.GearRatio.L2, FRONT_RIGHT_MODULE_DRIVE_MOTOR, FRONT_RIGHT_MODULE_STEER_MOTOR, FRONT_RIGHT_MODULE_STEER_ENCODER, FRONT_RIGHT_MODULE_STEER_OFFSET ); m_backLeftModule = Mk4SwerveModuleHelper.createNeo( tab.getLayout("Back Left Module", BuiltInLayouts.kList) .withSize(2, 4) .withPosition(4, 0), Mk4SwerveModuleHelper.GearRatio.L2, BACK_LEFT_MODULE_DRIVE_MOTOR, BACK_LEFT_MODULE_STEER_MOTOR, BACK_LEFT_MODULE_STEER_ENCODER, BACK_LEFT_MODULE_STEER_OFFSET ); m_backRightModule = Mk4SwerveModuleHelper.createNeo( tab.getLayout("Back Right Module", BuiltInLayouts.kList) .withSize(2, 4) .withPosition(6, 0), Mk4SwerveModuleHelper.GearRatio.L2, BACK_RIGHT_MODULE_DRIVE_MOTOR, BACK_RIGHT_MODULE_STEER_MOTOR, BACK_RIGHT_MODULE_STEER_ENCODER, BACK_RIGHT_MODULE_STEER_OFFSET ); } /** * Sets the gyroscope angle to zero. This can be used to set the direction the robot is currently facing to the * 'forwards' direction. */ public void zeroGyroscope() { // FIXME Remove if you are using a Pigeon //m_pigeon.setFusedHeading(0.0); // FIXME Uncomment if you are using a NavX m_navx.zeroYaw(); } public Rotation2d getGyroscopeRotation() { // FIXME Remove if you are using a Pigeon //return Rotation2d.fromDegrees(m_pigeon.getFusedHeading()); //FIXME Uncomment if you are using a NavX if (m_navx.isMagnetometerCalibrated()) { // We will only get valid fused headings if the magnetometer is calibrated return Rotation2d.fromDegrees(m_navx.getFusedHeading()); } // We have to invert the angle of the NavX so that rotating the robot counter-clockwise makes the angle increase. return Rotation2d.fromDegrees(360.0 - m_navx.getYaw()); } public void drive(ChassisSpeeds chassisSpeeds) { m_chassisSpeeds = chassisSpeeds; } @Override public void periodic() { 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()); }
}Constants.java
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot;
/** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean * constants. This class should not be used for any other purpose. All constants should be declared * globally (i.e. public static). Do not put anything functional in this class. * * <p>It is advised to statically import this class (or one of its inner classes) wherever the * constants are needed, to reduce verbosity. */
public final class Constants { /** * The left-to-right distance between the drivetrain wheels * * Should be measured from center to center. */ public static final double DRIVETRAIN_TRACKWIDTH_METERS = 0.47; // FIXME Measure and set trackwidth /** * The front-to-back distance between the drivetrain wheels. * * Should be measured from center to center. */ public static final double DRIVETRAIN_WHEELBASE_METERS = 0.47; // FIXME Measure and set wheelbase //public static final int DRIVETRAIN_PIGEON_ID = 0; // FIXME Set Pigeon ID public static final int FRONT_LEFT_MODULE_DRIVE_MOTOR = 5; // FIXME Set front left module drive motor ID public static final int FRONT_LEFT_MODULE_STEER_MOTOR = 6; // FIXME Set front left module steer motor ID public static final int FRONT_LEFT_MODULE_STEER_ENCODER = 12; // FIXME Set front left steer encoder ID public static final double FRONT_LEFT_MODULE_STEER_OFFSET = -Math.toRadians(260.42); // FIXME Measure and set front left steer offset public static final int FRONT_RIGHT_MODULE_DRIVE_MOTOR = 1; // FIXME Set front right drive motor ID public static final int FRONT_RIGHT_MODULE_STEER_MOTOR = 2; // FIXME Set front right steer motor ID public static final int FRONT_RIGHT_MODULE_STEER_ENCODER = 9; // FIXME Set front right steer encoder ID public static final double FRONT_RIGHT_MODULE_STEER_OFFSET = -Math.toRadians(315.00); // FIXME Measure and set front right steer offset public static final int BACK_LEFT_MODULE_DRIVE_MOTOR = 8; // FIXME Set back left drive motor ID public static final int BACK_LEFT_MODULE_STEER_MOTOR = 7; // FIXME Set back left steer motor ID public static final int BACK_LEFT_MODULE_STEER_ENCODER = 11; // FIXME Set back left steer encoder ID public static final double BACK_LEFT_MODULE_STEER_OFFSET = -Math.toRadians(192.83); // FIXME Measure and set back left steer offset public static final int BACK_RIGHT_MODULE_DRIVE_MOTOR = 4; // FIXME Set back right drive motor ID public static final int BACK_RIGHT_MODULE_STEER_MOTOR = 3; // FIXME Set back right steer motor ID public static final int BACK_RIGHT_MODULE_STEER_ENCODER = 10; // FIXME Set back right steer encoder ID public static final double BACK_RIGHT_MODULE_STEER_OFFSET = -Math.toRadians(246.71); // FIXME Measure and set back right steer offset
}Main.java
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot;
import edu.wpi.first.wpilibj.RobotBase;
/** * Do NOT add any static variables to this class, or any initialization at all. Unless you know what * you are doing, do not modify this file except to change the parameter class to the startRobot * call. */
public final class Main { private Main() {} /** * Main initialization function. Do not perform any initialization here. * * <p>If you change your main robot class, change the parameter type. */ public static void main(String... args) { RobotBase.startRobot(Robot::new); }
}Robot.java
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
/** * The VM is configured to automatically run this class, and to call the functions corresponding to * each mode, as described in the TimedRobot documentation. If you change the name of this class or * the package after creating this project, you must also update the build.gradle file in the * project. */
public class Robot extends TimedRobot { private Command m_autonomousCommand; private RobotContainer m_robotContainer; /** * This function is run when the robot is first started up and should be used for any * initialization code. */ @Override public void robotInit() { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); } /** * This function is called every robot packet, no matter the mode. Use this for items like * diagnostics that you want ran during disabled, autonomous, teleoperated and test. * * <p>This runs after the mode specific periodic functions, but before LiveWindow and * SmartDashboard integrated updating. */ @Override public void robotPeriodic() { // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled // commands, running already-scheduled commands, removing finished or interrupted commands, // and running subsystem periodic() methods. This must be called from the robot's periodic // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); } /** This function is called once each time the robot enters Disabled mode. */ @Override public void disabledInit() {} @Override public void disabledPeriodic() {} /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @Override public void autonomousInit() { m_autonomousCommand = m_robotContainer.getAutonomousCommand(); // schedule the autonomous command (example) if (m_autonomousCommand != null) { m_autonomousCommand.schedule(); } } /** This function is called periodically during autonomous. */ @Override public void autonomousPeriodic() {} @Override public void teleopInit() { // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove // this line or comment it out. if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } } /** This function is called periodically during operator control. */ @Override public void teleopPeriodic() {} @Override public void testInit() { // Cancels all running commands at the start of test mode. CommandScheduler.getInstance().cancelAll(); } /** This function is called periodically during test mode. */ @Override public void testPeriodic() {}
}RobotContainer.java
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.FunctionalCommand;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.button.Button;
import frc.robot.commands.DefaultDriveCommand;
import frc.robot.subsystems.DrivetrainSubsystem;
/** * This class is where the bulk of the robot should be declared. Since Command-based is a * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including * subsystems, commands, and button mappings) should be declared here. */
public class RobotContainer { // The robot's subsystems and commands are defined here... private final DrivetrainSubsystem m_drivetrainSubsystem = new DrivetrainSubsystem(); private final XboxController m_controller = new XboxController(0); /** * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { // Set up the default command for the drivetrain. // The controls are for field-oriented driving: // Left stick Y axis -> forward and backwards movement // Left stick X axis -> left and right movement // Right stick X axis -> rotation m_drivetrainSubsystem.setDefaultCommand(new DefaultDriveCommand( m_drivetrainSubsystem, () -> -modifyAxis(m_controller.getLeftY()) * DrivetrainSubsystem.MAX_VELOCITY_METERS_PER_SECOND, () -> -modifyAxis(m_controller.getLeftX()) * DrivetrainSubsystem.MAX_VELOCITY_METERS_PER_SECOND, () -> -modifyAxis(m_controller.getRightX()) * DrivetrainSubsystem.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND
)); // Configure the button bindings configureButtonBindings(); } /** * Use this method to define your button->command mappings. Buttons can be created by * instantiating a {@link GenericHID} or one of its subclasses ({@link * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { // Back button zeros the gyroscope new Button(m_controller::getBackButton) // No requirements because we don't need to interrupt anything .whenPressed(m_drivetrainSubsystem::zeroGyroscope); } /** * Use this to pass the autonomous command to the main {@link Robot} class. * * @return the command to run in autonomous */ public Command getAutonomousCommand() { // An ExampleCommand will run in autonomous return new InstantCommand(); } private static double deadband(double value, double deadband) { if (Math.abs(value) > deadband) { if (value > 0.0) { return (value - deadband) / (1.0 - deadband); } else { return (value + deadband) / (1.0 - deadband); } } else { return 0.0; } } private static double modifyAxis(double value) { // Deadband value = deadband(value, 0.3); // Square the axis value = Math.copySign(value * value, value); return value; }
}
