Merge pull request #4 from blair-robot-project/autoAim

simgui.json

@@ -4,9 +4,6 @@
       "0": {
         "columns": 2,
         "order": 1
-      },
-      "window": {
-        "visible": true
       }
     }
   },
@@ -125,15 +122,7 @@
         "left": 150,
         "right": 2961,
         "top": 79,
-        "width": 16.541748046875,
-        "window": {
-          "visible": true
-        }
-      },
-      "/SmartDashboard/Routine Chooser": {
-        "window": {
-          "visible": true
-        }
+        "width": 16.541748046875
       },
       "/SmartDashboard/VisionSystemSim-main/Sim Field": {
         "EstimatedRobot": {
@@ -316,7 +305,6 @@
               "open": true
             }
           },
-          "open": true,
           "undertaker": {
             "open": true
           }

src/main/kotlin/frc/team449/RobotLoop.kt

@@ -15,6 +15,7 @@ import frc.team449.robot2024.commands.PivotCalibration
 import frc.team449.robot2024.commands.light.BlairChasing
 import frc.team449.robot2024.commands.light.BreatheHue
 import frc.team449.robot2024.commands.light.Rainbow
+import frc.team449.robot2024.constants.field.FieldConstants
 import frc.team449.robot2024.constants.vision.VisionConstants
 import frc.team449.robot2024.subsystems.ControllerBindings
 import monologue.Annotations.Log
@@ -83,7 +84,7 @@ class RobotLoop : TimedRobot(), Logged {
   }
 
   override fun autonomousInit() {
-    /** Every time auto starts, we update the chosen auto command */
+    /** Every time auto starts, we update the chosen auto command. */
     this.autoCommand = routineMap[routineChooser.selected]
     CommandScheduler.getInstance().schedule(this.autoCommand)
 
@@ -92,6 +93,12 @@ class RobotLoop : TimedRobot(), Logged {
     } else {
       BreatheHue(robot.light, 95).schedule()
     }
+
+    if (DriverStation.getAlliance().getOrNull() == DriverStation.Alliance.Red) {
+      FieldConstants.SPEAKER_POSE = FieldConstants.RED_SPEAKER_POSE
+    } else if (DriverStation.getAlliance().getOrNull() == DriverStation.Alliance.Blue) {
+      FieldConstants.SPEAKER_POSE = FieldConstants.BLUE_SPEAKER_POSE
+    }
   }
 
   override fun autonomousPeriodic() {}
