diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 78c6890..8fc1ca3 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -7,21 +7,9 @@ package frc4388.robot; -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; /** @@ -37,34 +25,6 @@ public class Robot extends TimedRobot { private RobotTime m_robotTime = RobotTime.getInstance(); private RobotContainer m_robotContainer; - public static class MicroBot extends SubsystemBase { - public WPI_Pigeon2 pigeon = new WPI_Pigeon2(14); - public RobotGyro gyro = new RobotGyro(pigeon); - - public TalonSRX frontLeft = new TalonSRX(2); - public TalonSRX backLeft = new TalonSRX(3); - public TalonSRX backRight = new TalonSRX(5); - public TalonSRX frontRight = new TalonSRX(4); - - public MicroBot() { - frontRight.configFactoryDefault(); - frontLeft.configFactoryDefault(); - backLeft.configFactoryDefault(); - backRight.configFactoryDefault(); - - frontLeft.setInverted(true); - backLeft.setInverted(true); - } - - public void setOutput(double output) { - frontRight.set(ControlMode.PercentOutput, output); - frontLeft.set(ControlMode.PercentOutput, output); - backLeft.set(ControlMode.PercentOutput, output); - backRight.set(ControlMode.PercentOutput, output); - } - } - - // private MicroBot bot = new MicroBot(); /** * This function is run when the robot is first started up and should be @@ -75,8 +35,6 @@ public class Robot extends TimedRobot { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); - - // bot.setDefaultCommand(new AutoBalance(bot)); } /** @@ -118,13 +76,6 @@ public class Robot extends TimedRobot { public void autonomousInit() { m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - /* - * String autoSelected = SmartDashboard.getString("Auto Selector", - * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand - * = new MyAutoCommand(); break; case "Default Auto": default: - * autonomousCommand = new ExampleCommand(); break; } - */ - // schedule the autonomous command (example) if (m_autonomousCommand != null) { m_autonomousCommand.schedule(); @@ -149,8 +100,6 @@ public class Robot extends TimedRobot { m_autonomousCommand.cancel(); } m_robotTime.startMatchTime(); - - // m_robotContainer.gyroRef.reset(); } /** @@ -158,9 +107,6 @@ public class Robot extends TimedRobot { */ @Override public void teleopPeriodic() { - // SmartDashboard.putNumber("yaw", m_robotContainer.m_robotMap.gyro.getAngle()); - SmartDashboard.putNumber("Robot.java Pitch", m_robotContainer.m_robotMap.gyro.getPitch()); - // SmartDashboard.putNumber("roll", m_robotContainer.m_robotMap.gyro.getRoll()); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 2a2e90c..031cd46 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,23 +7,14 @@ package frc4388.robot; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.math.trajectory.TrajectoryConfig; -import edu.wpi.first.math.trajectory.TrajectoryGenerator; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.commands.AutoBalance; import frc4388.robot.commands.DriveWithInput; -import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.SwerveDrive; -import frc4388.utility.LEDPatterns; -import frc4388.utility.RobotGyro; import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; @@ -47,45 +38,22 @@ public class RobotContainer { private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); - // public RobotGyro gyroRef = m_robotMap.gyro; - /** * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { configureButtonBindings(); + /* Default Commands */ + m_robotSwerveDrive.setDefaultCommand(new DriveWithInput(m_robotSwerveDrive, () -> getDriverController().getLeftXAxis(), () -> getDriverController().getLeftYAxis(), () -> getDriverController().getRightXAxis(), false)); - - /* Default Commands */ - // drives the robot with a two-axis input from the driver controller - // continually sends updates to the Blinkin LED controller to keep the lights on + // m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); - // m_robotSwerveDrive.setDefaultCommand( - // new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-0.3*getDriverController().getLeftXAxis(), - // 0.3*getDriverController().getLeftYAxis(), - // -0.3*getDriverController().getRightXAxis(), false), m_robotSwerveDrive) - // ); - - // m_robotSwerveDrive.setDefaultCommand( - // new RunCommand(() -> m_robotSwerveDrive.driveWithInput(0, - // -0.1, - // 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")); } @@ -104,23 +72,11 @@ public class RobotContainer { // .onFalse() /* Operator Buttons */ - // activates "Lit Mode" - // new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - // .whileTrue(new RunCommand(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW), m_robotLED)) - // .whileFalse(new RunCommand(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN), m_robotLED)); - // new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) - // .whileTrue(new RunCommand(() -> m_robotSwerveDrive.rotateCANCodersToAngle(0), m_robotSwerveDrive)); - - // new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetCANCoders(0), m_robotSwerveDrive)); - - // new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) - // .onTrue() new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); - //New interupt button + // interrupt button new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) .onTrue(new InstantCommand()); } @@ -131,11 +87,6 @@ public class RobotContainer { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - // no auto - // TrajectoryConfig trajConfig = new TrajectoryConfig(1, 1).setKinematics(m_robotSwerveDrive.getKinematics()); // TODO: make these AutoConstants - - // Trajectory traj = TrajectoryGenerator.generateTrajectory( - // new Pose2d(0, 0, new Rotation2d(0)), null, null, trajConfig); return new InstantCommand(); } diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index c33a95f..93a114d 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -11,11 +11,8 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.WPI_Pigeon2; -import edu.wpi.first.wpilibj.motorcontrol.Spark; -import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.subsystems.SwerveModule; -import frc4388.utility.RobotEncoder; import frc4388.utility.RobotGyro; /** @@ -49,22 +46,18 @@ public class RobotMap { public final WPI_TalonFX leftFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID); public final WPI_TalonFX leftFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID); public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID); - //public final RobotEncoder leftFrontEncoder = new RobotEncoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID, 268.900); public final WPI_TalonFX rightFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID); public final WPI_TalonFX rightFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID); public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID); - //public final RobotEncoder rightFrontEncoder = new RobotEncoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID, 266.700); public final WPI_TalonFX leftBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID); public final WPI_TalonFX leftBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID); public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID); - //public final RobotEncoder leftBackEncoder = new RobotEncoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID, 268.285); public final WPI_TalonFX rightBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID); public final WPI_TalonFX rightBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID); public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID); - //public final RobotEncoder rightBackEncoder = new RobotEncoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID, 86.965); void configureDriveMotors() { @@ -120,21 +113,10 @@ public class RobotMap { rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - // config magnet offset - // leftFrontEncoder.configMagnetOffset(90.0); //180.0); //270);//271.58203125);//SwerveDriveConstants.Configurations.LEFT_FRONT_ENCODER_OFFSET); - // rightFrontEncoder.configMagnetOffset(0.0); //180.0); //270);//271.58203125);//SwerveDriveConstants.Configurations.RIGHT_FRONT_ENCODER_OFFSET); - // leftBackEncoder.configMagnetOffset(23.99414); //0.0); //90.0);//92.98828125);//SwerveDriveConstants.Configurations.LEFT_BACK_ENCODER_OFFSET); - // rightBackEncoder.configMagnetOffset(225.0); //180.0); //270.0);//267.01171875);//SwerveDriveConstants.Configurations.RIGHT_BACK_ENCODER_OFFSET); - // initialize SwerveModules this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, -181.230469); this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, -270.615234); this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, -240.029297); this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, -40.869142); - - // LEFT FRONT: -181.230469 - // RIGHT FRONT: -270.615234 - // LEFT BACK: -240.029297 - // RIGHT BACK: -40.869142 } } diff --git a/src/main/java/frc4388/robot/commands/AutoBalance.java b/src/main/java/frc4388/robot/commands/AutoBalance.java index aeb4610..374de0a 100644 --- a/src/main/java/frc4388/robot/commands/AutoBalance.java +++ b/src/main/java/frc4388/robot/commands/AutoBalance.java @@ -6,13 +6,9 @@ package frc4388.robot.commands; import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc4388.robot.Robot; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.RobotGyro; -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html public class AutoBalance extends PelvicInflammatoryDisease { RobotGyro gyro; SwerveDrive drive; diff --git a/src/main/java/frc4388/robot/commands/DriveWithInput.java b/src/main/java/frc4388/robot/commands/DriveWithInput.java index 1fcaccc..3b4d638 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithInput.java +++ b/src/main/java/frc4388/robot/commands/DriveWithInput.java @@ -4,19 +4,17 @@ package frc4388.robot.commands; -import java.util.function.DoubleSupplier; import java.util.function.Supplier; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.subsystems.SwerveDrive; public class DriveWithInput extends CommandBase { - /** Creates a new DriveWithInput. */ + private final SwerveDrive swerve; private final Supplier xSpeed; @@ -26,9 +24,7 @@ public class DriveWithInput extends CommandBase { private final SlewRateLimiter xLimiter, yLimiter, rotLimiter; - - - + /** Creates a new DriveWithInput. */ public DriveWithInput(SwerveDrive swerve, Supplier xSpeed, Supplier ySpeed, Supplier rot, boolean fieldRelative) { // Use addRequirements() here to declare subsystem dependencies. this.swerve = swerve; diff --git a/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java b/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java index 1ab08d5..fb6a871 100644 --- a/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java +++ b/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java @@ -11,7 +11,7 @@ public abstract class PelvicInflammatoryDisease extends CommandBase { protected Gains gains; private double output = 0; - /** Creates a new PelvicInflamitoryDisease. */ + /** Creates a new PelvicInflammatoryDisease. */ public PelvicInflammatoryDisease(double kp, double ki, double kd, double kf) { gains = new Gains(kp, ki, kd, kf, 0); } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index b1ca512..d93f892 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -4,21 +4,13 @@ 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; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveDriveOdometry; -import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.SwerveDriveConstants; -import frc4388.utility.RobotGyro; public class SwerveDrive extends SubsystemBase { @@ -29,8 +21,6 @@ public class SwerveDrive extends SubsystemBase { private SwerveModule[] modules; - // 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)); private Translation2d leftBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); @@ -61,45 +51,7 @@ public class SwerveDrive extends SubsystemBase { this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack}; } - // public void driveWithInput(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { - - // Translation2d speed = new Translation2d(-xSpeed, ySpeed); - // double mag = speed.getNorm(); - // speed = speed.times(mag * speedAdjust); - - // double xSpeedMetersPerSecond = -speed.getX(); //SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; - // double ySpeedMetersPerSecond = speed.getY(); //SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; - // // SwerveModuleState[] states = kinematics.toSwerveModuleStates( - // // fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot, gyro.getRotation2d()) - // // : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot) - // //); - - // SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED)); - - // setModuleStates(states); - // } - - // public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative) { - // // ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0; - // Translation2d speed = new Translation2d(-leftX, leftY); - // speed = speed.times(speed.getNorm() * speedAdjust); - // // if (Math.abs(rightX) > SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND || Math.abs(rightY) > SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND) - // // rotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0, 1)); - // // double rot = rotTarget.minus(gyro.getRotation2d()).getRadians(); - // double xSpeedMetersPerSecond = -speed.getX(); - // double ySpeedMetersPerSecond = speed.getY(); - // // chassisSpeeds = fieldRelative - // // ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, - // // rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d()) - // // : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED); - - // ChassisSpeeds chassisSpeeds = new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED); - - // SwerveModuleState[] states = kinematics.toSwerveModuleStates(chassisSpeeds); - // setModuleStates(states); - // } - - // ! experimental WPILib swerve drive example + // 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()) @@ -179,32 +131,6 @@ public class SwerveDrive extends SubsystemBase { // ); // } - public void runAllDriveMotors(double output) { - this.leftFront.driveMotor.set(output); - this.rightFront.driveMotor.set(output); - this.leftBack.driveMotor.set(output); - this.rightBack.driveMotor.set(output); - } - - public void runAllSteerMotors(double output) { - this.leftFront.angleMotor.set(output); - this.rightFront.angleMotor.set(output); - this.leftBack.angleMotor.set(output); - this.rightBack.angleMotor.set(output); - } - - public void rotateCANCodersToAngle(double angle) { - for (SwerveModule module : this.modules) { - module.rotateToAngle(angle); - } - } - - public void resetCANCoders(double position) { - for (SwerveModule module : this.modules) { - module.reset(position); - } - } - public SwerveDriveKinematics getKinematics() { return this.kinematics; } @@ -213,10 +139,6 @@ public class SwerveDrive extends SubsystemBase { public void periodic() { // This method will be called once per scheduler run // updateOdometry(); - SmartDashboard.putNumber("LeftFront CC", this.modules[0].getAngle().getDegrees()); - SmartDashboard.putNumber("RightFront CC", this.modules[1].getAngle().getDegrees()); - SmartDashboard.putNumber("LeftBack CC", this.modules[2].getAngle().getDegrees()); - SmartDashboard.putNumber("RightBack CC", this.modules[3].getAngle().getDegrees()); } /** diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index e8dd497..4bbbefb 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -10,22 +10,18 @@ import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; -import com.ctre.phoenix.sensors.CANCoderConfiguration; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.motorcontrol.Talon; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.Gains; -import frc4388.utility.RobotEncoder; public class SwerveModule extends SubsystemBase { public WPI_TalonFX driveMotor; public WPI_TalonFX angleMotor; - // private CANCoder canCoder; private CANCoder encoder; public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS; @@ -47,14 +43,7 @@ public class SwerveModule extends SubsystemBase { angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; angleMotor.configAllSettings(angleConfig); - // CANCoderConfiguration canCoderConfig = new CANCoderConfiguration(); - // canCoderConfig.magnetOffsetDegrees = offset; - // encoder.configAllSettings(canCoderConfig); encoder.configMagnetOffset(offset); - - // driveMotor.config_kP(0, 0.4); - - // canCoderConfig.magnetOffsetDegrees = 270; } /** @@ -86,7 +75,7 @@ public class SwerveModule extends SubsystemBase { * @return the angle of a SwerveModule as a Rotation2d */ public Rotation2d getAngle() { - // Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees. + // * Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees. return Rotation2d.fromDegrees(encoder.getAbsolutePosition()); } @@ -138,16 +127,13 @@ public class SwerveModule extends SubsystemBase { angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks); double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); - double inchesPerSecond = Units.metersToInches(state.speedMetersPerSecond); driveMotor.set(-1 * angleMotor.get() + feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND); // driveMotor.set(TalonFXControlMode.Velocity, inchesPerSecond * SwerveDriveConstants.Conversions.TICKS_PER_INCH * SwerveDriveConstants.Conversions.SECONDS_TO_TICK_TIME); } public void reset(double position) { - // encoder.setPosition(position); encoder.setPositionToAbsolute(); - // canCoder.setPosition(1024); } public double getCurrent() { diff --git a/src/main/java/frc4388/utility/RobotEncoder.java b/src/main/java/frc4388/utility/RobotEncoder.java deleted file mode 100644 index 2e397ac..0000000 --- a/src/main/java/frc4388/utility/RobotEncoder.java +++ /dev/null @@ -1,23 +0,0 @@ -// 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 frc4388.utility; - -import com.ctre.phoenix.sensors.CANCoder; - -/** Add your docs here. */ -public class RobotEncoder extends CANCoder { - private double offset; - - public RobotEncoder(int id, double offset) { - super(id); - - this.offset = offset; - } - - @Override - public double getAbsolutePosition() { - return super.getAbsolutePosition() - this.offset; - } -}