Merge remote-tracking branch 'origin/dev' into dev

# Conflicts:
#	src/main/kotlin/frc/robot/RobotContainer.kt
#	src/main/kotlin/frc/robot/commands/drivetrain/Nearest180.kt

.github/workflows/dependency-checker.yml

@@ -0,0 +1,25 @@
+name: build
+on:
+  push:
+    branches:
+      - main # run the action on your projects default branch
+      #- dev
+
+jobs:
+  build:
+    name: Dependencies
+    runs-on: ubuntu-latest
+    permissions: # The Dependency Submission API requires write permission
+      contents: write
+    steps:
+      - name: 'Checkout Repository'
+        uses: actions/checkout@v3
+
+      - name: Root Gradle Dependency Submission
+        uses: mikepenz/gradle-dependency-submission@v0.8.6
+        with:
+          use-gradlew: false
+          gradle-build-module: |-
+            :
+          gradle-build-configuration: |-
+            compileClasspath

.glass/glass.json

@@ -22,22 +22,23 @@
     },
     "types": {
       "/FMSInfo": "FMSInfo",
+      "/Shuffleboard/Autonomous/Autonomous": "String Chooser",
+      "/SmartDashboard/Arm/sendable": "Subsystem",
       "/SmartDashboard/Drivetrain/sendable": "Subsystem",
       "/SmartDashboard/FIELD": "Field2d",
       "/SmartDashboard/Field": "Field2d",
-      "/SmartDashboard/elevcmd": "Subsystem"
+      "/SmartDashboard/Manipulator/sendable": "Subsystem",
+      "/SmartDashboard/elevator/sendable": "Subsystem",
+      "/SmartDashboard/elevcmd": "Subsystem",
+      "/SmartDashboard/field": "Field2d",
+      "/SmartDashboard/huhhh": "Subsystem"
     },
     "windows": {
       "/FMSInfo": {
         "window": {
           "visible": true
         }
       },
-      "/SmartDashboard/FIELD": {
-        "window": {
-          "visible": true
-        }
-      },
       "/SmartDashboard/Field": {
         "Robot": {
           "length": 0.7620000243186951,
@@ -54,6 +55,18 @@
         "window": {
           "visible": true
         }
+      },
+      "/SmartDashboard/field": {
+        "bottom": 544,
+        "height": 8.013679504394531,
+        "image": "C:\\Users\\frc\\IdeaProjects\\FRC2023\\field\\2023-field.png",
+        "left": 46,
+        "right": 1088,
+        "top": 36,
+        "width": 16.541748046875,
+        "window": {
+          "visible": true
+        }
       }
     }
   },
@@ -81,7 +94,7 @@
     }
   },
   "NetworkTables Settings": {
-    "mode": "Client (NT4)",
+    "mode": "Client (NT3)",
     "serverTeam": "6502"
   },
   "Plots": {
@@ -133,7 +146,10 @@
             }
           ]
         }
-      ]
+      ],
+      "window": {
+        "visible": false
+      }
     }
   }
 }

src/main/kotlin/frc/robot/Robot.kt

@@ -33,7 +33,7 @@ class Robot : TimedRobot() {
     }
 
     override fun teleopInit() {
-        ZeroModeMotor(robotContainer.intake).schedule()
+        ZeroModeMotor(robotContainer).schedule()
     }
 
     var auto: Command? = null

src/main/kotlin/frc/robot/commands/alltogether/PlacementValues.kt

@@ -79,10 +79,10 @@ enum class IOLevel(
         0.0, 0.0
     ),
     Balance(
-        bottomLimit,
-        Rotation2d.fromRadians(-PI / 2),
-        bottomLimit,
-        Rotation2d.fromRadians(-PI / 2),
+        bottomLimit + 0.28,
+        Rotation2d.fromRadians(-PI / 2).plus(Rotation2d.fromDegrees(-10.0)),
+        bottomLimit + 0.28,
+        Rotation2d.fromRadians(-PI / 2).plus(Rotation2d.fromDegrees(-10.0)),
         0.0, 0.0
     )
 }

src/main/kotlin/frc/robot/commands/alltogether/SetSubsystemPosition.kt

@@ -78,3 +78,5 @@ class SetSubsystemPosition(
             (elevator.height - goalElevatorPosition).absoluteValue < 0.05
 }
 
+
+

src/main/kotlin/frc/robot/commands/balance/AutoBalance.kt

@@ -50,7 +50,7 @@ class AutoBalance(private val drivetrain: Drivetrain) : CommandBase() {
                 true
             )
         } else {
-            drivetrain.drive(ChassisSpeeds(2.0 * speedMul, 0.0, 0.0), true)
+            drivetrain.drive(ChassisSpeeds(3.0 * speedMul, 0.0, 0.0), true)
         }
 
 

src/main/kotlin/frc/robot/commands/drivetrain/DriveCommand.kt