@@ -104,6 +111,12 @@ class RobotLoop : TimedRobot(), Logged {
     (robot.light.currentCommand ?: InstantCommand()).cancel()
 
     robot.drive.defaultCommand = robot.driveCommand
+
+    if (DriverStation.getAlliance().getOrNull() == DriverStation.Alliance.Red) {
+      FieldConstants.SPEAKER_POSE = FieldConstants.RED_SPEAKER_POSE
+    } else if (DriverStation.getAlliance().getOrNull() == DriverStation.Alliance.Blue) {
+      FieldConstants.SPEAKER_POSE = FieldConstants.BLUE_SPEAKER_POSE
+    }
   }
 
   override fun teleopPeriodic() {

src/main/kotlin/frc/team449/control/holonomic/SwerveOrthogonalCommand.kt

@@ -75,7 +75,7 @@ class SwerveOrthogonalCommand(
     atGoal = true
   }
 
-  private fun snapToAngle(angle: Double) {
+  fun snapToAngle(angle: Double) {
     val desAngle = MathUtil.angleModulus(angle + allianceCompensation.invoke())
     if (abs(desAngle - drive.heading.radians) > RobotConstants.SNAP_TO_ANGLE_TOLERANCE_RAD &&
       abs(desAngle - drive.heading.radians) < 2 * PI - RobotConstants.SNAP_TO_ANGLE_TOLERANCE_RAD

src/main/kotlin/frc/team449/robot2024/commands/AutoAim.kt

@@ -0,0 +1,63 @@
+package frc.team449.robot2024.commands
+
+import edu.wpi.first.wpilibj2.command.Command
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup
+import edu.wpi.first.wpilibj2.command.WaitUntilCommand
+import frc.team449.robot2024.Robot
+import frc.team449.robot2024.commands.driveAlign.OrbitAlign
+import frc.team449.robot2024.constants.field.FieldConstants
+import frc.team449.robot2024.constants.subsystem.SpinShooterConstants
+import kotlin.math.abs
+
+class AutoAim(
+  private val robot: Robot
+) : Command() {
+
+  private val shootingMap = SpinShooterConstants.SHOOTING_MAP
+
+  override fun initialize() {
+    if (getDistanceToSpeaker() > SpinShooterConstants.MAX_RANGE) {
+      this.cancel()
+    }
+
+    addRequirements(robot.shooter, robot.pivot)
+  }
+
+  override fun execute() {
+    robot.shooter.shootSubwoofer()
+    robot.pivot.moveToAngle(shootingMap.get(getDistanceToSpeaker()))
+
+
+
+    val fieldToRobot = robot.drive.pose.translation
+    val robotToPoint = FieldConstants.SPEAKER_POSE - fieldToRobot
+    robot.driveCommand.snapToAngle(robotToPoint.angle.radians)
+
+    if (robot.mechController.().asBoolean &&
+      abs(robot.drive.currentSpeeds.vxMetersPerSecond) <= 0.1 && abs(robot.drive.currentSpeeds.vyMetersPerSecond) <= 0.1 && abs(robot.drive.currentSpeeds.omegaRadiansPerSecond) <= 0.1
+    ) {
+      SequentialCommandGroup(
+        WaitUntilCommand { robot.shooter.atSetpoint() },
+        robot.feeder.intake(),
+        robot.undertaker.intake()
+      ).alongWith(
+        robot.shooter.shootSubwoofer()
+      ).schedule()
+    }
+  }
+
+  override fun isFinished(): Boolean {
+    return getDistanceToSpeaker() > SpinShooterConstants.MAX_RANGE
+  }
+
+  override fun end(interrupted: Boolean) {
+    robot.undertaker.stop()
+    robot.shooter.rampStop()
+    robot.feeder.stop()
+    robot.pivot.moveStow()
+  }
+
+  private fun getDistanceToSpeaker(): Double {
+    return abs(FieldConstants.SPEAKER_POSE.getDistance(robot.drive.pose.translation))
+  }
+}

src/main/kotlin/frc/team449/robot2024/commands/driveAlign/OrbitAlign.kt

@@ -1,14 +1,15 @@
 package frc.team449.robot2024.commands.driveAlign
 
 import edu.wpi.first.math.MathUtil
-import edu.wpi.first.math.controller.PIDController
+import edu.wpi.first.math.controller.ProfiledPIDController
 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.trajectory.TrapezoidProfile
 import edu.wpi.first.wpilibj.DriverStation
 import edu.wpi.first.wpilibj.Timer
-import edu.wpi.first.wpilibj.XboxController
 import edu.wpi.first.wpilibj2.command.Command
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController
 import frc.team449.control.holonomic.SwerveDrive
 import frc.team449.robot2024.constants.RobotConstants
 import frc.team449.robot2024.constants.auto.AutoConstants
@@ -23,12 +24,13 @@ import kotlin.math.*
  */
 class OrbitAlign(
   private val drive: SwerveDrive,
-  private val controller: XboxController,
-  private val point: Translation2d,
-  private val headingPID: PIDController = PIDController(
+  private val controller: CommandXboxController,
+  private val point: () -> Translation2d,
+  private val headingPID: ProfiledPIDController = ProfiledPIDController(
     AutoConstants.ORBIT_KP,
     0.0,
-    0.0
+    AutoConstants.ORBIT_KD,
+    TrapezoidProfile.Constraints(AutoConstants.MAX_ROT_VEL, AutoConstants.MAX_ROT_ACCEL)
   ),
   tolerance: Double = 0.015
 ) : Command() {
@@ -82,8 +84,8 @@ class OrbitAlign(
 
   override fun execute() {
     fieldToRobot = drive.pose.translation
-    robotToPoint = point - fieldToRobot
-    headingPID.setpoint = robotToPoint.angle.radians
+    robotToPoint = point.invoke() - fieldToRobot
+    headingPID.goal = TrapezoidProfile.State(robotToPoint.angle.radians, 0.0)
 
     val currTime = timer.get()
     dt = currTime - prevTime

src/main/kotlin/frc/team449/robot2024/constants/auto/AutoConstants.kt

@@ -6,10 +6,14 @@ object AutoConstants {
   const val DEFAULT_Y_KP = 2.55
   const val DEFAULT_X_KD = 0.05
   const val DEFAULT_Y_KD = 0.05
-  const val DEFAULT_ROTATION_KP = 2.45
+  const val DEFAULT_ROTATION_KP = 2.45 // 2.45
   const val DEFAULT_ROTATION_KD = 0.05
 
-  const val ORBIT_KP = 2.0
+  const val ORBIT_KP = 5.0
+  const val ORBIT_KD = 0.00
+
+  const val MAX_ROT_VEL = 10.0 // rad/s
+  const val MAX_ROT_ACCEL = 10.0 // rad/s
 
   const val SHOOT_INTAKE_TIME = 0.35
 

src/main/kotlin/frc/team449/robot2024/constants/field/FieldConstants.kt

@@ -1,11 +1,15 @@
 package frc.team449.robot2024.constants.field
 
 import edu.wpi.first.math.geometry.Translation2d
+import edu.wpi.first.math.util.Units
 
 object FieldConstants {
   const val fieldLength = 16.54175
   const val fieldWidth = 8.21055
 
   /** Find these constants in meters for the blue alliance */
-  val SUBWOOFER_POSE = Translation2d(1.0, 4.0)
+  val BLUE_SPEAKER_POSE = Translation2d(Units.inchesToMeters(-1.5), Units.inchesToMeters(218.42))
+  val RED_SPEAKER_POSE = Translation2d(Units.inchesToMeters(652.73), Units.inchesToMeters(218.42))
+
+  var SPEAKER_POSE = BLUE_SPEAKER_POSE
 }

src/main/kotlin/frc/team449/robot2024/constants/subsystem/SpinShooterConstants.kt

@@ -1,10 +1,6 @@
 package frc.team449.robot2024.constants.subsystem
 
-import edu.wpi.first.math.InterpolatingMatrixTreeMap
-import edu.wpi.first.math.MatBuilder
-import edu.wpi.first.math.Nat
-import edu.wpi.first.math.numbers.N1
-import edu.wpi.first.math.numbers.N3
+import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap
 import edu.wpi.first.math.util.Units
 import kotlin.math.PI
 
@@ -28,7 +24,8 @@ object SpinShooterConstants {
 
   val BRAKE_RATE_LIMIT = Units.rotationsPerMinuteToRadiansPerSecond(3750.0)
 
-  val SHOOTING_MAP = InterpolatingMatrixTreeMap<Double, N3, N1>()
+  val SHOOTING_MAP = InterpolatingDoubleTreeMap()
+  const val MAX_RANGE = 10.0 // m
 
   const val LEFT_KS = 0.051294
   const val RIGHT_KS = 0.067149
@@ -74,24 +71,9 @@ object SpinShooterConstants {
 
   init {
     /**
-     * Fill with values of optimized left/right and pivot angles
-     *  for a given distance to the Speaker
-     *
-     * It may be better to mathematically calculate pivot angle,
-     *  this is something to test
-     *
-     * Data is entered as following:
-     *  Right shooter speed, left shooter speed, pivot angle
+     * Data is entered as follows:
+     * SHOOTING_MAP.put(distanceToSpeaker, pivotAngle)
      */
-    SHOOTING_MAP.put(
-      0.0,
-      MatBuilder.fill(
-        Nat.N3(),
-        Nat.N1(),
-        0.0,
-        0.0,
-        0.0
-      )
-    )
+    SHOOTING_MAP.put(1.0, 1.0)
   }
 }

src/main/kotlin/frc/team449/robot2024/subsystems/ControllerBindings.kt

@@ -16,6 +16,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism
 import frc.team449.control.holonomic.SwerveSim
 import frc.team449.robot2024.Robot
+import frc.team449.robot2024.commands.AutoAim
 import frc.team449.robot2024.commands.driveAlign.OrbitAlign
 import frc.team449.robot2024.constants.RobotConstants
 import frc.team449.robot2024.constants.field.FieldConstants
@@ -85,8 +86,8 @@ class ControllerBindings(
 
   val orbitCmd = OrbitAlign(
     robot.drive,
-    robot.driveController.hid,
-    FieldConstants.SUBWOOFER_POSE
+    robot.driveController,
+    { FieldConstants.SPEAKER_POSE }
   )
 
   private fun stopAll(): Command {
@@ -243,6 +244,10 @@ class ControllerBindings(
       stopAll()
     )
 
+    driveController.y().onTrue(
+      AutoAim(robot)
+    )
+
 //    /** Characterization */
 //    // Quasistatic Forwards
 //    driveController.povUp().onTrue(

src/main/kotlin/frc/team449/robot2024/subsystems/pivot/Pivot.kt

@@ -197,8 +197,8 @@ open class Pivot(
 
   fun pivotShootAnywhere(): Command {
     return this.run {
-      val distance = FieldConstants.SUBWOOFER_POSE.getDistance(robot.drive.pose.translation)
-      val goal = SpinShooterConstants.SHOOTING_MAP.get(distance).get(2, 0)
+      val distance = FieldConstants.SPEAKER_POSE.getDistance(robot.drive.pose.translation)
+      val goal = SpinShooterConstants.SHOOTING_MAP.get(distance)
 
       correctAndPredict()
       motor.setVoltage(getVoltage())
@@ -207,15 +207,15 @@ open class Pivot(
     }
   }
 
-  private fun moveToAngleSlow(goal: Double) {
+  fun moveToAngleSlow(goal: Double) {
     lastProfileReference = slowProfile.calculate(RobotConstants.LOOP_TIME, lastProfileReference, TrapezoidProfile.State(goal, 0.0))
 
     correctAndPredict()
 
     motor.setVoltage(getVoltage())
   }
 
-  private fun moveToAngle(goal: Double) {
+  fun moveToAngle(goal: Double) {
     lastProfileReference = profile.calculate(RobotConstants.LOOP_TIME, lastProfileReference, TrapezoidProfile.State(goal, 0.0))
 
     correctAndPredict()

src/main/kotlin/frc/team449/robot2024/subsystems/shooter/Shooter.kt

@@ -152,7 +152,7 @@ open class Shooter(
     return cmd
   }
 
-  private fun shootPiece(speed: Double) {
+  fun shootPiece(speed: Double) {
     if (DriverStation.isDisabled()) {
       correct()
     } else {

src/main/kotlin/frc/team449/robot2024/subsystems/shooter/SpinShooter.kt

@@ -15,7 +15,6 @@ import edu.wpi.first.wpilibj.RobotBase
 import edu.wpi.first.wpilibj2.command.*
 import frc.team449.robot2024.Robot
 import frc.team449.robot2024.constants.RobotConstants
-import frc.team449.robot2024.constants.field.FieldConstants
 import frc.team449.robot2024.constants.subsystem.SpinShooterConstants
 import frc.team449.system.encoder.NEOEncoder
 import frc.team449.system.motor.WrappedMotor
@@ -200,20 +199,20 @@ open class SpinShooter(
     return cmd
   }
 
-  fun shootAnywhere(): Command {
-    val cmd = this.run {
-      val distance = FieldConstants.SUBWOOFER_POSE.getDistance(robot.drive.pose.translation)
-
-      val rightSpeed = SpinShooterConstants.SHOOTING_MAP.get(distance).get(0, 0)
-      val leftSpeed = SpinShooterConstants.SHOOTING_MAP.get(distance).get(1, 0)
-
-      desiredVels = Pair(leftSpeed, rightSpeed)
-
-      shootPiece(rightSpeed, leftSpeed)
-    }
-    cmd.name = "shooting anywhere"
-    return cmd
-  }
+//  fun shootAnywhere(): Command {
+//    val cmd = this.run {
+//      val distance = FieldConstants.SPEAKER_POSE.getDistance(robot.drive.pose.translation)
+//
+//      val rightSpeed = SpinShooterConstants.SHOOTING_MAP.get(distance).get(0, 0)
+//      val leftSpeed = SpinShooterConstants.SHOOTING_MAP.get(distance).get(1, 0)
+//
+//      desiredVels = Pair(leftSpeed, rightSpeed)
+//
+//      shootPiece(rightSpeed, leftSpeed)
+//    }
+//    cmd.name = "shooting anywhere"
+//    return cmd
+//  }
 
   fun atSetpoint(): Boolean {
     return abs(leftVelocity.get() - desiredVels.first) < SpinShooterConstants.IN_TOLERANCE &&