🚧 assorted mid-comp fixes

robot.py

@@ -1,29 +1,22 @@
 from commands2 import CommandScheduler
-from wpilib import DriverStation, Preferences
-from robotcontainer import RobotContainer
-from logging import basicConfig, DEBUG, info
+from wpilib import Preferences
+from wpilib.shuffleboard import Shuffleboard
+from robotcontainer import RobotContainer, DummyGyro
 from commands2.timedcommandrobot import TimedCommandRobot
 
-basicConfig(level=DEBUG)
-
 
 class Ghost(TimedCommandRobot):
     __slots__ = ("auto_active", "subsystems")
 
-    def info(self, message: str):
-        if (not DriverStation.isDSAttached()) or DriverStation.isFMSAttached():
-            # * don't run if there's no driver station/we're in competition
-            return
-        info(message)
-
     # dio- port 0 green, port 1 yellow, port 2 blue, and port 3 white
 
     def robotInit(self):
         self.subsystems = RobotContainer(self.isSimulation())
 
         # * allows the robot to remember + the dashboard to configure whether or not autonomous should run
         Preferences.initBoolean("auto_active", False)
-        self.auto_active = Preferences.getBoolean("auto_active", False)
+        if type(self.subsystems.gyro) != DummyGyro:
+            Shuffleboard.getTab("LiveWindow").add("gyro", self.subsystems.gyro)
 
     def teleopInit(self):
         """executed when teleoperated mode is enabled. this function turns on the controller's safety features to stop us from breaking things."""
@@ -34,12 +27,13 @@ def teleopPeriodic(self): ...
     def teleopExit(self) -> None:
         self.subsystems.arm.mode = 0
 
-    def autonomousInit(self) -> None: ...
+    def autonomousInit(self) -> None:
+        self.auto_active = Preferences.getBoolean("auto_active", False)
 
     def autonomousPeriodic(self) -> None:
         if self.auto_active:
             self.subsystems.shooter.shoot()
-            self.subsystems.drivetrain.stop()
+            # self.subsystems.drivetrain.stop()
 
     def autonomousExit(self) -> None:
         self.subsystems.drivetrain.stop()

robotcontainer.py

@@ -156,7 +156,17 @@ def configureButtonBindings(self):
         )
 
         # * toggle arm up and down with x
-        self.controller.x().onTrue(InstantCommand(lambda: self.arm.toggle(), self.arm))
+        self.controller.x().onTrue(
+            InstantCommand(
+                self.drivetrain.invert, self.drivetrain
+            ).andThen(InstantCommand(self.arm.toggle, self.arm))
+        )
+
+        self.controller.y().onTrue(
+            InstantCommand(lambda: self.arm.motor.set(0.3))
+        ).onFalse(
+            InstantCommand(lambda: self.arm.motor.set(0))
+        )
 
         # self.controller.povUp().or_(self.controller.povUpLeft()).or_(
         #     self.controller.povUpLeft()
@@ -184,7 +194,15 @@ def reset_arm():
             self.arm.motor.stopMotor()
             self.arm.encoder.reset()
 
-        self.controller.back().onTrue(FunctionalCommand(onInit=self.arm.motor.stopMotor, onExecute=self.arm.motor.set(-0.5), onEnd=reset_arm, isFinished=self.arm.encoder.getRate(0)))
+        self.controller.back().onTrue(
+            FunctionalCommand(
+                onInit=self.arm.motor.stopMotor,
+                onExecute=lambda: self.arm.motor.set(-0.75),
+                onEnd=reset_arm,
+                isFinished=lambda: self.arm.encoder.getRate() == 0,
+            )
+        )
+
         ...
 
     def getAutonomousCommand(self):

subsystems/arm.py

@@ -1,12 +1,10 @@
 from commands2.subsystem import Subsystem
 from rev import CANSparkMax
-from wpilib import Encoder, Preferences
-from wpilib.shuffleboard import Shuffleboard
+from wpilib import Encoder
 from constants import ArmPID
 from wpimath.controller import PIDController
 
 
-
 class Arm(Subsystem):
     __slots__ = ("motor", "encoder", "setpoint", "pid")
 
@@ -19,14 +17,19 @@ def __init__(self):
         self.encoder = Encoder(1, 2, False, Encoder.EncodingType.k4X)
         self.mode = 0
         self.encoder.setDistancePerPulse(1 / 7)
