Team4388/2023WayOfTheRobot
1
0
Fork 0
mirror of https://github.com/Team4388/2023WayOfTheRobot.git synced 2026-06-09 00:37:59 -06:00
Code Issues Packages Projects Releases Wiki Activity

cleanup before master merge

Browse Source
This commit is contained in:
aarav18
2023-02-02 20:54:12 -07:00
parent f9b89de6c5
commit 784fd3f3b6
9 changed files with 9 additions and 253 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Robot.java
-54
View File
@@ -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());
}
/**
src/main/java/frc4388/robot/RobotContainer.java
+4 -53
View File
@@ -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();
}
src/main/java/frc4388/robot/RobotMap.java
-18
View File
@@ -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
}
}
src/main/java/frc4388/robot/commands/AutoBalance.java
-4
View File
@@ -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;
src/main/java/frc4388/robot/commands/DriveWithInput.java
+2 -6
View File
@@ -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<Double> 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<Double> xSpeed, Supplier<Double> ySpeed, Supplier<Double> rot, boolean fieldRelative) {
// Use addRequirements() here to declare subsystem dependencies.
this.swerve = swerve;
src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java
+1 -1
View File
@@ -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);
}
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+1 -79
View File
@@ -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());
}
/**
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+1 -15
View File
@@ -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() {