increased speeds of all kinds

DurhamAcademy · Mar 30, 2023 · 6bcdfba · 6bcdfba
1 parent b5ebbbe
commit 6bcdfba
Show file tree

Hide file tree

Showing 7 changed files with 39 additions and 19 deletions.
diff --git a/src/main/kotlin/frc/robot/PhotonCameraWrapper.kt b/src/main/kotlin/frc/robot/PhotonCameraWrapper.kt
@@ -17,7 +17,7 @@ class PhotonCameraWrapper {
     var validPoseCount = 0UL
 
     val canTrustPose: Boolean
-        get() = validPoseCount > 100UL
+        get() = validPoseCount.toDouble() > 100UL.toDouble()
 
     val percentage: Double
         get() = validPoseCount.toDouble() / 100.0

diff --git a/src/main/kotlin/frc/robot/RobotContainer.kt b/src/main/kotlin/frc/robot/RobotContainer.kt
@@ -18,6 +18,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard
 import edu.wpi.first.wpilibj2.command.Command
 import edu.wpi.first.wpilibj2.command.Commands
+import edu.wpi.first.wpilibj2.command.InstantCommand
 import frc.kyberlib.command.Game
 import frc.kyberlib.lighting.KLEDRegion
 import frc.kyberlib.lighting.KLEDStrip
@@ -212,6 +213,16 @@ class RobotContainer {
 //                                )
 //                            )
                     )
+
+                ledColor
+                    .onTrue(InstantCommand({
+                        wantedObject = when (wantedObject) {
+                            cone -> cube
+                            cube -> cone
+                            else -> cone
+                        }
+                    }))
+
                 alignClosestHPS
                     .whileTrue(
                         goToHumanPlayerStation(
@@ -344,10 +355,10 @@ class RobotContainer {
     var fullAuto = false
     var fullAutoObject: GamePiece = none
 
-    val wantedObject: GamePiece
+    var wantedObject: GamePiece = none
         get() =
             if (fullAuto) fullAutoObject
-            else smartDashboardSelector.placementSide.asObject
+            else field
 
     val leds = KLEDStrip(9, count).apply {
         val coral = Color(255, 93, 115)
@@ -448,7 +459,7 @@ class RobotContainer {
             return@AnimationCustom List<Color>(len) { i ->
                 if (i >= index) color else Color.black
             }
-        }, { !drivetrain.canTrustPose && lightStatus != TeleopFMSRed || lightStatus != TeleopFMSBlue })
+        }, { false && !drivetrain.canTrustPose && (lightStatus != TeleopFMSRed || lightStatus != TeleopFMSBlue) })
 
 
         val body = KLEDRegion(

diff --git a/src/main/kotlin/frc/robot/commands/pathing/MoveToPosition.kt b/src/main/kotlin/frc/robot/commands/pathing/MoveToPosition.kt
@@ -8,7 +8,6 @@ import edu.wpi.first.math.geometry.Transform2d
 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.Timer
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard
 import edu.wpi.first.wpilibj2.command.Command
 import edu.wpi.first.wpilibj2.command.CommandBase
@@ -103,7 +102,7 @@ open class MoveToPosition(
     private val xPIDController = ProfiledPIDController(
         Companion.xP, 0.0, 0.05, TrapezoidProfile.Constraints(
             maxPosSpeed,
-            max(10.0, drivetrainConstants.maxAcceleration)
+            max(drivetrainConstants.maxAutonomousVelocity, drivetrainConstants.maxAutonomousAcceleration)
         )
     ).also {
         it.reset(drivetrain.estimatedPose2d.translation.x, 0.0)
@@ -112,15 +111,16 @@ open class MoveToPosition(
     private val yPIDController = ProfiledPIDController(
         Companion.yP, 0.0, 0.05, TrapezoidProfile.Constraints(
             maxPosSpeed,
-            max(10.0, drivetrainConstants.maxAcceleration)
+            max(drivetrainConstants.maxAutonomousVelocity, drivetrainConstants.maxAutonomousAcceleration)
         )
     ).also {
         it.reset(drivetrain.estimatedPose2d.translation.y, 0.0)
         it.setTolerance(toleranceppos, tolerancepvel)
     }
     private val rPIDController = ProfiledPIDController(
         Companion.rP, 0.0, 0.0, TrapezoidProfile.Constraints(
-            maxRotSpeed, max(PI, drivetrainConstants.maxAngularAcceleration)
+            maxRotSpeed,
+            max(drivetrainConstants.maxAutonomousAngularVelocity, drivetrainConstants.maxAutonomousAngularAcceleration)
         )
     ).also {
         it.enableContinuousInput(-PI, PI)

diff --git a/src/main/kotlin/frc/robot/commands/pathing/building/blocks/BuildingBlocks.kt b/src/main/kotlin/frc/robot/commands/pathing/building/blocks/BuildingBlocks.kt
@@ -212,15 +212,15 @@ object BuildingBlocks {
                 if (drivetrain.estimatedPose2d.y < clearDown + .25 && (drivetrain.estimatedPose2d.x - (xCenter + (4.27 * -alliance().xMul))).absoluteValue < (robotLength / 2.0) + 0.25) {
                     xPID.setConstraints(
                         TrapezoidProfile.Constraints(
-                            0.2,
-                            drivetrainConstraints.maxAcceleration
+                            drivetrainConstraints.maxAutonomousVelocity,
+                            drivetrainConstraints.maxAutonomousAcceleration
                         )
                     )
                 } else {
                     xPID.setConstraints(
                         TrapezoidProfile.Constraints(
-                            10.0,
-                            drivetrainConstraints.maxAcceleration
+                            drivetrainConstraints.maxAutonomousVelocity,
+                            drivetrainConstraints.maxAutonomousAcceleration
                         )
                     )
                 }
@@ -363,15 +363,15 @@ object BuildingBlocks {
                 else if (arm.armPosition.absoluteValue > 0.5) {
                     rotPid.setConstraints(
                         TrapezoidProfile.Constraints(
-                            PI / 2,
-                            drivetrainConstraints.maxAngularAcceleration * 0.25
+                            drivetrainConstraints.maxAutonomousAngularVelocity,
+                            drivetrainConstraints.maxAutonomousAngularAcceleration
                         )
                     )
                 } else {
                     rotPid.setConstraints(
                         TrapezoidProfile.Constraints(
-                            PI / 2,
-                            drivetrainConstraints.maxAcceleration
+                            drivetrainConstraints.maxAutonomousAngularVelocity,
+                            drivetrainConstraints.maxAutonomousAcceleration
                         )
                     )
                 }

diff --git a/src/main/kotlin/frc/robot/constants/drivetrain.kt b/src/main/kotlin/frc/robot/constants/drivetrain.kt
@@ -3,6 +3,15 @@ package frc.robot.constants
 import kotlin.math.PI
 
 object drivetrain {
-    const val maxAcceleration = 6.0
+    const val maxAcceleration = 12.0
     const val maxAngularAcceleration = 3.0 * 2 * PI
+
+    const val maxVelocity = 3.0
+    const val maxAngularVelocity = 2.0 * 2 * PI
+
+    const val maxAutonomousVelocity = maxVelocity
+    const val maxAutonomousAngularVelocity = maxAngularVelocity
+
+    const val maxAutonomousAcceleration = maxAcceleration * .75
+    const val maxAutonomousAngularAcceleration = maxAngularAcceleration * .75
 }
diff --git a/src/main/kotlin/frc/robot/constants/leds.kt b/src/main/kotlin/frc/robot/constants/leds.kt
@@ -1,5 +1,5 @@
 package frc.robot.constants
 
 object leds {
-    val count = 74// 74 is what chris said
+    val count = 78// 74 is what chris said
 }
diff --git a/src/main/kotlin/frc/robot/controls/BryanControlScheme.kt b/src/main/kotlin/frc/robot/controls/BryanControlScheme.kt
@@ -18,7 +18,7 @@ class BryanControlScheme(
     override val placeLvl2 = xbox.b()
     override val placeLvl3 = xbox.y()
 
-//    override val ledColor = xbox.povLeft()
+    override val ledColor = xbox.povLeft()
 
     //    override val stopIntake = xbox.povUp()
     override val throwObject: Trigger = xbox.rightBumper()