Add changes from 3/9/24

frc2204 · Mar 10, 2024 · f998f6b · f998f6b
1 parent 4af6a0d
commit f998f6b
Show file tree

Hide file tree

Showing 10 changed files with 268 additions and 8 deletions.
diff --git a/src/main/java/org/rambots/BuildConstants.java b/src/main/java/org/rambots/BuildConstants.java
@@ -7,12 +7,12 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "frc-2024";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 9;
-  public static final String GIT_SHA = "e235e0327a4c8b1b2be006c6080e1000250747ba";
-  public static final String GIT_DATE = "2024-03-05 18:20:38 PST";
+  public static final int GIT_REVISION = 10;
+  public static final String GIT_SHA = "4af6a0d87fab6e63d1cb30c4959e65c908757abf";
+  public static final String GIT_DATE = "2024-03-06 15:29:43 PST";
   public static final String GIT_BRANCH = "james/swerve";
-  public static final String BUILD_DATE = "2024-03-06 15:22:24 PST";
-  public static final long BUILD_UNIX_TIME = 1709767344100L;
+  public static final String BUILD_DATE = "2024-03-09 19:35:02 PST";
+  public static final long BUILD_UNIX_TIME = 1710041702135L;
   public static final int DIRTY = 1;
 
   private BuildConstants(){}

diff --git a/src/main/java/org/rambots/RobotContainer.kt b/src/main/java/org/rambots/RobotContainer.kt
@@ -22,6 +22,8 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser
 import org.rambots.commands.*
+import org.rambots.subsystems.ArmSubsystem
+import org.rambots.subsystems.ElevatorSubsystem
 import org.rambots.subsystems.drive.*
 import org.rambots.subsystems.drive.DriveConstants.moduleConfigs
 import org.rambots.subsystems.vision.AprilTagVision
@@ -30,6 +32,7 @@ import org.rambots.subsystems.vision.AprilTagVisionIOLimelight
 import org.rambots.subsystems.vision.AprilTagVisionIOPhotonVisionSIM
 import org.rambots.util.VisionHelpers.TimestampedVisionUpdate
 import java.util.function.Consumer
