diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 2598bfa..31a45f5 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/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); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 60379ea..3180343 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/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( - new RunCommand(() -> m_robotSwerveDrive.driveWithInput( - -0.3 * getDriverController().getLeftXAxis(), - 0.3 * getDriverController().getLeftYAxis(), - 0.3 * getDriverController().getRightXAxis(), - 0.3 * getDriverController().getRightYAxis(), - true), - m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); + // m_robotSwerveDrive.setDefaultCommand( + // new RunCommand(() -> m_robotSwerveDrive.driveWithInput( + // -0.3 * getDriverController().getLeftXAxis(), + // 0.3 * getDriverController().getLeftYAxis(), + // 0.3 * getDriverController().getRightXAxis(), + // 0.3 * getDriverController().getRightYAxis(), + // true), + // 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 diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index b066f9c..f35fa57 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/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.