From 9ff0564b43eae955f58a9e9bb6bc01fa003a9c5b Mon Sep 17 00:00:00 2001 From: Garrett Goebel Date: Fri, 19 Feb 2021 11:39:09 -0500 Subject: [PATCH 1/4] Initial sim support. compiles clean. needs testing --- src/main/java/frc/robot/ITeamTalon.java | 4 + src/main/java/frc/robot/RobotContainer.java | 4 +- .../robot/constants/DriveTrainConstants.java | 38 ++- .../robot/subsystems/DriveTrainSubsystem.java | 236 +++++++++++++++--- .../robot/subsystems/NavigationSubsystem.java | 10 +- 5 files changed, 247 insertions(+), 45 deletions(-) diff --git a/src/main/java/frc/robot/ITeamTalon.java b/src/main/java/frc/robot/ITeamTalon.java index 050ba142..0a9f8ddf 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/constants/DriveTrainConstants.java b/src/main/java/frc/robot/constants/DriveTrainConstants.java index d5ff6cd7..ac928fba 100644 --- a/src/main/java/frc/robot/constants/DriveTrainConstants.java +++ b/src/main/java/frc/robot/constants/DriveTrainConstants.java @@ -1,13 +1,45 @@ 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..36f8856e 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveTrainSubsystem.java @@ -12,32 +12,51 @@ 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 double maxPowerChange = 0.215; public static double maxOutputSlow = .5; public static double maxOutputFast = 1; public double currentMaxPower = maxOutputSlow; @@ -71,27 +90,56 @@ 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 +153,44 @@ 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 +201,95 @@ 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 51ca021f..9d4f6c1f 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() { From 32bba3913db5ffcdbd533f0f33194ebe35174b95 Mon Sep 17 00:00:00 2001 From: Garrett Goebel Date: Fri, 19 Feb 2021 11:42:07 -0500 Subject: [PATCH 2/4] fix renamed constant in comment --- .../java/frc/robot/commands/drivingCommands/DriveEncoders.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/drivingCommands/DriveEncoders.java b/src/main/java/frc/robot/commands/drivingCommands/DriveEncoders.java index 68497f98..cc92162c 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) From 4db66cbe9e319aeaad4520448011363e9d725422 Mon Sep 17 00:00:00 2001 From: github-actions <> Date: Fri, 19 Feb 2021 16:47:40 +0000 Subject: [PATCH 3/4] Google Java Format --- src/main/java/frc/robot/ITeamTalon.java | 6 +- .../robot/constants/DriveTrainConstants.java | 20 ++-- .../robot/subsystems/DriveTrainSubsystem.java | 104 +++++++++++------- .../robot/subsystems/NavigationSubsystem.java | 2 +- 4 files changed, 78 insertions(+), 54 deletions(-) diff --git a/src/main/java/frc/robot/ITeamTalon.java b/src/main/java/frc/robot/ITeamTalon.java index 0a9f8ddf..59dbfb2c 100644 --- a/src/main/java/frc/robot/ITeamTalon.java +++ b/src/main/java/frc/robot/ITeamTalon.java @@ -151,9 +151,9 @@ public default void configureWithPidParameters(PidParameters pidParameters, int public ErrorCode configPeakOutputReverse(double percentOut); - public ErrorCode configFactoryDefault (int timeoutMs); - - public ErrorCode configFactoryDefault (); + 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/constants/DriveTrainConstants.java b/src/main/java/frc/robot/constants/DriveTrainConstants.java index ac928fba..07fc4ed6 100644 --- a/src/main/java/frc/robot/constants/DriveTrainConstants.java +++ b/src/main/java/frc/robot/constants/DriveTrainConstants.java @@ -26,20 +26,18 @@ public class DriveTrainConstants { 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 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 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 + 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); + 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 36f8856e..f7e41212 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveTrainSubsystem.java @@ -50,10 +50,18 @@ public class DriveTrainSubsystem extends SubsystemBase { 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; + + @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 = 0.215; @@ -103,10 +111,10 @@ public DriveTrainSubsystem( 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(); + rightDriveFalconMainSim = ((WPI_TalonSRX) rightDriveFalconMain).getSimCollection(); + leftDriveFalconMainSim = ((WPI_TalonSRX) rightDriveFalconMain).getSimCollection(); + rightDriveFalconSubSim = ((WPI_TalonSRX) rightDriveFalconMain).getSimCollection(); + leftDriveFalconSubSim = ((WPI_TalonSRX) rightDriveFalconMain).getSimCollection(); } setupDrivetrain(); @@ -114,22 +122,25 @@ public DriveTrainSubsystem( 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? - ); + 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.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_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); @@ -174,23 +185,27 @@ public static DriveTrainSubsystem Create(NavigationSubsystem nav) { 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); + 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); + 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( - rightDriveFalconMain, - leftDriveFalconMain, - rightDriveFalconSub, - leftDriveFalconSub, - nav); + rightDriveFalconMain, leftDriveFalconMain, rightDriveFalconSub, leftDriveFalconSub, nav); } @Override @@ -202,7 +217,10 @@ public void periodic() { printDrivetrainData(); updateMotorOutputs(); field.setRobotPose(getPose()); - nav.odometry.update(Rotation2d.fromDegrees(nav.getHeading()), getLeftDistanceMeters(), getRightDistanceMeters()); + nav.odometry.update( + Rotation2d.fromDegrees(nav.getHeading()), + getLeftDistanceMeters(), + getRightDistanceMeters()); SmartDashboard.putNumber("Left position", getLeftPosition()); SmartDashboard.putNumber("Right position", getRightPosition()); } @@ -210,8 +228,8 @@ public void periodic() { @Override public void simulationPeriodic() { drivetrainSim.setInputs( - leftDriveFalconMain.get() * RobotController.getBatteryVoltage(), - rightDriveFalconMain.get() * RobotController.getBatteryVoltage()); + leftDriveFalconMain.get() * RobotController.getBatteryVoltage(), + rightDriveFalconMain.get() * RobotController.getBatteryVoltage()); drivetrainSim.update(0.020); System.out.println("Gyro Set to: " + -drivetrainSim.getHeading().getDegrees()); @@ -220,7 +238,7 @@ public void simulationPeriodic() { 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()); @@ -249,28 +267,36 @@ public double getLeftVelocityTicksPerDs() { if (RobotBase.isSimulation()) { return leftEncoderSimVelocity; } - return (leftDriveFalconMain.getSelectedSensorVelocity(0) + leftDriveFalconSub.getSelectedSensorVelocity(0)) / 2.0d; + 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; + 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; + 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; + return (rightDriveFalconMain.getSelectedSensorPosition(0) + + rightDriveFalconSub.getSelectedSensorPosition(0)) + / 2.0d; } public double getLeftDistanceMeters() { diff --git a/src/main/java/frc/robot/subsystems/NavigationSubsystem.java b/src/main/java/frc/robot/subsystems/NavigationSubsystem.java index 9d4f6c1f..0192696d 100644 --- a/src/main/java/frc/robot/subsystems/NavigationSubsystem.java +++ b/src/main/java/frc/robot/subsystems/NavigationSubsystem.java @@ -88,7 +88,7 @@ public void periodic() { public double getHeading() { double heading = -navx.getYaw(); if (heading > 180) { - heading -=180; + heading -= 180; } else if (heading < -180) { heading += 180; } From c2d46d824ca41a58aa78aef61e432d48d78ab063 Mon Sep 17 00:00:00 2001 From: Keon Date: Tue, 2 Mar 2021 16:30:06 -0500 Subject: [PATCH 4/4] Fix MaxPowerChange --- src/main/java/frc/robot/subsystems/DriveTrainSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/DriveTrainSubsystem.java b/src/main/java/frc/robot/subsystems/DriveTrainSubsystem.java index f7e41212..45874241 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveTrainSubsystem.java @@ -64,7 +64,7 @@ public class DriveTrainSubsystem extends SubsystemBase { private static TalonSRXSimCollection leftDriveFalconSubSim; // This is a temp number that's theoretically best - public double maxPowerChange = 0.215; + public double maxPowerChange = 21.5; public static double maxOutputSlow = .5; public static double maxOutputFast = 1; public double currentMaxPower = maxOutputSlow;