diff --git a/src/main/java/frc/robot/ITeamTalon.java b/src/main/java/frc/robot/ITeamTalon.java index 050ba142..59dbfb2c 100644 --- a/src/main/java/frc/robot/ITeamTalon.java +++ b/src/main/java/frc/robot/ITeamTalon.java @@ -151,6 +151,10 @@ public default void configureWithPidParameters(PidParameters pidParameters, int public ErrorCode configPeakOutputReverse(double percentOut); + public ErrorCode configFactoryDefault(int timeoutMs); + + public ErrorCode configFactoryDefault(); + // Public wrapper for protected method (which aren't allowed in interfaces) public ErrorCode configBaseAllSettings(BaseTalonConfiguration allConfigs); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0a5d0ed4..4d446250 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -62,13 +62,13 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... - private final DriveTrainSubsystem driveTrain = DriveTrainSubsystem.Create(); + private final NavigationSubsystem navigation = NavigationSubsystem.Create(); + private final DriveTrainSubsystem driveTrain = DriveTrainSubsystem.Create(navigation); private final IntakeSubsystem intake = IntakeSubsystem.Create(); private final ShooterSubsystem shooter = ShooterSubsystem.Create(); private final VerticalIndexerSubsystem verticalIndexer = VerticalIndexerSubsystem.Create(); private final TurretSubsystem turret = TurretSubsystem.Create(); private final HorizontalIndexerSubsystem horizontalIndexer = HorizontalIndexerSubsystem.Create(); - private final NavigationSubsystem navigation = NavigationSubsystem.Create(); private final ClimberSubsystem climber = ClimberSubsystem.Create(); public static final Joystick driveController = new Joystick(0); diff --git a/src/main/java/frc/robot/commands/drivingCommands/DriveEncoders.java b/src/main/java/frc/robot/commands/drivingCommands/DriveEncoders.java index 3ddf437f..ec8258ec 100644 --- a/src/main/java/frc/robot/commands/drivingCommands/DriveEncoders.java +++ b/src/main/java/frc/robot/commands/drivingCommands/DriveEncoders.java @@ -67,7 +67,7 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { // if - // (Utils.checkTolerance((Math.abs(drive.getRightEncoder()-initialRightEncoderValue))*Constants.kEncoderDistancePerPulse, Math.abs(userMeters), Constants.driveEpsilon*Constants.kEncoderDistancePerPulse) && Utils.checkTolerance((Math.abs(drive.getLeftEncoder()-initialLeftEncoderValue))*Constants.kEncoderDistancePerPulse, Math.abs(userMeters), Constants.kEncoderDistancePerPulse*Constants.driveEpsilon)){ + // (Utils.checkTolerance((Math.abs(drive.getRightEncoder()-initialRightEncoderValue))*Constants.kEncoderDistancePerPulse, Math.abs(userMeters), Constants.driveEpsilon*Constants.kEncoderDistancePerPulse) && Utils.checkTolerance((Math.abs(drive.getLeftEncoder()-initialLeftEncoderValue))*Constants.kEncoderDistancePerPulse, Math.abs(userMeters), Constants.kEncoderDistancePerPulse * DriveTrainConstants.kDriveEpsilon)){ if ((Math.abs(drive.getRightEncoder() - initialRightEncoderValue) * DriveTrainConstants.kEncoderDistancePerPulse) >= Math.abs(userMeters) diff --git a/src/main/java/frc/robot/constants/DriveTrainConstants.java b/src/main/java/frc/robot/constants/DriveTrainConstants.java index d5ff6cd7..07fc4ed6 100644 --- a/src/main/java/frc/robot/constants/DriveTrainConstants.java +++ b/src/main/java/frc/robot/constants/DriveTrainConstants.java @@ -1,13 +1,43 @@ package frc.robot.constants; +import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.wpilibj.system.LinearSystem; +import edu.wpi.first.wpilibj.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint; +import edu.wpi.first.wpilibj.util.Units; +import edu.wpi.first.wpiutil.math.numbers.N2; + public class DriveTrainConstants { - // Our gear ratio. TBD + // Following are made up. Update to reflect reality public static final double kDriveGearRatio = 10.71; + public static final double kWheelRadiusMeters = 0.0762; + public static final double kTrackWidthMeters = Units.inchesToMeters(19.5); + + public static final int kEncoderTicksPerRotation = 2048; + public static final double kWheelCircumferenceMeters = 2 * Math.PI * kWheelRadiusMeters; // Distance per encoder pulse public static final double kEncoderDistancePerPulse = - (0.1524 * Math.PI) / (2048 * kDriveGearRatio); + (2 * kWheelRadiusMeters * Math.PI) / (kEncoderTicksPerRotation * kDriveGearRatio); // Epsilon for DriveEncoders Command (in ticks) - public static final double driveEpsilon = 50; + public static final double kDriveEpsilon = 50; + + public static final double kS = 0.22; // Static Volts + public static final double kVLinear = 1.98; // Linear Velocity Volt Seconds per Meter + public static final double KALinear = 0.2; // Linear Acceleration Volt Seconds Squared per Meter + public static final double kVAngular = 1.5; // Angular Velocity Volt Seconds per Meter + public static final double kAAngular = 0.3; // Angular Acceleration Volt Seconds Squared per Meter + public static final DifferentialDriveKinematics kKinematics = + new DifferentialDriveKinematics(kTrackWidthMeters); + public static final DifferentialDriveVoltageConstraint kVoltageConstraint = + new DifferentialDriveVoltageConstraint( + new SimpleMotorFeedforward(kS, kVLinear, KALinear), + kKinematics, + 10); // 10V max to account for battery sag + public static final LinearSystem kPlant = + LinearSystemId.identifyDrivetrainSystem(kVLinear, KALinear, kVAngular, kAAngular); + public static final double kMaxSpeedMetersPerSecond = 3; // Tune + public static final double kMaxAccelerationMetersPerSecondSquared = 3; } diff --git a/src/main/java/frc/robot/subsystems/DriveTrainSubsystem.java b/src/main/java/frc/robot/subsystems/DriveTrainSubsystem.java index aa86b762..45874241 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveTrainSubsystem.java @@ -12,30 +12,57 @@ import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.TalonSRXSimCollection; -import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.hal.simulation.SimDeviceDataJNI; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.SlewRateLimiter; +import edu.wpi.first.wpilibj.system.plant.DCMotor; import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpiutil.math.MathUtil; +import edu.wpi.first.wpiutil.math.VecBuilder; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.DriveTrainConstants; import frc.robot.constants.Ports; import frc.robot.ITeamTalon; import frc.robot.TeamTalonFX; +import frc.robot.TeamTalonSRX; public class DriveTrainSubsystem extends SubsystemBase { - final DifferentialDrive drive; - - ITeamTalon rightDriveFalconMain; - ITeamTalon leftDriveFalconMain; - ITeamTalon rightDriveFalconSub; - ITeamTalon leftDriveFalconSub; - TalonSRXSimCollection rightDriveFalconMainSim; - TalonSRXSimCollection leftDriveFalconMainSim; - TalonSRXSimCollection rightDriveFalconSubSim; - TalonSRXSimCollection leftDriveFalconSubSim; + private final DifferentialDrive drive; + private final NavigationSubsystem nav; + + private static ITeamTalon rightDriveFalconMain; + private static ITeamTalon leftDriveFalconMain; + private static ITeamTalon rightDriveFalconSub; + private static ITeamTalon leftDriveFalconSub; + + // Simulation stuff + private DifferentialDrivetrainSim drivetrainSim; + private final Field2d field = new Field2d(); + private double leftEncoderSimVelocity = 0, rightEncoderSimVelocity = 0; + private double leftEncoderSimPosition = 0, rightEncoderSimPosition = 0; + + @SuppressWarnings("unused") + private static TalonSRXSimCollection rightDriveFalconMainSim; + + @SuppressWarnings("unused") + private static TalonSRXSimCollection leftDriveFalconMainSim; + + @SuppressWarnings("unused") + private static TalonSRXSimCollection rightDriveFalconSubSim; + + @SuppressWarnings("unused") + private static TalonSRXSimCollection leftDriveFalconSubSim; + // This is a temp number that's theoretically best public double maxPowerChange = 21.5; public static double maxOutputSlow = .5; @@ -71,27 +98,59 @@ public DriveTrainSubsystem( ITeamTalon rightDriveFalconMain, ITeamTalon leftDriveFalconMain, ITeamTalon rightDriveFalconSub, - ITeamTalon leftDriveFalconSub) { - this.rightDriveFalconMain = + ITeamTalon leftDriveFalconSub, + NavigationSubsystem nav) { + rightDriveFalconMain = Objects.requireNonNull(rightDriveFalconMain, "rightDriveFalconMain must not be null"); - this.leftDriveFalconMain = + leftDriveFalconMain = Objects.requireNonNull(leftDriveFalconMain, "leftDriveFalconMain must not be null"); - this.rightDriveFalconSub = + rightDriveFalconSub = Objects.requireNonNull(rightDriveFalconSub, "rightDriveFalconSub must not be null"); - this.leftDriveFalconSub = + leftDriveFalconSub = Objects.requireNonNull(leftDriveFalconSub, "leftDriveFalconSub must not be null"); + this.nav = nav; + + if (RobotBase.isSimulation()) { + rightDriveFalconMainSim = ((WPI_TalonSRX) rightDriveFalconMain).getSimCollection(); + leftDriveFalconMainSim = ((WPI_TalonSRX) rightDriveFalconMain).getSimCollection(); + rightDriveFalconSubSim = ((WPI_TalonSRX) rightDriveFalconMain).getSimCollection(); + leftDriveFalconSubSim = ((WPI_TalonSRX) rightDriveFalconMain).getSimCollection(); + } setupDrivetrain(); drive = new DifferentialDrive(leftDriveFalconMain, rightDriveFalconMain); setupDifferentialDrive(); + + if (RobotBase.isSimulation()) { + drivetrainSim = + new DifferentialDrivetrainSim( + DriveTrainConstants.kPlant, + DCMotor.getFalcon500(2), + DriveTrainConstants.kDriveGearRatio, + DriveTrainConstants.kTrackWidthMeters, + DriveTrainConstants.kWheelRadiusMeters, + VecBuilder.fill(0, 0, 0.0001, 0.1, 0.1, 0.005, 0.005) // Tune default STDEV? + ); + } + } + + private void configureTalon(ITeamTalon talon) { + talon.configFactoryDefault(); + talon.configSelectedFeedbackSensor( + FeedbackDevice.IntegratedSensor, 0, 50); // use internal encoder + talon.setSelectedSensorPosition(0, 0, 50); // zero encoders + talon.config_kP( + 0, 0); // slotIdx, value need to know the frc_characterization values to plug in here... + talon.config_kI(0, 0); + talon.config_kD(0, 0); + talon.setNeutralMode(NeutralMode.Coast); } private void setupDrivetrain() { - // This configures the falcons to use their internal encoders - TalonFXConfiguration configs = new TalonFXConfiguration(); - configs.primaryPID.selectedFeedbackSensor = FeedbackDevice.IntegratedSensor; - rightDriveFalconMain.configBaseAllSettings(configs); - leftDriveFalconMain.configBaseAllSettings(configs); + configureTalon(rightDriveFalconMain); + configureTalon(leftDriveFalconMain); + configureTalon(rightDriveFalconSub); + configureTalon(leftDriveFalconSub); leftDriveFalconSub.follow(leftDriveFalconMain); rightDriveFalconSub.follow(rightDriveFalconMain); @@ -105,23 +164,48 @@ private void setupDrivetrain() { private void setupDifferentialDrive() { drive.setDeadband(0); - drive.setRightSideInverted(false); - } - - public static DriveTrainSubsystem Create() { - ITeamTalon rightDriveFalconMainCAN = - new TeamTalonFX("Subsystems.DriveTrain.RightMain", Ports.RightDriveFalconMainCAN); - ITeamTalon leftDriveFalconMainCAN = - new TeamTalonFX("Subsystems.DriveTrain.LeftMain", Ports.LeftDriveFalconMainCAN); - ITeamTalon rightDriveFalconSubCAN = - new TeamTalonFX("Subsystems.DriveTrain.RightSub", Ports.RightDriveFalconSubCAN); - ITeamTalon leftDriveFalconSub = - new TeamTalonFX("Subsystems.DriveTrain.LeftSub", Ports.LeftDriveFalconSubCAN); + drive.setRightSideInverted(true); + } + + private void resetEncoders() { + rightDriveFalconMain.setSelectedSensorPosition(0, 0, 50); + leftDriveFalconMain.setSelectedSensorPosition(0, 0, 50); + rightDriveFalconSub.setSelectedSensorPosition(0, 0, 50); + leftDriveFalconSub.setSelectedSensorPosition(0, 0, 50); + } + + public Pose2d getPose() { + return nav.odometry.getPoseMeters(); + } + + public static DriveTrainSubsystem Create(NavigationSubsystem nav) { + ITeamTalon rightDriveFalconMain; + ITeamTalon leftDriveFalconMain; + ITeamTalon rightDriveFalconSub; + ITeamTalon leftDriveFalconSub; + + if (RobotBase.isSimulation()) { + rightDriveFalconMain = + new TeamTalonSRX("Subsystems.DriveTrain.RightMain", Ports.RightDriveFalconMainCAN); + leftDriveFalconMain = + new TeamTalonSRX("Subsystems.DriveTrain.LeftMain", Ports.RightDriveFalconMainCAN); + rightDriveFalconSub = + new TeamTalonSRX("Subsystems.DriveTrain.RightSub", Ports.RightDriveFalconMainCAN); + leftDriveFalconSub = + new TeamTalonSRX("Subsystems.DriveTrain.LeftSub", Ports.RightDriveFalconMainCAN); + } else { + rightDriveFalconMain = + new TeamTalonFX("Subsystems.DriveTrain.RightMain", Ports.RightDriveFalconMainCAN); + leftDriveFalconMain = + new TeamTalonFX("Subsystems.DriveTrain.LeftMain", Ports.RightDriveFalconMainCAN); + rightDriveFalconSub = + new TeamTalonFX("Subsystems.DriveTrain.RightSub", Ports.RightDriveFalconMainCAN); + leftDriveFalconSub = + new TeamTalonFX("Subsystems.DriveTrain.LeftSub", Ports.RightDriveFalconMainCAN); + } + return new DriveTrainSubsystem( - rightDriveFalconMainCAN, - leftDriveFalconMainCAN, - rightDriveFalconSubCAN, - leftDriveFalconSub); + rightDriveFalconMain, leftDriveFalconMain, rightDriveFalconSub, leftDriveFalconSub, nav); } @Override @@ -132,6 +216,106 @@ public void periodic() { updateVelocityForStopMetersPerSecond(); printDrivetrainData(); updateMotorOutputs(); + field.setRobotPose(getPose()); + nav.odometry.update( + Rotation2d.fromDegrees(nav.getHeading()), + getLeftDistanceMeters(), + getRightDistanceMeters()); + SmartDashboard.putNumber("Left position", getLeftPosition()); + SmartDashboard.putNumber("Right position", getRightPosition()); + } + + @Override + public void simulationPeriodic() { + drivetrainSim.setInputs( + leftDriveFalconMain.get() * RobotController.getBatteryVoltage(), + rightDriveFalconMain.get() * RobotController.getBatteryVoltage()); + drivetrainSim.update(0.020); + System.out.println("Gyro Set to: " + -drivetrainSim.getHeading().getDegrees()); + + // Gyro + int dev = SimDeviceDataJNI.getSimDeviceHandle("navX-Sensor[0]"); + SimDouble angle = new SimDouble(SimDeviceDataJNI.getSimValueHandle(dev, "Yaw")); + // NavX expects clockwise positive, but sim outputs clockwise negative + angle.set(-drivetrainSim.getHeading().getDegrees()); + + // Encoders + leftEncoderSimVelocity = metersToTicks(drivetrainSim.getLeftVelocityMetersPerSecond()) / 10d; + leftEncoderSimPosition = metersToTicks(drivetrainSim.getLeftPositionMeters()); + rightEncoderSimVelocity = metersToTicks(drivetrainSim.getRightVelocityMetersPerSecond()) / 10d; + rightEncoderSimPosition = metersToTicks(drivetrainSim.getRightPositionMeters()); + } + + public double ticksToRotations(double ticks) { + return ticks / (double) DriveTrainConstants.kEncoderTicksPerRotation; + } + + public double rotationsToTicks(double rotations) { + return rotations * (double) DriveTrainConstants.kEncoderTicksPerRotation; + } + + public double ticksToMeters(double ticks) { + return ticksToRotations(ticks) * DriveTrainConstants.kWheelCircumferenceMeters; + } + + public double metersToTicks(double meters) { + double rotations = meters / DriveTrainConstants.kWheelCircumferenceMeters; + return rotationsToTicks(rotations); + } + + public double getLeftVelocityTicksPerDs() { + if (RobotBase.isSimulation()) { + return leftEncoderSimVelocity; + } + return (leftDriveFalconMain.getSelectedSensorVelocity(0) + + leftDriveFalconSub.getSelectedSensorVelocity(0)) + / 2.0d; + } + + public double getRightVelocityTicksPerDs() { + if (RobotBase.isSimulation()) { + return rightEncoderSimVelocity; + } + return (rightDriveFalconMain.getSelectedSensorVelocity(0) + + rightDriveFalconSub.getSelectedSensorVelocity(0)) + / 2.0d; + } + + private double getLeftPosition() { + if (RobotBase.isSimulation()) { + return leftEncoderSimPosition; + } + return (leftDriveFalconMain.getSelectedSensorPosition(0) + + leftDriveFalconSub.getSelectedSensorPosition(0)) + / 2.0d; + } + + private double getRightPosition() { + if (RobotBase.isSimulation()) { + return rightEncoderSimPosition; + } + return (rightDriveFalconMain.getSelectedSensorPosition(0) + + rightDriveFalconSub.getSelectedSensorPosition(0)) + / 2.0d; + } + + public double getLeftDistanceMeters() { + double leftTicks = getLeftPosition(); + return ticksToMeters(leftTicks); + } + + public double getRightDistanceMeters() { + double rightTicks = getRightPosition(); + return ticksToMeters(rightTicks); + } + + public void resetOdometry() { + resetOdometry(new Pose2d(0, 0, new Rotation2d(0))); + } + + public void resetOdometry(Pose2d translationPose) { + resetEncoders(); + nav.odometry.resetPosition(translationPose, Rotation2d.fromDegrees(nav.getHeading())); } private void updateMaxPowerChange() { diff --git a/src/main/java/frc/robot/subsystems/NavigationSubsystem.java b/src/main/java/frc/robot/subsystems/NavigationSubsystem.java index a2926556..e8bdb921 100644 --- a/src/main/java/frc/robot/subsystems/NavigationSubsystem.java +++ b/src/main/java/frc/robot/subsystems/NavigationSubsystem.java @@ -83,8 +83,16 @@ public void periodic() { accumulatedHeading += difference; } + // Inverts NavX yaw as Odometry takes CCW as positive + // returns -180..180 public double getHeading() { - return navx.getYaw(); + double heading = -navx.getYaw(); + if (heading > 180) { + heading -= 180; + } else if (heading < -180) { + heading += 180; + } + return heading; } public double getAccumulatedHeading() {