@@ -20,15 +20,15 @@ class DriveCommand(
     override fun execute() {
         drivetrain.drive(
             ChassisSpeeds(x(), y(), rotation()),
-            true,
+            isFieldOriented,
             Translation2d(0.0, 0.0)
         )
     }
 
     override fun end(interrupted: Boolean) {
         drivetrain.drive(
             ChassisSpeeds(0.0, 0.0, 0.0),
-            true,
+            isFieldOriented,
             Translation2d(0.0, 0.0)
         )
     }

src/main/kotlin/frc/robot/commands/drivetrain/DriverCommand.kt

@@ -8,7 +8,11 @@ import edu.wpi.first.math.util.Units.degreesToRadians
 import edu.wpi.first.wpilibj.DriverStation
 import edu.wpi.first.wpilibj2.command.CommandBase
 import frc.kyberlib.command.Game
+import frc.robot.commands.pathing.MoveToPosition
 import frc.robot.constants.Constants
+import frc.robot.constants.drivetrain.maxVelocity
+import frc.robot.constants.drivetrain.maxAutonomousAngularAcceleration
+import frc.robot.constants.drivetrain.maxAutonomousAngularVelocity
 import frc.robot.controls.ControlScheme
 import frc.robot.subsystems.Drivetrain
 import frc.robot.subsystems.slewLimited
@@ -20,21 +24,31 @@ class DriverCommand(
     var controlScheme: ControlScheme,
     var nearestStation: () -> Boolean,
 ) : CommandBase() {
-    private val rotationPid = ProfiledPIDController(1.0, 0.0, 0.0, TrapezoidProfile.Constraints(2*PI, 4*PI)).apply {
+    private val rotationPid = ProfiledPIDController(
+        MoveToPosition.rP, 0.0, 0.0, TrapezoidProfile.Constraints(
+            maxAutonomousAngularVelocity, maxAutonomousAngularAcceleration
+        )
+    ).apply {
         enableContinuousInput(-PI, PI)
+
     }
+    var lastNearestStation = nearestStation()
 
     init {
         addRequirements(drivetrain)
     }
 
     override fun execute() {
+        if (nearestStation() != lastNearestStation) {
+            lastNearestStation = nearestStation()
+            rotationPid.reset(drivetrain.estimatedPose2d.rotation.radians)
+        }
         val allianceMulitplier = when (Game.alliance) {
             DriverStation.Alliance.Invalid -> 1.0
             else -> Game.alliance.xMul
         }
         val vec = Translation2d(-controlScheme.forward, -controlScheme.strafe)
-            .times(3.5)
+            .times(maxVelocity)
         drivetrain.drive(
             ChassisSpeeds(
                 vec.x * Constants.powerPercent *
@@ -52,7 +66,7 @@ class DriverCommand(
                     -controlScheme.rotation * 2 * Math.PI *
                             Constants.powerPercent * .5 * (if (drivetrain.invertrot.getBoolean(false)) -1 else 1) * controlScheme.speedMutiplier
                 }
-            ).slewLimited(drivetrain.xSlewRateLimiter, drivetrain.ySlewRateLimiter, drivetrain.rotSlewRateLimiter),
+            ),//.slewLimited(drivetrain.xSlewRateLimiter, drivetrain.ySlewRateLimiter, drivetrain.rotSlewRateLimiter),
             true,
             Translation2d() // chris wants in the middle
         )

src/main/kotlin/frc/robot/commands/drivetrain/RotateTo180.kt

@@ -6,9 +6,10 @@ import frc.robot.RobotContainer
 class RotateTo180(
     val robotContainer: RobotContainer,
 ) : CommandBase() {
-    override fun execute() {
+    override fun initialize() {
         robotContainer.rotateTo180 = true
     }
+
     override fun end(interrupted: Boolean) {
         robotContainer.rotateTo180 = false
     }

src/main/kotlin/frc/robot/commands/intake/ManualZeroIntake.kt

@@ -0,0 +1,33 @@
+//package frc.robot.commands.intake
+//
+//import edu.wpi.first.wpilibj.Timer
+//import edu.wpi.first.wpilibj2.command.CommandBase
+//import frc.robot.RobotContainer
+//import frc.robot.constants.intake
+//import frc.robot.subsystems.Intake
+//
+//class ManualZeroIntake(private val robotContainer: RobotContainer): CommandBase() {
+//
+////    val timer = Timer()
+//    init {
+//        addRequirements(robotContainer.intake)
+//    }
+//
+////    override fun initialize() {
+////        timer.restart()
+////        robotContainer.intake.modeZeroed = false
+////        robotContainer.intake.setDeployAngle(robotContainer.intake.deployPosition)
+////    }
+//    override fun execute() {
+//        robotContainer.intake.modeVoltage = -3.0
+//        robotContainer.intake.setModeAngle(-1.7)
+//    }
+//
+//    override fun end(interrupted: Boolean) {
+//        robotContainer.intake.zeroModeMotor()
+//        robotContainer.intake.modeVoltage = 0.0
+//    }
+//
+//    override fun isFinished() = intake.an
+//
+//}

src/main/kotlin/frc/robot/commands/intake/ZeroModeMotor.kt

@@ -2,27 +2,28 @@ package frc.robot.commands.intake
 
 import edu.wpi.first.wpilibj.Timer
 import edu.wpi.first.wpilibj2.command.CommandBase
+import frc.robot.RobotContainer
 import frc.robot.subsystems.Intake
 
-class ZeroModeMotor(private val intake: Intake): CommandBase() {
+class ZeroModeMotor(private val robotContainer: RobotContainer): CommandBase() {
 
     val timer = Timer()
     init {
-        addRequirements(intake)
+        addRequirements(robotContainer.intake)
     }
 
     override fun initialize() {
         timer.restart()
-        intake.modeZeroed = false
-        intake.setDeployAngle(intake.deployPosition)
+        robotContainer.intake.modeZeroed = false
+        robotContainer.intake.setDeployAngle(robotContainer.intake.deployPosition)
     }
     override fun execute() {
-        intake.modeVoltage = -3.0
+        robotContainer.intake.modeVoltage = -3.0
     }
 
     override fun end(interrupted: Boolean) {
-        intake.zeroModeMotor()
-        intake.modeVoltage = 0.0
+        robotContainer.intake.zeroModeMotor()
+        robotContainer.intake.modeVoltage = 0.0
     }
 
     override fun isFinished() = timer.hasElapsed(1.0)

src/main/kotlin/frc/robot/commands/pathing/AutoPlaceAndBalance.kt

@@ -16,6 +16,7 @@ import frc.robot.utils.GamePiece
 import frc.robot.utils.grid.PlacementGroup
 import frc.robot.utils.grid.PlacementLevel
 import frc.robot.utils.grid.PlacementSide
+import java.lang.Exception
 
 class AutoPlaceAndBalance(
     private val robotContainer: RobotContainer,
@@ -52,25 +53,31 @@ class AutoPlaceAndBalance(
                 Throw(
                     robotContainer.manipulator,
                     { GamePiece.cone },
-                    { PlacementLevel.Level3 }).withTimeout(0.75)
+                    { PlacementLevel.Level3 })
+                    .withTimeout(0.5)
             ) // shoot cube
             .withTimeout(4.0)
             .andThen(
-                MoveToPosition(robotContainer.drivetrain, 14.4, 2.72, 180.0, maxPosSpeed = 3.7)
+                MoveToPosition(robotContainer.drivetrain,
+                    14.4, 2.72, 180.0,
+                    maxPosSpeed = 3.7
+                )
                     .andThen(
-                        MoveToPosition(
-                            robotContainer.drivetrain,
-                            (13.34 - 0.5),
-                            2.72,
-                            180.0,
+                        MoveToPosition(robotContainer.drivetrain,
+                            (13.34 - 0.5), 2.72, 180.0,
                             maxPosSpeed = 3.7
                         ).withTimeout(4.0)
                     )
-                    .alongWith(
+                    .deadlineWith(
+                        SetSubsystemPosition(robotContainer, { IOLevel.Balance }, { GamePiece.cone }, true)
+                    )
+            )
+            .andThen(
+                AutoBalance(robotContainer.drivetrain)
+                    .deadlineWith(
                         SetSubsystemPosition(robotContainer, { IOLevel.Balance }, { GamePiece.cone }, true)
                     )
             )
-            .andThen(AutoBalance(robotContainer.drivetrain))
     }
 
     private fun pathBlue(robotContainer: RobotContainer): Command {
@@ -80,7 +87,8 @@ class AutoPlaceAndBalance(
                 { IOLevel.High },
                 { GamePiece.cone },
                 true
-            ))
+            )
+            )
             .andThen(
                 SetSubsystemPosition(
                     robotContainer,
@@ -103,13 +111,17 @@ class AutoPlaceAndBalance(
                     .andThen(
                         MoveToPosition(robotContainer.drivetrain, 16.52 - (13.34 - 0.5), 2.72, 0.0,
                             maxPosSpeed = 3.7
-                        )
-                            .withTimeout(4.0)
+                        ).withTimeout(4.0)
                     )
-                    .alongWith(
+                    .deadlineWith(
+                        SetSubsystemPosition(robotContainer, { IOLevel.Balance }, { GamePiece.cone }, true)
+                    )
+            )
+            .andThen(
+                AutoBalance(robotContainer.drivetrain)
+                    .deadlineWith(
                         SetSubsystemPosition(robotContainer, { IOLevel.Balance }, { GamePiece.cone }, true)
                     )
             )
-            .andThen(AutoBalance(robotContainer.drivetrain))
     }
 }