-        Shuffleboard.getTab("LiveWindow").add(self.encoder)
-        Shuffleboard.getTab("LiveWindow").add(self.pid)
+        # Shuffleboard.getTab("LiveWindow").add(self.encoder)
+        # Shuffleboard.getTab("LiveWindow").add(self.pid)
 
     def get_setpoint(self) -> int:
-        return self.mode * 88
+        if self.mode == 2:
+            return 88
+        return self.mode * 176
 
     def toggle(self):
-        self.mode = (self.mode + 1) % 3
+        self.mode = (self.mode + 1) % 2
+
+    def aim_amp(self):
+        self.mode = 2
 
     def periodic(self):
         # return # skip all the other stuff
@@ -35,8 +38,13 @@ def periodic(self):
         # self.motor.set(self.pid.calculate(error, self.get_setpoint()))
         if abs(error) < 1:
             self.motor.set(0)
+            if self.get_setpoint() == 0:
+                self.encoder.reset()
         else:
-            self.motor.set((error * ArmPID.K_P) + (0.3 * error / abs(error)))
+            if self.mode == 2:  # if it's aiming for the amp
+                self.motor.set(0.75)  # lower power
+            else:
+                self.motor.set((error * ArmPID.K_P) + (0.3 * error / abs(error)))
         # self.pid.setReference(self.get_setpoint(), CANSparkMax.ControlType.kPosition)
         # print(error)
         # if abs(error) < 1:  # * if it's less than 0.1 degree off...

subsystems/mecanum.py

@@ -12,6 +12,9 @@ def __init__(self, front: CANSparkMax, rear: CANSparkMax):
         self.front: SparkRelativeEncoder = front.getEncoder()
         self.rear: SparkRelativeEncoder = rear.getEncoder()
 
+        self.front.setPosition(0)
+        self.rear.setPosition(0)
+
     def set_conversion_factor(self, factor: float):
         self.front.setPositionConversionFactor(factor)
         self.rear.setPositionConversionFactor(factor)
@@ -43,7 +46,7 @@ def __init__(self, max_output: float = 1.0):
         # self.arm.setIdleMode(CANSparkMax.IdleMode.kCoast)
 
         # * start off not inveretd
-        self.inversion_factor = 1
+        self.inversion_factor = -1
 
         # * initialize the motors
         left_rear = CANSparkMax(1, CANSparkMax.MotorType.kBrushless)
@@ -54,11 +57,11 @@ def __init__(self, max_output: float = 1.0):
 
         # * when we're not driving, brake
         # ? do we want this? driving seems much smoother when coasting
-        left_front.setIdleMode(CANSparkMax.IdleMode.kBrake)
-        left_rear.setIdleMode(CANSparkMax.IdleMode.kBrake)
+        left_front.setIdleMode(CANSparkMax.IdleMode.kCoast)
+        left_rear.setIdleMode(CANSparkMax.IdleMode.kCoast)
 
-        right_front.setIdleMode(CANSparkMax.IdleMode.kBrake)
-        right_rear.setIdleMode(CANSparkMax.IdleMode.kBrake)
+        right_front.setIdleMode(CANSparkMax.IdleMode.kCoast)
+        right_rear.setIdleMode(CANSparkMax.IdleMode.kCoast)
 
         # * invert the right side (they just are like this)
         right_front.setInverted(True)
@@ -73,6 +76,9 @@ def __init__(self, max_output: float = 1.0):
         self.left_encoders = EncoderGroup(left_front, left_rear)
         self.right_encoders = EncoderGroup(right_front, right_rear)
 
+        dash = Shuffleboard.getTab("LiveWindow")
+        dash.add("left front", self.left_encoders.front.getPosition())
+        dash.add("right front", self.right_encoders.front.getPosition())
         # * make the encoders return meters
         self.left_encoders.set_conversion_factor(NEO_CPR / WHEEL_CIRCUMFERENCE)
         self.right_encoders.set_conversion_factor(NEO_CPR / WHEEL_CIRCUMFERENCE)
@@ -83,7 +89,6 @@ def __init__(self, max_output: float = 1.0):
 
         self.drivetrain.setMaxOutput(max_output)
 
-        dash = Shuffleboard.getTab("LiveWindow")
         dash.add("drivetrain", self.drivetrain)
 
     def invert(self):
