wpilib example swerve

src/main/java/frc4388/robot/Robot.java

@@ -11,11 +11,15 @@ import com.ctre.phoenix.motorcontrol.ControlMode;
 import com.ctre.phoenix.motorcontrol.can.TalonSRX;
 import com.ctre.phoenix.sensors.WPI_Pigeon2;
 
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.robot.commands.AutoBalance;
 import frc4388.utility.RobotGyro;
 import frc4388.utility.RobotTime;
@@ -60,6 +64,10 @@ public class Robot extends TimedRobot {
     }
   }
 
+  private final SlewRateLimiter xLimiter = new SlewRateLimiter(3);
+  private final SlewRateLimiter yLimiter = new SlewRateLimiter(3);
+  private final SlewRateLimiter rotLimiter = new SlewRateLimiter(3);
+
   // private MicroBot bot = new MicroBot();
 
   /**
@@ -157,6 +165,15 @@ public class Robot extends TimedRobot {
     // SmartDashboard.putNumber("yaw", m_robotContainer.gyroRef.getAngle());
     // SmartDashboard.putNumber("pitch", m_robotContainer.gyroRef.getPitch());
     // SmartDashboard.putNumber("roll", m_robotContainer.gyroRef.getRoll());
+    this.drive(false);
+  }
+
+  private void drive(boolean fieldRelative) {
+    final double xSpeed = -xLimiter.calculate(MathUtil.applyDeadband(m_robotContainer.getDriverController().getLeftYAxis(), 0.02) * Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
+    final double ySpeed = -yLimiter.calculate(MathUtil.applyDeadband(m_robotContainer.getDriverController().getLeftXAxis(), 0.02) * Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
+    final double rot = -rotLimiter.calculate(MathUtil.applyDeadband(m_robotContainer.getDriverController().getRightXAxis(), 0.02) * Units.feetToMeters(Math.PI));
+
+    m_robotContainer.m_robotSwerveDrive.drive(xSpeed, ySpeed, rot, fieldRelative);
   }
 
   /**

src/main/java/frc4388/robot/RobotContainer.java

@@ -32,7 +32,7 @@ public class RobotContainer {
     private final RobotMap m_robotMap = new RobotMap();
 
     /* Subsystems */
-    private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack);//, m_robotMap.gyro);
+    public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack);//, m_robotMap.gyro);
     // private final LED m_robotLED = new LED(m_robotMap.LEDController);
     
 
@@ -65,16 +65,18 @@ public class RobotContainer {
         //                                                            0, false), m_robotSwerveDrive)
         // );
 
-        m_robotSwerveDrive.setDefaultCommand(
+    //     m_robotSwerveDrive.setDefaultCommand(
-            new RunCommand(() -> m_robotSwerveDrive.driveWithInput(
+    //         new RunCommand(() -> m_robotSwerveDrive.driveWithInput(
-                -0.3 * getDriverController().getLeftXAxis(),
+    //             -0.3 * getDriverController().getLeftXAxis(),
-                0.3 * getDriverController().getLeftYAxis(),
+    //             0.3 * getDriverController().getLeftYAxis(),
-                0.3 * getDriverController().getRightXAxis(),
+    //             0.3 * getDriverController().getRightXAxis(),
-                0.3 * getDriverController().getRightYAxis(),
+    //             0.3 * getDriverController().getRightYAxis(),
-                true),
+    //             true),
-                m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand"));
+    //             m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand"));
     }
 
+    
+
     /**
      * Use this method to define your button->command mappings. Buttons can be
      * created by instantiating a {@link GenericHID} or one of its subclasses

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -4,6 +4,7 @@
 
 package frc4388.robot.subsystems;
 
+import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -28,7 +29,9 @@ public class SwerveDrive extends SubsystemBase {
 
   private SwerveModule[] modules;
 
-  // private RobotGyro gyro;
+ 
+
+  // private RobotGyro gyro3
 
   private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
   private Translation2d rightFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
@@ -98,6 +101,16 @@ public class SwerveDrive extends SubsystemBase {
     setModuleStates(states);
   }
 
+  // ! experimental WPILib swerve drive example
+  public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
+    // SwerveModuleState[] states = kinematics.toSwerveModuleStates(
+    //   fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, gyro.getRotation2d()) 
+    //                 : new ChassisSpeeds(xSpeed, ySpeed, rot)
+    // );
+    SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(xSpeed, ySpeed, rot * SwerveDriveConstants.ROTATION_SPEED));
+    setModuleStates(states);
+  }
+
   /**
    * Set each module of the swerve drive to the corresponding desired state.
    * @param desiredStates Array of module states to set.