Dev

.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/RobotContainer.kt

@@ -12,8 +12,10 @@ import edu.wpi.first.wpilibj.DriverStation
 import edu.wpi.first.wpilibj.DriverStation.Alliance.Blue
 import edu.wpi.first.wpilibj.DriverStation.Alliance.Red
 import edu.wpi.first.wpilibj.GenericHID
+import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab
+import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard
 import edu.wpi.first.wpilibj2.command.Command
@@ -41,6 +43,7 @@ import frc.robot.commands.pathing.*
 import frc.robot.commands.pathing.building.blocks.BuildingBlocks.goToHumanPlayerStation
 import frc.robot.commands.pathing.building.blocks.BuildingBlocks.goToPlacementPoint
 import frc.robot.constants.Field2dLayout
+import frc.robot.constants.drivetrain
 import frc.robot.constants.leds.count
 import frc.robot.controls.BryanControlScheme
 import frc.robot.controls.ChrisControlScheme
@@ -314,8 +317,10 @@ class RobotContainer {
                 shootToLThree
                     .whileTrue(ShootToLTwo(intake, this@RobotContainer))
 
-//                snapTo180
-//                    .whileTrue(RotateTo180(this@RobotContainer))
+                snapTo180
+                    .whileTrue(
+                        MoveToPosition(drivetrain, drivetrain.estimatedPose2d, 180.0)
+                    )
             }
         }
     }
@@ -472,7 +477,10 @@ class RobotContainer {
             return@AnimationCustom List<Color>(len) { i ->
                 if (i >= index) color else Color.black
             }
-        }, { false && !drivetrain.canTrustPose && (lightStatus != TeleopFMSRed || lightStatus != TeleopFMSBlue) })
+        }, {
+            @Suppress("KotlinConstantConditions", "SimplifyBooleanWithConstants")
+            false && !drivetrain.canTrustPose && (lightStatus != TeleopFMSRed || lightStatus != TeleopFMSBlue)
+        })
 
 
         val body = KLEDRegion(
@@ -538,10 +546,11 @@ class RobotContainer {
     }
 
     // shuffleboard auto chooser
-    val autoChooserTab: ShuffleboardTab = Shuffleboard.getTab("Autonomous")
-    val autoChooserWidget = autoChooserTab.add("Autonomous", autoChooser)
+    private val autoChooserTab: ShuffleboardTab = Shuffleboard.getTab("Autonomous")
 
-    val armFieldPosition = drivetrain.field2d.getObject("arm")
+    @Suppress("unused")
+    val autoChooserWidget: ComplexWidget = autoChooserTab.add("Autonomous", autoChooser)
+    val armFieldPosition: FieldObject2d = drivetrain.field2d.getObject("arm")
 
 //    val DriveTab: ShuffleboardTab = Shuffleboard.getTab("DriveTab")
 //    val autoChoice = DriveTab.add("Autonomous", autoChooser)

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/BackupSnapTo180.kt

@@ -0,0 +1,22 @@
+package frc.robot.commands.drivetrain
+
+import edu.wpi.first.math.geometry.Pose2d
+import edu.wpi.first.wpilibj2.command.CommandBase
+import frc.robot.RobotContainer
+import frc.robot.commands.pathing.MoveToPosition
+import frc.robot.constants.drivetrain
+
+class BackupSnapTo180 (
+    val robotContainer: RobotContainer
+) : CommandBase(){
+
+    override fun execute() {
+        //MoveToPosition(robotContainer.drivetrain, Pose2d())
+        DriveCommand(robotContainer.drivetrain, { 0.0 }, { 0.0 }, )
+        MoveToPosition()
+    }
+    override fun end(interrupted: Boolean) {
+        robotContainer.rotateTo180 = false
+    }
+
+}

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

@@ -4,10 +4,15 @@ import edu.wpi.first.math.controller.ProfiledPIDController
 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.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
@@ -19,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 *
@@ -44,14 +59,14 @@ class DriverCommand(
                         * controlScheme.speedMutiplier,
                 if(nearestStation()) {
                     rotationPid.calculate(drivetrain.estimatedPose2d.rotation.radians,
-                         Nearest180.findNearest180(drivetrain)
+                        degreesToRadians(findNearest180(drivetrain.estimatedPose2d.rotation.degrees))
                     ) // desired radians
                 }
                 else{
                     -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/Nearest180.kt

@@ -1,14 +1,16 @@
 package frc.robot.commands.drivetrain
 
-import edu.wpi.first.math.util.Units
-import edu.wpi.first.wpilibj2.command.CommandBase
-import frc.robot.subsystems.Drivetrain
-import kotlin.math.round
-
-object Nearest180{
-    fun findNearest180(
-        drivetrain: Drivetrain
-    ): Double {
-        return Units.degreesToRadians(round((drivetrain.estimatedPose2d.rotation.degrees/180) * 180))
+fun findNearest180(
+    currentAngle: Double,
+): Double {
+    // Find the nearest 180 degree angle. the input should be in degrees with a range of -180 to 180
+    // the output will be in degrees with a range of -180 to 180 and will be the closest 180 degree angle to the input
+    // for example, if the input is 0, the output will be 0, if the input is 179, the output will be 180, if the input
+    // is -179, the output will be 180 etc.
+    val nearest180 = currentAngle % 360
+    return if (nearest180 >=90 || nearest180 <= -90) {
+        180.0
+    } else {
+        0.0
     }
-}
+}

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)