+import kotlin.math.pow
 
 
 /**
@@ -74,7 +77,7 @@ object RobotContainer {
                 // new ModuleIOTalonFX(2),
                 // new ModuleIOTalonFX(3));
 //                flywheel = Flywheel(FlywheelIOTalonFX())
-                aprilTagVision = AprilTagVision(AprilTagVisionIOLimelight("limelight"))
+                aprilTagVision = AprilTagVision(AprilTagVisionIOLimelight("limelight-three"))
             }
 
             Constants.Mode.SIM -> {
@@ -160,11 +163,25 @@ object RobotContainer {
      * instantiating a [GenericHID] or one of its subclasses ([ ] or [XboxController]), and then passing it to a [ ].
      */
     private fun configureButtonBindings() {
-        drive.defaultCommand = DriveCommands.joystickDrive(drive, driveController, { -controller.leftY }, { -controller.leftX }, { -controller.rightX })
+        drive.defaultCommand = DriveCommands.joystickDrive(
+            drive,
+            driveController,
+            { (-controller.leftY).pow(3.0) },
+            { (-controller.leftX).pow(3.0) },
+            { -controller.rightX }
+        )
+        ArmSubsystem.defaultCommand = ArmPositionCommand { ArmSubsystem.desiredPosition }
+        ElevatorSubsystem.defaultCommand = ElevatorPositionCommand { ElevatorSubsystem.desiredPosition }
 
         controller.L1().whileTrue(AmpScoring())
         controller.R2().whileTrue(SourceIntake())
 
+        controller.square().whileTrue(ArmPositionCommand { -70.0 })
+        controller.circle().whileTrue(ArmPositionCommand { 0.0 })
+
+        controller.povUp().whileTrue(ElevatorPositionCommand { 0.0 })
+        controller.povDown().whileTrue(ElevatorPositionCommand { -70.0 })
+
 //        controller.a().whileTrue(PathFinderAndFollow(driveController.driveModeType))
 
 //        controller

diff --git a/src/main/java/org/rambots/commands/ArmPositionCommand.kt b/src/main/java/org/rambots/commands/ArmPositionCommand.kt
@@ -0,0 +1,29 @@
+package org.rambots.commands
+
+import edu.wpi.first.wpilibj2.command.Command
+import org.rambots.config.ArmConstants.ARM_ACCEPTABLE_ERROR
+import org.rambots.subsystems.ArmSubsystem
+import org.rambots.util.CommandUtils.generateErrorRange
+
+class ArmPositionCommand(private val angle: () -> Double, private val finishState: () -> Double) : Command() {
+
+    constructor(angle: () -> Double) : this(angle, angle)
+    constructor(angle: Double) : this ({angle})
+
+    init {
+        addRequirements(ArmSubsystem)
+    }
+
+    override fun initialize() {
+        ArmSubsystem.desiredPosition = angle.invoke()
+    }
+
+    override fun execute() {
+        ArmSubsystem.position = angle.invoke()
+    }
+
+    override fun isFinished(): Boolean {
+        return generateErrorRange(ArmSubsystem.position, ARM_ACCEPTABLE_ERROR).contains(finishState.invoke())
+    }
+
+}
diff --git a/src/main/java/org/rambots/commands/DriveCommands.java b/src/main/java/org/rambots/commands/DriveCommands.java
@@ -45,7 +45,7 @@ public static Command joystickDrive(
             DoubleSupplier xSupplier,
             DoubleSupplier ySupplier,
             DoubleSupplier omegaSupplier) {
-        return Commands.run(
+        return Commands.runOnce(
                 () -> {
                     // Apply deadband
                     double linearMagnitude =

diff --git a/src/main/java/org/rambots/commands/ElevatorPositionCommand.kt b/src/main/java/org/rambots/commands/ElevatorPositionCommand.kt
@@ -0,0 +1,29 @@
+package org.rambots.commands
+
+import edu.wpi.first.wpilibj2.command.Command
+import org.rambots.config.ArmConstants.ARM_ACCEPTABLE_ERROR
+import org.rambots.subsystems.ElevatorSubsystem
+import org.rambots.util.CommandUtils.generateErrorRange
+
+class ElevatorPositionCommand(private val position: () -> Double, private val finishState: () -> Double) : Command() {
+
+    constructor(position: () -> Double) : this(position, position)
+    constructor(position: Double) : this ({position})
+
+    init {
+        addRequirements(ElevatorSubsystem)
+    }
+
+    override fun initialize() {
+        ElevatorSubsystem.desiredPosition = position.invoke()
+    }
+
+    override fun execute() {
+        ElevatorSubsystem.position = position.invoke()
+    }
+
+    override fun isFinished(): Boolean {
+        return generateErrorRange(ElevatorSubsystem.position, ARM_ACCEPTABLE_ERROR).contains(finishState.invoke())
+    }
+
+}
diff --git a/src/main/java/org/rambots/config/ArmConstants.kt b/src/main/java/org/rambots/config/ArmConstants.kt
@@ -0,0 +1,63 @@
+package org.rambots.config
+
+import com.pathplanner.lib.util.PIDConstants
+
+object ArmConstants {
+    /** Arm Configuration */
+    const val ARM_NEO_LEADER_ID = 13
+    const val ARM_NEO_FOLLOWER_ID = 14
+
+    val ARM_PID = PIDConstants(0.02, 0.0, 0.0)
+
+    val ARM_ACCEPTABLE_ERROR = 0.5
+
+
+
+    /** Elevator PID */
+    const val ElevatorKP = 0.2
+    const val ElevatorKI = 0.0
+    const val ElevatorKD = 0.0
+
+    /** Intake Configuration */
+    const val IntakeMotorOneCANID = 17
+    const val IntakeMotorTwoCANID = 18
+    const val IntakePower = -0.2
+    const val IntakeCurrent = 20
+
+    /** Shooter Configuration */
+    const val TopShooterCANID = 19
+    const val BottomShooterCANID = 20
+
+    /** Shooter PID */
+    const val ShooterKP = 0.00035
+    const val ShooterKD = 0.15
+
+    /** Wrist Configuration */
+    const val WristCANID = 21
+
+    /** Wrist PID */
+    const val WristKP = 0.2
+    const val WristKI = 0.0
+    const val WristKD = 0.0
+
+    /** Intake Positions */
+    const val ArmIntakePosition = 0.0
+    const val WristIntakePosition = -63.0
+    const val ElevatorIntakePosition = -50.0
+
+    /** Climb Positions */
+    const val ArmClimbPosition = -70.0
+    const val ElevatorClimbPosition = -70.0
+    const val ElevatorRetractPosition = 0.0
+
+    /** Manual Arm Controls */
+    // place holders
+    const val MaximumAngle = 90
+    const val MinimumAngle = 30
+    const val ArmIdlePosition = -45.0
+
+    /** Limelight */
+    const val LimelightMountAngleDegrees = 25.0
+    const val LimelightLensHeightInches = 20.0
+    const val SpeakerGoalHeightInches = 50.0
+}
diff --git a/src/main/java/org/rambots/config/ElevatorConstants.kt b/src/main/java/org/rambots/config/ElevatorConstants.kt
@@ -0,0 +1,12 @@
+package org.rambots.config
+
+import com.pathplanner.lib.util.PIDConstants
+
+object ElevatorConstants {
+
+    const val ELEVATOR_LEADER_ID = 15
+    const val ELEVATOR_FOLLOWER_ID = 16
+
+    val ELEVATOR_PID = PIDConstants(0.2, 0.0, 0.0)
+
+}
diff --git a/src/main/java/org/rambots/subsystems/ArmSubsystem.kt b/src/main/java/org/rambots/subsystems/ArmSubsystem.kt
@@ -0,0 +1,50 @@
+package org.rambots.subsystems
+
+import com.revrobotics.CANSparkBase
+import com.revrobotics.CANSparkLowLevel
+import com.revrobotics.CANSparkMax
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.Logger
+import org.rambots.config.ArmConstants.ARM_NEO_FOLLOWER_ID
+import org.rambots.config.ArmConstants.ARM_NEO_LEADER_ID
+import org.rambots.config.ArmConstants.ARM_PID
+
+object ArmSubsystem : SubsystemBase() {
+
+    var offset = 0.0
+    var desiredPosition = 0.0 + offset
+    var position
+        get() = leader.encoder.position
+        set(value) {
+            leader.pidController.setReference(value, CANSparkBase.ControlType.kPosition)
+        }
+
+    private val leader = CANSparkMax(ARM_NEO_LEADER_ID, CANSparkLowLevel.MotorType.kBrushless).apply {
+        restoreFactoryDefaults()
+
+        idleMode = CANSparkBase.IdleMode.kBrake
+        pidController.apply {
+            p = ARM_PID.kP
+            i = ARM_PID.kI
+            d = ARM_PID.kD
+        }
+
+        setSmartCurrentLimit(40)
+    }
+
+    private val follower = CANSparkMax(ARM_NEO_FOLLOWER_ID, CANSparkLowLevel.MotorType.kBrushless)
+
+    init {
+        follower.restoreFactoryDefaults()
+        follower.follow(leader, true)
+    }
+
+    override fun periodic() {
+        Logger.recordOutput("Arm/DesiredPosition", desiredPosition)
+        Logger.recordOutput("Arm/Offset", offset)
+        Logger.recordOutput("Arm/Position", leader.encoder.position)
+        Logger.recordOutput("Arm/Velocity", leader.encoder.velocity)
+        Logger.recordOutput("Arm/Current", leader.outputCurrent)
+    }
+
+}
diff --git a/src/main/java/org/rambots/subsystems/ElevatorSubsystem.kt b/src/main/java/org/rambots/subsystems/ElevatorSubsystem.kt
@@ -0,0 +1,53 @@
+package org.rambots.subsystems
+
+import com.revrobotics.CANSparkBase
+import com.revrobotics.CANSparkLowLevel
+import com.revrobotics.CANSparkMax
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.Logger
+import org.rambots.config.ArmConstants.ARM_NEO_FOLLOWER_ID
+import org.rambots.config.ArmConstants.ARM_NEO_LEADER_ID
+import org.rambots.config.ArmConstants.ARM_PID
+import org.rambots.config.ElevatorConstants.ELEVATOR_FOLLOWER_ID
+import org.rambots.config.ElevatorConstants.ELEVATOR_LEADER_ID
+import org.rambots.config.ElevatorConstants.ELEVATOR_PID
+
+object ElevatorSubsystem : SubsystemBase() {
+
+    var offset = 0.0
+    var desiredPosition = 0.0 + offset
+    var position
+        get() = leader.encoder.position
+        set(value) {
+            leader.pidController.setReference(value, CANSparkBase.ControlType.kPosition)
+        }
+
+    private val leader = CANSparkMax(ELEVATOR_LEADER_ID, CANSparkLowLevel.MotorType.kBrushless).apply {
+        restoreFactoryDefaults()
+
+        idleMode = CANSparkBase.IdleMode.kBrake
+        pidController.apply {
+            p = ELEVATOR_PID.kP
+            i = ELEVATOR_PID.kI
+            d = ELEVATOR_PID.kD
+        }
+
+        setSmartCurrentLimit(40)
+    }
+
+    private val follower = CANSparkMax(ELEVATOR_FOLLOWER_ID, CANSparkLowLevel.MotorType.kBrushless)
+
+    init {
+        follower.restoreFactoryDefaults()
+        follower.follow(leader, true)
+    }
+
+    override fun periodic() {
+        Logger.recordOutput("Elevator/DesiredPosition", desiredPosition)
+        Logger.recordOutput("Elevator/Offset", offset)
+        Logger.recordOutput("Elevator/Position", leader.encoder.position)
+        Logger.recordOutput("Elevator/Velocity", leader.encoder.velocity)
+        Logger.recordOutput("Elevator/Current", leader.outputCurrent)
+    }
+
+}
diff --git a/src/main/java/org/rambots/util/CommandUtils.kt b/src/main/java/org/rambots/util/CommandUtils.kt
@@ -0,0 +1,7 @@
+package org.rambots.util
+
+object CommandUtils {
+
+    fun generateErrorRange(value: Double, error: Double): ClosedRange<Double> = (value - error)..(value + error)
+
+}