diff --git a/build.gradle b/build.gradle index d593fad..c22eae5 100644 --- a/build.gradle +++ b/build.gradle @@ -51,7 +51,7 @@ def deployArtifact = deploy.targets.roborio.artifacts.frcJava wpi.java.debugJni = false // Set this to true to enable desktop support. -def includeDesktopSupport = false +def includeDesktopSupport = true // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Also defines JUnit 5. diff --git a/simgui.json b/simgui.json index 772bb4e..4a005a3 100644 --- a/simgui.json +++ b/simgui.json @@ -1,8 +1,35 @@ { + "HALProvider": { + "PWM Outputs": { + "window": { + "visible": true + } + }, + "RoboRIO": { + "window": { + "visible": true + } + } + }, "NTProvider": { "types": { "/FMSInfo": "FMSInfo", + "/SmartDashboard/Auto Chooser": "String Chooser", "/SmartDashboard/SendableChooser[0]": "String Chooser" } + }, + "NetworkTables": { + "Persistent Values": { + "open": false + }, + "Retained Values": { + "open": false + }, + "Transitory Values": { + "open": false + } + }, + "NetworkTables Info": { + "visible": true } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 913017c..089e9e3 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -32,7 +32,7 @@ public void autonomousInit() { } public void teleopInit() { - auto.cancel(); + if (auto != null) auto.cancel(); container.led.setRGB(255, 255, 255); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 607e8c5..ace08b0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -2,8 +2,6 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; -import edu.wpi.first.cameraserver.CameraServer; -import edu.wpi.first.cscore.VideoMode; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -22,18 +20,13 @@ import frc.robot.commands.ShintakeCommands.Outtake; import frc.robot.commands.ShintakeCommands.SetShooter; import frc.robot.commands.ShintakeCommands.Shoot; -import frc.robot.commands.VisionCommands.AngleShooter; -import frc.robot.commands.VisionCommands.TagAlign; import frc.robot.constants.ShintakeConstants; import frc.robot.subsystems.Climber; import frc.robot.subsystems.Led; import frc.robot.subsystems.Shintake; import frc.robotkt.constants.IOConstants; -import frc.robotkt.constants.VisionConstants; import frc.robotkt.subsystems.Arm; -import frc.robotkt.subsystems.Camera; import frc.robotkt.subsystems.Drivetrain; -import org.photonvision.PhotonCamera; public class RobotContainer { // Controllers @@ -48,8 +41,8 @@ public class RobotContainer { final Arm arm = new Arm(); // Cameras - final Camera camera = new Camera(new PhotonCamera(VisionConstants.kCameraName), drivetrain); - final PhotonCamera drivercam = new PhotonCamera(VisionConstants.kDriverCameraName); + // final Camera camera = new Camera(new PhotonCamera(VisionConstants.kCameraName), drivetrain); + // final PhotonCamera drivercam = new PhotonCamera(VisionConstants.kDriverCameraName); // Auto final SendableChooser autoChooser = AutoBuilder.buildAutoChooser(); @@ -60,7 +53,7 @@ public RobotContainer() { NamedCommands.registerCommand("SetShooter", new SetShooter(shintake, driverctl)); NamedCommands.registerCommand("Shoot", new Shoot(shintake)); NamedCommands.registerCommand("Intake", new Intake(shintake, led)); - NamedCommands.registerCommand("Angle", new AngleShooter(drivetrain, arm, camera, led, driverctl)); + // NamedCommands.registerCommand("Angle", new AngleShooter(drivetrain, arm, camera, led, driverctl)); NamedCommands.registerCommand("ResetArm", new ResetArm(arm)); // Auto Chooser @@ -71,16 +64,16 @@ public RobotContainer() { // Default commands drivetrain.setDefaultCommand(new RunCommand(() -> drivetrain.drive(driverctl), drivetrain)); - camera.setDefaultCommand(new RunCommand(camera::reset, camera)); + // camera.setDefaultCommand(new RunCommand(camera::reset, camera)); // Shuffleboard/Driver camera - drivercam.setDriverMode(true); - CameraServer.startAutomaticCapture().setVideoMode(new VideoMode(1, 320, 240, 60)); + // drivercam.setDriverMode(true); + // CameraServer.startAutomaticCapture().setVideoMode(new VideoMode(1, 320, 240, 60)); } private void bindings() { // Tag stuff - driverctl.b().whileTrue(new TagAlign(drivetrain, camera, driverctl)); + // driverctl.b().whileTrue(new TagAlign(drivetrain, camera, driverctl)); // Emergency driverctl.rightTrigger().whileTrue(new RunCommand(drivetrain::lock, drivetrain)); @@ -105,8 +98,8 @@ private void bindings() { driverctl.leftBumper().onTrue(new Shoot(shintake)); // Arm - operctl.axisGreaterThan(1, 0.4).whileTrue(new ManualAngle(arm, true)); - operctl.axisLessThan(1, -0.4).whileTrue(new ManualAngle(arm, false)); + operctl.axisLessThan(1, -0.4).whileTrue(new ManualAngle(arm, true)); + operctl.axisGreaterThan(1, 0.4).whileTrue(new ManualAngle(arm, false)); operctl.rightBumper().whileTrue(new ManualExtend(arm, true)); operctl.leftBumper().whileTrue(new ManualExtend(arm, false)); diff --git a/src/main/kotlin/frc/robotkt/constants/DriveConstants.kt b/src/main/kotlin/frc/robotkt/constants/DriveConstants.kt index 981c67a..fbdef94 100644 --- a/src/main/kotlin/frc/robotkt/constants/DriveConstants.kt +++ b/src/main/kotlin/frc/robotkt/constants/DriveConstants.kt @@ -32,7 +32,7 @@ object DriveConstants { Translation2d(-Dimensions.kWheelBase / 2, -Dimensions.kTrackWidth / 2) ) - const val kMaxSpeedMps = 1.0 + const val kMaxSpeedMps = 15.0 const val kMaxSpeedAnglular = kTau const val kGyroFactor = -1.0 } \ No newline at end of file diff --git a/src/main/kotlin/frc/robotkt/constants/IOConstants.kt b/src/main/kotlin/frc/robotkt/constants/IOConstants.kt index c1bae23..8033a68 100644 --- a/src/main/kotlin/frc/robotkt/constants/IOConstants.kt +++ b/src/main/kotlin/frc/robotkt/constants/IOConstants.kt @@ -17,7 +17,6 @@ object IOConstants { const val kRightMotor = 31 const val kLeftMotor = 32 const val kExtMotor = 33 - const val kPotChannel = 0 } object Led { @@ -40,7 +39,7 @@ object IOConstants { object Climber { const val kLeft = 41 const val kRight = 42 - + const val kLeftLock = 3 const val kRightLock = 4 } diff --git a/src/main/kotlin/frc/robotkt/constants/ModuleConstants.kt b/src/main/kotlin/frc/robotkt/constants/ModuleConstants.kt index 2e40825..095bd57 100644 --- a/src/main/kotlin/frc/robotkt/constants/ModuleConstants.kt +++ b/src/main/kotlin/frc/robotkt/constants/ModuleConstants.kt @@ -22,13 +22,6 @@ object ModuleConstants { const val kBevelPinionTeeth = 15.0 const val kReduction = (kBevelGearTeeth * kSpurTeeth) / (kPinionTeeth * kBevelPinionTeeth) const val kCurrentLimit = 40 - - const val kP = 0.15 - const val kI = 0.0 - const val kD = 0.01 - const val kFF = 1 / Wheel.kFreeSpeed - const val kPidMin = -1.0 - const val kPidMax = 1.0 } object DriveEncoder { @@ -38,13 +31,6 @@ object ModuleConstants { object TurnMotor { const val kCurrentLimit = 40 - - const val kP = 0.3 - const val kI = 0.0 - const val kD = 0.02 - const val kFF = 0.0 - const val kPidMin = -1.0 - const val kPidMax = 1.0 } object TurnEncoder { diff --git a/src/main/kotlin/frc/robotkt/constants/PidConstants.kt b/src/main/kotlin/frc/robotkt/constants/PidConstants.kt index f89e9ba..bcce33a 100644 --- a/src/main/kotlin/frc/robotkt/constants/PidConstants.kt +++ b/src/main/kotlin/frc/robotkt/constants/PidConstants.kt @@ -38,12 +38,12 @@ object PidConstants { } object Arm { - const val kPivP = 0.2 + const val kPivP = 0.001 const val kPivI = 0.0 const val kPivD = 0.0 const val kPivFF = 0.0 - const val kExtP = 0.7 + const val kExtP = 0.3 const val kExtI = 0.0 const val kExtD = 0.0 const val kExtFF = 0.0 diff --git a/src/main/kotlin/frc/robotkt/swerve/SwerveModule.kt b/src/main/kotlin/frc/robotkt/swerve/SwerveModule.kt index 2124ff8..bc3b426 100644 --- a/src/main/kotlin/frc/robotkt/swerve/SwerveModule.kt +++ b/src/main/kotlin/frc/robotkt/swerve/SwerveModule.kt @@ -10,16 +10,17 @@ import edu.wpi.first.math.geometry.Rotation2d import edu.wpi.first.math.kinematics.SwerveModulePosition import edu.wpi.first.math.kinematics.SwerveModuleState import frc.robotkt.constants.ModuleConstants +import frc.robotkt.constants.PidConstants -class SwerveModule(driveID: Int, turnID: Int, val angularOffset: Double) { - val drive = CANSparkMax(driveID, MotorType.kBrushless) - val turn = CANSparkMax(turnID, MotorType.kBrushless) +class SwerveModule(driveId: Int, turnId: Int, val angularOffset: Double) { + val driveMotor = CANSparkMax(driveId, MotorType.kBrushless) + val turnMotor = CANSparkMax(turnId, MotorType.kBrushless) val driveEncoder: RelativeEncoder val turnEncoder: SparkAbsoluteEncoder - val drivePID: SparkPIDController - val turnPID: SparkPIDController + val drivePid: SparkPIDController + val turnPid: SparkPIDController var desiredState = SwerveModuleState(0.0, Rotation2d()) set(desired) { @@ -28,25 +29,25 @@ class SwerveModule(driveID: Int, turnID: Int, val angularOffset: Double) { corrected.angle = desired.angle.plus(Rotation2d(angularOffset)) val optimized = SwerveModuleState.optimize(corrected, Rotation2d(turnEncoder.position))!! - drivePID.setReference(optimized.speedMetersPerSecond, ControlType.kVelocity) - turnPID.setReference(optimized.angle.radians, ControlType.kPosition) + drivePid.setReference(optimized.speedMetersPerSecond, ControlType.kVelocity) + turnPid.setReference(optimized.angle.radians, ControlType.kPosition) field = desired } init { - drive.restoreFactoryDefaults() - turn.restoreFactoryDefaults() + driveMotor.restoreFactoryDefaults() + turnMotor.restoreFactoryDefaults() // Setup encoders and PID controllers - driveEncoder = drive.encoder - turnEncoder = turn.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle) + driveEncoder = driveMotor.encoder + turnEncoder = turnMotor.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle) - drivePID = drive.getPIDController() - turnPID = turn.getPIDController() + drivePid = driveMotor.getPIDController() + turnPid = turnMotor.getPIDController() - drivePID.setFeedbackDevice(driveEncoder) - turnPID.setFeedbackDevice(turnEncoder) + drivePid.setFeedbackDevice(driveEncoder) + turnPid.setFeedbackDevice(turnEncoder) // Apply position and velocity conversion factors driveEncoder.positionConversionFactor = ModuleConstants.DriveEncoder.kPositionFactor @@ -58,32 +59,35 @@ class SwerveModule(driveID: Int, turnID: Int, val angularOffset: Double) { turnEncoder.setInverted(ModuleConstants.TurnEncoder.kInverted) // Turn PID wrap - turnPID.setPositionPIDWrappingEnabled(true) - turnPID.setPositionPIDWrappingMinInput(ModuleConstants.TurnEncoder.kMinPidInput) - turnPID.setPositionPIDWrappingMaxInput(ModuleConstants.TurnEncoder.kMaxPidInput) + turnPid.setPositionPIDWrappingEnabled(true) + turnPid.setPositionPIDWrappingMinInput(ModuleConstants.TurnEncoder.kMinPidInput) + turnPid.setPositionPIDWrappingMaxInput(ModuleConstants.TurnEncoder.kMaxPidInput) // Apply PID constants - drivePID.setP(ModuleConstants.DriveMotor.kP) - drivePID.setI(ModuleConstants.DriveMotor.kI) - drivePID.setD(ModuleConstants.DriveMotor.kD) - drivePID.setFF(ModuleConstants.DriveMotor.kFF) - drivePID.setOutputRange(ModuleConstants.DriveMotor.kPidMin, ModuleConstants.DriveMotor.kPidMax) - - turnPID.setP(ModuleConstants.TurnMotor.kP) - turnPID.setI(ModuleConstants.TurnMotor.kI) - turnPID.setD(ModuleConstants.TurnMotor.kD) - turnPID.setFF(ModuleConstants.TurnMotor.kFF) - turnPID.setOutputRange(ModuleConstants.TurnMotor.kPidMin, ModuleConstants.TurnMotor.kPidMax) + drivePid.p = PidConstants.DriveMotor.kP + drivePid.i = PidConstants.DriveMotor.kI + drivePid.d = PidConstants.DriveMotor.kD + drivePid.ff = PidConstants.DriveMotor.kFF + drivePid.setOutputRange(PidConstants.DriveMotor.kPidMin, PidConstants.DriveMotor.kPidMax) + + turnPid.p = PidConstants.TurnMotor.kP + turnPid.i = PidConstants.TurnMotor.kI + turnPid.d = PidConstants.TurnMotor.kD + turnPid.ff = PidConstants.TurnMotor.kFF + turnPid.setOutputRange(PidConstants.TurnMotor.kPidMin, PidConstants.TurnMotor.kPidMax) // idle mode and smart current - drive.setIdleMode(ModuleConstants.kIdleMode) - turn.setIdleMode(ModuleConstants.kIdleMode) - drive.setSmartCurrentLimit(ModuleConstants.DriveMotor.kCurrentLimit) - turn.setSmartCurrentLimit(ModuleConstants.TurnMotor.kCurrentLimit) + driveMotor.setIdleMode(ModuleConstants.kIdleMode) + turnMotor.setIdleMode(ModuleConstants.kIdleMode) + driveMotor.setSmartCurrentLimit(ModuleConstants.DriveMotor.kCurrentLimit) + turnMotor.setSmartCurrentLimit(ModuleConstants.TurnMotor.kCurrentLimit) + + // One module is f*ed up + if (driveId == 40) driveMotor.inverted = true // Maintain configs - drive.burnFlash() - turn.burnFlash() + driveMotor.burnFlash() + turnMotor.burnFlash() // Finish setup desiredState.angle = Rotation2d(turnEncoder.position) @@ -97,8 +101,8 @@ class SwerveModule(driveID: Int, turnID: Int, val angularOffset: Double) { get() = SwerveModulePosition(driveEncoder.position, Rotation2d(turnEncoder.position - angularOffset)) fun stop() { - drive.stopMotor() - turn.stopMotor() + driveMotor.stopMotor() + turnMotor.stopMotor() } fun resetEncoders() {