Merge pull request #189 from thedropbears/macquire-day-2

Macquarie 1

components/chassis.py

@@ -49,9 +49,6 @@ def __init__(
         self.steer = steer
         self.drive = drive
 
-        steer.configFactoryDefault()
-        drive.configFactoryDefault()
-
         # Reduce CAN status frame rates before configuring
         steer.setStatusFramePeriod(ctre.StatusFrameEnhanced.Status_1_General, 250, 10)
         drive.setStatusFramePeriod(ctre.StatusFrameEnhanced.Status_1_General, 250, 10)

components/indexer.py

@@ -12,7 +12,7 @@ class CargoColour(Enum):
     BLUE = wpilib.DriverStation.Alliance.kBlue
 
     def is_opposition(self) -> bool:
-        return self.value != wpilib.DriverStation.getAlliance()
+        return self.is_valid() and self.value != wpilib.DriverStation.getAlliance()
 
     def is_valid(self) -> bool:
         return self is not self.NONE
@@ -51,8 +51,6 @@ class Direction(Enum):
 
     def setup(self) -> None:
         for motor in (self.indexer_chimney_motor, self.indexer_tunnel_motor):
-            motor.restoreFactoryDefaults()
-
             # Reduce all CAN periodic status frame rates.
             motor.setPeriodicFramePeriod(
                 rev.CANSparkMaxLowLevel.PeriodicFrame.kStatus0, 500

components/intake.py

@@ -28,7 +28,6 @@ def __init__(self):
         self.motor_enabled = True
 
     def setup(self) -> None:
-        self.intake_motor.restoreFactoryDefaults()
         self.intake_motor.setInverted(False)
         self._intake_limit = self.intake_motor.getForwardLimitSwitch(
             rev.SparkMaxLimitSwitch.Type.kNormallyOpen

components/shooter.py

@@ -32,8 +32,6 @@ def setup(self) -> None:
             self.left_motor,
             self.right_motor,
         ):
-            motor.configFactoryDefault()
-
             motor.setStatusFramePeriod(
                 ctre.StatusFrameEnhanced.Status_1_General, 250, 10
             )

components/turret.py

@@ -43,6 +43,7 @@ class Turret:
     PISTON_CONTRACT_THRESHOLD = math.radians(60)
 
     is_piston_extended = False
+    is_running = magicbot.tunable(True)
 
     logger: Logger
 
@@ -51,8 +52,6 @@ def __init__(self) -> None:
         self.sync_count = 0
 
     def setup(self) -> None:
-        self.motor.configFactoryDefault()
-
         # Positive motion is counterclockwise from above.
         self.motor.setInverted(False)
 
@@ -72,6 +71,13 @@ def setup(self) -> None:
         self.motor.configSelectedFeedbackSensor(
             ctre.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 10
         )
+
+        # If the robot has just been turned on, assume the turret is in the starting configuration
+        if self.motor.getSelectedSensorPosition() == 0.0:
+            self.motor.setSelectedSensorPosition(
+                3 / 4 * math.tau * self.COUNTS_PER_TURRET_RADIAN, timeoutMs=10
+            )
+
         self.absolute_encoder.setDistancePerRotation(-math.tau)
         self.absolute_encoder.setPositionOffset(0.9668)
 
@@ -109,6 +115,10 @@ def execute(self) -> None:
         self.target = self.wrap_allowable_angle(self.target)
         self.update_angle_history()
 
+        if not self.is_running:
+            self.motor.stopMotor()
+            return
+
         self.motor.set(
             ctre.ControlMode.MotionMagic,
             self.target * self.COUNTS_PER_TURRET_RADIAN,

components/vision.py

@@ -71,7 +71,7 @@ def __init__(self) -> None:
             self.camera = self.sim_vision_system.cam
 
         self.camera.setLEDMode(LEDMode.kOn)
-        self.max_std_dev = 0.1
+        self.max_std_dev = 0.05
         self.has_target = False
         self.distance = -1.0
         self.target_pitch = 0.0

controllers/indexer.py

@@ -111,7 +111,7 @@ def forced_clearing(self) -> None:
         if not self.wants_to_intake:
             self.intake.deployed = False
 
-    @timed_state(duration=10.0, next_state="stopping", must_finish=True)
+    @timed_state(duration=2.0, next_state="stopping", must_finish=True)
     def transferring_to_chimney(self) -> None:
         if self.indexer.has_cargo_in_chimney():
             self.next_state("stopping")

controllers/leds.py

@@ -26,7 +26,7 @@ def on_disable(self):
 
     def execute(self) -> None:
         if not self.vision.is_connected() and wpilib.RobotBase.isReal():
-            self.status_lights.set(DisplayType.PULSE, LedColours.WHITE)
+            self.status_lights.set(DisplayType.PULSE, LedColours.RED)
         elif not self.is_enabled:
             self.status_lights.set_disabled()
         elif (
@@ -35,12 +35,12 @@ def execute(self) -> None:
             > self.shooter_control.MAX_ROTATION
         ):
             self.status_lights.set(DisplayType.SOLID, LedColours.PINK)
-        elif not self.indexer.is_full():
-            self.status_lights.set(DisplayType.SOLID, LedColours.RED)
+        elif self.indexer.is_full():
+            self.status_lights.set(DisplayType.SOLID, LedColours.WHITE)
         elif self.indexer.has_cargo_in_tunnel() or self.indexer.has_cargo_in_chimney():
-            self.status_lights.set(DisplayType.SOLID, LedColours.ORANGE)
+            self.status_lights.set(DisplayType.SOLID, LedColours.CYAN)
         else:
-            self.status_lights.set(DisplayType.SOLID, LedColours.GREEN)
+            self.status_lights.set(DisplayType.SOLID, LedColours.BLUE)
 
         if self.shooter_control.auto_shoot and self.indexer.has_cargo_in_chimney():
             self.status_lights.set(DisplayType.PULSE)

controllers/shooter.py

@@ -38,7 +38,7 @@ class ShooterController(StateMachine):
     distance = 0.0
     # fmt: off
     ranges_lookup =         ( 3.0,  3.5,  4.0,  4.5,  5.0,  5.5,  6.0,  6.5,  7.0,  7.5,  8.0)
-    flywheel_speed_lookup = (27.5, 28.0, 30.0, 32.5, 36.0, 38.5, 41.5, 42.5, 43.5, 44.5, 46.5)
+    flywheel_speed_lookup = (26.0, 28.0, 29.0, 30.0, 36.0, 38.0, 40.0, 41.0, 42.0, 42.5, 43.0)
     times_lookup =          (0.77, 0.86, 0.94, 1.02, 1.10, 1.18, 1.26, 1.34, 1.43, 1.51, 1.59)
     # fmt: on
 

robot.py

@@ -126,7 +126,6 @@ def teleopInit(self) -> None:
         self.shooter_control.lead_shots = True
         self.indexer_control.ignore_colour = False
         self.shooter_control.auto_shoot = False
-        self.vision.max_std_dev = 0.4
         self.vision.fuse_vision_observations = True
 
     def disabledPeriodic(self) -> None:
@@ -197,10 +196,6 @@ def teleopPeriodic(self) -> None:
             self.chassis.zero_yaw()
 
     def testPeriodic(self) -> None:
-        # hold y and use joystick throttle to set flywheel speed
-        throttle = scale_value(self.joystick.getThrottle(), 1, -1, 0, 1)
-        self.shooter.motor_speed = throttle * 60
-
         # hold x and use left stick to slew turret
         if self.joystick.getPOV() != -1:
             self.turret.target += math.sin(math.radians(self.joystick.getPOV(0))) * 0.03
@@ -230,18 +225,18 @@ def testPeriodic(self) -> None:
         if self.joystick.getRawButton(11):
             self.indexer_control.wants_to_intake = True
 
-        if self.codriver.getBButtonPressed():
+        if self.gamepad.getBButtonPressed():
             self.indexer_control.engage("forced_clearing", force=True)
 
-        self.indexer_control.catflap_active = self.codriver.getXButton()
+        self.indexer_control.catflap_active = self.gamepad.getXButton()
 
         # lower intake without running it
-        if self.codriver.getLeftBumper():
+        if self.gamepad.getLeftBumper():
             self.intake.motor_enabled = False
             self.intake.deployed = not self.intake.deployed
             self.intake.auto_retract = False
 
-        if self.codriver.getRightBumper():
+        if self.gamepad.getRightBumper():
             self.chassis.drive_local(self.test_chassis_speed, 0, 0)
         else:
             self.chassis.drive_local(0, 0, 0)