@@ -101,7 +106,7 @@ def drive(self, forward_motion: float, side_motion: float, rotation: float):
         self.drivetrain.driveCartesian(
             self.deadzone(forward_motion) * self.inversion_factor,
             self.deadzone(side_motion) * self.inversion_factor,
-            self.deadzone(rotation) * self.inversion_factor,
+            self.deadzone(rotation),
         )
 
     def stop(self):

subsystems/odometry.py

@@ -6,14 +6,23 @@
     MecanumDriveWheelPositions,
 )
 from wpimath.geometry import Translation2d, Rotation2d, Pose2d
-from wpilib import Field2d, Timer
+from wpilib import Field2d, Timer, DriverStation
 from wpilib.shuffleboard import Shuffleboard
 
 from navx import AHRS
 from subsystems.vision import Vision
 from subsystems.mecanum import Mecanum
 
 from constants import LINEAR_SPEED
+from logging import basicConfig, DEBUG, info as linfo
+
+basicConfig(level=DEBUG)
+
+def info(message: str):
+    if (not DriverStation.isDSAttached()) or DriverStation.isFMSAttached():
+        # * don't run if there's no driver station/we're in competition
+        return
+    linfo(message)
 
 
 class Odometry(Subsystem):
@@ -46,12 +55,13 @@ def __init__(self, gyro: AHRS, drivetrain: Mecanum, vision: Vision):
             self.kinematics,
             self.gyro.getRotation2d(),
             self.wheel_positions,
-            Pose2d(5.0, 13.5, Rotation2d()),
+            robot := Pose2d(5.0, 13.5, Rotation2d()),
         )
         self.field = Field2d()
+        self.field.setRobotPose(robot)
         dash = Shuffleboard.getTab("LiveWindow")
         dash.add("field", self.field)
-        dash.add("gyro", gyro.getAngle())
+
 
         self.update()
         self.vision_timer.start()
@@ -68,6 +78,7 @@ def periodic(self):
             self.update()
             return
         if self.vision_timer.advanceIfElapsed(1):
+            info(self.gyro.getRotation2d().degrees())
             estimated_pose = self.vision.estimate_pose()
             if estimated_pose:  # * if we have a pose estimation from photonvision...
                 pose = estimated_pose.toPose2d()

subsystems/shooter.py

@@ -15,16 +15,17 @@ def __init__(self):
         intake_top_left.setInverted(True)
         # intake_bot_right.setInverted(True)
 
-        shooter_left = CANSparkMax(10, CANSparkMax.MotorType.kBrushed)
+        shooter_left = CANSparkMax(10, CANSparkMax.MotorType.kBrushless)
         shooter_right = CANSparkMax(11, CANSparkMax.MotorType.kBrushless)
         shooter_right.setInverted(False)
+        shooter_left.setInverted(True)
 
         shooter_left.setIdleMode(CANSparkMax.IdleMode.kBrake)
         shooter_right.setIdleMode(CANSparkMax.IdleMode.kBrake)
 
         top_group = MotorControllerGroup(intake_top_left, intake_top_right)
         bottom_group = MotorControllerGroup(intake_bot_left, intake_bot_right)
-        top_group.setInverted(True)
+        intake_top_right.setInverted(True)
 
         shooter_group = MotorControllerGroup(shooter_left, shooter_right)
 
@@ -40,17 +41,19 @@ def arm_intake(self):
 
     def arm_spit(self):
         # print("Regurgitating the whole note.")
-        self.bottom_group.set(-0.5)
-        self.top_group.set(-0.5)
+        self.bottom_group.set(-0.25)
+        self.top_group.set(-0.25)
 
     def intake(self):
         self.shooter_group.set(-0.5)
 
     def shoot(self):
-        self.shooter_group.set(1)
+        self.shooter_group.set(0.75)
+        self.arm_spit()
 
     def stop(self):
         self.shooter_group.set(0)
+        self.arm_stop()
 
     def set(self, speed: float):
         self.shooter_group.set(speed)

subsystems/vision.py

@@ -85,7 +85,10 @@ def estimate_pose(self) -> Optional[Pose3d]:
             self.driver_mode = False
 
         # gets the current best target
-        pose_result = self.pose_estimator.update(self.latest_result())
+        latest_result = self.latest_result()
+        if not latest_result:
+            return None
+        pose_result = self.pose_estimator.update()
         return pose_result.estimatedPose if pose_result else None
 
     def calculate_distance(