bobchanges

team4099 · Oct 24, 2023 · f5f3b24 · f5f3b24
1 parent 91f4bc2
commit f5f3b24
Show file tree

Hide file tree

Showing 13 changed files with 47 additions and 22 deletions.
diff --git a/src/main/kotlin/com/team4099/robot2023/Robot.kt b/src/main/kotlin/com/team4099/robot2023/Robot.kt
@@ -148,6 +148,7 @@ object Robot : LoggedRobot() {
     FMSData.allianceColor = DriverStation.getAlliance()
     RobotContainer.setDriveBrakeMode()
     RobotContainer.setSteeringBrakeMode()
+    RobotContainer.requestSuperstructureIdle()
     RobotContainer.getAutonomousCommand().schedule()
   }
 

diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
@@ -78,9 +78,9 @@ object RobotContainer {
         )
       superstructure =
         Superstructure(
-          Elevator(object: ElevatorIO {}),
-          GroundIntake(object: GroundIntakeIO {}),
-          Manipulator(object: ManipulatorIO {}),
+          Elevator(ElevatorIONeo),
+          GroundIntake(GroundIntakeIONeo),
+          Manipulator(ManipulatorIONeo),
           Led(object : LedIO {}),
           GameBoy(GameboyIOServer)
         )
@@ -140,6 +140,10 @@ object RobotContainer {
     superstructure.currentRequest = Request.SuperstructureRequest.Idle()
   }
 
+  fun requestSuperstructureHome() {
+    superstructure.currentRequest = Request.SuperstructureRequest.Home()
+  }
+
   fun zeroArm() {
     superstructure.groundIntakeZeroArm()
   }

diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/ConeCubeHoldBumpAuto.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/ConeCubeHoldBumpAuto.kt
@@ -51,7 +51,7 @@ class ConeCubeHoldBumpAuto(val drivetrain: Drivetrain, val superstructure: Super
               // pick up cube
               Waypoint(
                 Translation2d(
-                  FieldConstants.StagingLocations.translations[0]!!.x,
+                  FieldConstants.StagingLocations.translations[0]!!.x + 1.5.meters,
                   FieldConstants.StagingLocations.translations[0]!!.y
                 )
                   .translation2d,

diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt
@@ -67,8 +67,8 @@ object ElevatorConstants {
   // circumference / 2pi = radius
   val SPOOL_RADIUS = 0.005.meters * 32.0 / (2 * PI)
 
-  val MAX_VELOCITY = 140.inches.perSecond // 75
-  val MAX_ACCELERATION = 415.inches.perSecond.perSecond // 225
+  val MAX_VELOCITY = 100.inches.perSecond // 75
+  val MAX_ACCELERATION = 300.inches.perSecond.perSecond // 225
 
   val ENABLE_ELEVATOR = 1.0
 
@@ -86,7 +86,7 @@ object ElevatorConstants {
   // TODO(do tests to figure out what these values should be)
   val CUBE_DROP_POSITION_DELTA = 3.2.inches
   val CONE_DROP_POSITION_DELTA = 0.0.inches
-  val GROUND_INTAKE_CUBE_HEIGHT = 6.0.inches
+  val GROUND_INTAKE_CUBE_HEIGHT = 8.5.inches
   val DOUBLE_SUBSTATION_CUBE_OFFSET = 0.0.inches
   val DOUBLE_SUBSTATION_CONE_OFFSET = 11.0.inches
   val SINGLE_SUBSTATION_CUBE_OFFSET = 0.0.inches

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIOReal.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIOReal.kt
@@ -39,9 +39,6 @@ object DrivetrainIOReal : DrivetrainIO {
         )
       ),
       SwerveModule(
-        //        object: SwerveModuleIO {
-        //          override val label: String = Constants.Drivetrain.BACK_RIGHT_MODULE_NAME
-        //        }
         SwerveModuleIOFalcon(
           TalonFX(Constants.Drivetrain.BACK_RIGHT_STEERING_ID, CANIVORE_NAME),
           TalonFX(Constants.Drivetrain.BACK_RIGHT_DRIVE_ID, CANIVORE_NAME),

diff --git a/.../kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt b/.../kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt
@@ -135,6 +135,7 @@ class SwerveModuleIOFalcon(
 
     driveFalcon.setNeutralMode(NeutralMode.Brake)
 
+    /*
     MotorChecker.add(
       "Drivetrain",
       "Drive",
@@ -158,11 +159,12 @@ class SwerveModuleIOFalcon(
         110.celsius
       )
     )
+    */
   }
 
   override fun updateInputs(inputs: SwerveModuleIO.SwerveModuleIOInputs) {
-    inputs.driveAppliedVoltage = driveFalcon.motorOutputVoltage.volts
-    inputs.steeringAppliedVoltage = steeringFalcon.motorOutputVoltage.volts
+    inputs.driveAppliedVoltage = driveFalcon.motorOutputVoltage.volts //no
+    inputs.steeringAppliedVoltage = steeringFalcon.motorOutputVoltage.volts //no
 
     inputs.driveStatorCurrent = driveFalcon.statorCurrent.amps
     inputs.driveSupplyCurrent = driveFalcon.supplyCurrent.amps
@@ -175,13 +177,14 @@ class SwerveModuleIOFalcon(
     inputs.driveVelocity = driveSensor.velocity
     inputs.steeringVelocity = steeringSensor.velocity
 
-    inputs.driveTemp = driveFalcon.temperature.celsius
-    inputs.steeringTemp = steeringFalcon.temperature.celsius
+    inputs.driveTemp = driveFalcon.temperature.celsius  //no
+    inputs.steeringTemp = steeringFalcon.temperature.celsius //no
 
     inputs.potentiometerOutputRaw =
       potentiometer.voltage / RobotController.getVoltage5V() * 2.0 * Math.PI
     inputs.potentiometerOutputRadians = potentiometerOutput.radians
 
+
     Logger.getInstance()
       .recordOutput(
         "$label/potentiometerRadiansWithOffset",

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt
@@ -413,6 +413,9 @@ class Elevator(val io: ElevatorIO) {
     Logger.getInstance()
       .recordOutput("Elevator/currentRequest", currentRequest.javaClass.simpleName)
 
+    Logger.getInstance()
+      .recordOutput("Elevator/lastHomingStatorCurrentTripTime", lastHomingStatorCurrentTripTime.inSeconds)
+
     if (Constants.Tuning.DEBUGING_MODE) {
       Logger.getInstance()
         .recordOutput(

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt
@@ -41,7 +41,7 @@ interface ElevatorIO {
     var leaderRawPosition = 0.0
     var followerRawPosition = 0.0
 
-    var isSimulating = false
+    var isSimulating = true
 
     override fun toLog(table: LogTable) {
       table?.put("elevatorPositionInches", elevatorPosition.inInches)

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONeo.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONeo.kt
@@ -77,6 +77,7 @@ object ElevatorIONeo : ElevatorIO {
     leaderSparkMax.burnFlash()
     followerSparkMax.burnFlash()
 
+    /*
     MotorChecker.add(
       "Elevator",
       "Extension",
@@ -91,6 +92,7 @@ object ElevatorIONeo : ElevatorIO {
         90.celsius
       ),
     )
+     */
   }
 
   override fun updateInputs(inputs: ElevatorIO.ElevatorInputs) {
@@ -139,7 +141,7 @@ object ElevatorIONeo : ElevatorIO {
    */
   override fun setOutputVoltage(voltage: ElectricalPotential) {
     // divide by 2 cause full power elevator is scary
-    leaderSparkMax.setVoltage(voltage.inVolts)
+    leaderSparkMax.set(voltage.inVolts / 12)
   }
 
   /**

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/groundintake/GroundIntakeIONeo.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/groundintake/GroundIntakeIONeo.kt
@@ -108,17 +108,19 @@ object GroundIntakeIONeo : GroundIntakeIO {
 
     armSparkMax.burnFlash()
 
+    /*
     MotorChecker.add(
       "Ground Intake",
       "Roller",
       MotorCollection(
         mutableListOf(Neo(rollerSparkMax, "Roller Motor")),
         GroundIntakeConstants.ROLLER_CURRENT_LIMIT,
         70.celsius,
-        GroundIntakeConstants.ROLLER_CURRENT_LIMIT - 0.amps,
+        GroundIntakeConstants.ROLLER_CURRENT_LIMIT - 5.amps,
         90.celsius
       ),
     )
+    */
 
     MotorChecker.add(
       "Ground Intake",

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/manipulator/ManipulatorIONeo.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/manipulator/ManipulatorIONeo.kt
@@ -78,17 +78,19 @@ object ManipulatorIONeo : ManipulatorIO {
     armSparkMax.idleMode = CANSparkMax.IdleMode.kBrake
     armSparkMax.burnFlash()
 
+    /*
     MotorChecker.add(
       "Manipulator",
       "ExtensionMotor",
       MotorCollection(
         mutableListOf(Neo(armSparkMax, "Extension Motor")),
         ManipulatorConstants.ARM_STATOR_CURRENT_LIMIT,
         70.celsius,
-        ManipulatorConstants.ARM_STATOR_CURRENT_LIMIT - 30.amps,
+        ManipulatorConstants.ARM_STATOR_CURRENT_LIMIT - 10.amps,
         90.celsius
       )
     )
+     */
 
     MotorChecker.add(
       "Manipulator",

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt
@@ -233,6 +233,13 @@ class Superstructure(
           }
         }
 
+        if (elevator.currentRequest is Request.ElevatorRequest.Home){
+          elevator.currentRequest =
+            Request.ElevatorRequest.TargetingPosition(
+              3.inches
+            )
+        }
+
         // Outputs
         val rollerVoltage =
           when (theoreticalGamePiece) {
@@ -282,7 +289,7 @@ class Superstructure(
             if (manipulator.hasCube){
               elevator.currentRequest =
                 Request.ElevatorRequest.TargetingPosition(
-                  4.inches
+                  6.inches
                 )
             } else {
               elevator.currentRequest =
@@ -299,6 +306,10 @@ class Superstructure(
             Request.ManipulatorRequest.TargetingPosition(
               manipulator.armPositionTarget, rollerVoltage
             )
+          elevator.currentRequest =
+            Request.ElevatorRequest.TargetingPosition(
+              elevator.elevatorPositionTarget
+            )
         }
 
         // Transition

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt
@@ -111,9 +111,9 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() {
             // Logger.getInstance().recordOutput("Vision/${VisionConstants.CAMERA_NAMES[instance]}_transform", cameraPose.relativeTo(tuningPosition.toPose3d()).pose3d)
 
             robotPose = cameraPose.transformBy(cameraPoses[instance].inverse()).toPose2d()
-            println(
-              "CameraPoseX: ${cameraPose.x}, transformX: ${cameraPoses[instance].x}, robotPoseX: ${robotPose.x}"
-            )
+//            println(
+//              "CameraPoseX: ${cameraPose.x}, transformX: ${cameraPoses[instance].x}, robotPoseX: ${robotPose.x}"
+//            )
           }
           2.0 -> {
             val error0 = values[1]