Feat/week1auto move

rio/autonomous/botchauto.py

@@ -9,7 +9,7 @@
 
 from commands.kicker_button import Kicker
 from commands.catapult import Catapult
-from commands.flybywire import FlyByWire
+from commands.flywithwires import FlyWithWires
 from commands.zero_swerve import ZeroSwerveModules
 
 
@@ -24,4 +24,5 @@ def __init__(self, yoke: Yoke, drivetrain: Drivetrain) -> None:
             Catapult(yoke, 75, 0.25, True),  # Shoot like dis
             WaitCommand(2),  # Wait again
             Kicker(yoke),
+            FlyWithWires(drivetrain, fwd=-0.2, time=2),
         )

rio/autonomous/highBotchAuto.py

@@ -9,7 +9,7 @@
 
 from commands.kicker_button import Kicker
 from commands.catapult import Catapult
-from commands.flybywire import FlyByWire
+from commands.flywithwires import FlyWithWires
 from commands.zero_swerve import ZeroSwerveModules
 
 
@@ -24,4 +24,5 @@ def __init__(self, yoke: Yoke, drivetrain: Drivetrain) -> None:
             Catapult(yoke, 78.5, 0.52, True),  # Shoot like dis
             WaitCommand(2),  # Wait again
             Kicker(yoke),
+            FlyWithWires(drivetrain, fwd=-0.2, time=2),
         )

rio/commands/catapult.py

@@ -43,7 +43,7 @@ def end(self, interrupted: bool) -> None:
         if not self.endWhenDone:
             self.yoke.setSpeed(0)
 
-        print("Catapult done")
+        print("Catapult command ended")
 
         # Return true when done
         return True

rio/commands/flywithwires.py

@@ -0,0 +1,46 @@
+import commands2
+import wpilib
+
+from subsystems.drivetrain import Drivetrain
+
+
+class FlyWithWires(commands2.CommandBase):
+    """
+    Fires a ball at a fixed angle with no checks or fancy stuff.
+    """
+
+    def __init__(self, drivetrain: Drivetrain, fwd=0, srf=0, rot=0, time=-1) -> None:
+        super().__init__()
+
+        # Local instance of drivetrain
+        self.drivetrain = drivetrain
+
+        self.time = time
+        self.fwd = fwd
+        self.srf = srf
+        self.rot = rot
+
+        self.addRequirements([drivetrain])
+
+        # Timer
+        self.backgroundTimer = wpilib.Timer()
+        self.backgroundTimer.start()
+
+    def initialize(self) -> None:
+        self.backgroundTimer.reset()
+
+    def execute(self) -> None:
+        self.drivetrain.arcadeDrive(
+            self.fwd,
+            self.srf,
+            self.rot,
+        )
+
+    def end(self, interrupted: bool) -> None:
+        self.drivetrain.arcadeDrive(0, 0, 0)
+
+        print("Fly with wires done")
+
+    def isFinished(self) -> bool:
+        if self.time != -1 and self.backgroundTimer.hasElapsed(self.time):
+            return True

rio/commands/kicker_button.py

@@ -29,3 +29,6 @@ def isFinished(self) -> bool:
         if self.backgroundTimer.hasElapsed(1):
             self.yoke.kick(0)
             return True
+
+    def end(self, interrupted: bool) -> None:
+        print("Kicker command ended")

rio/commands/zero_swerve.py

@@ -16,8 +16,7 @@ def __init__(self, drivetrain, overwrite=False) -> None:
         self.overwrite = overwrite
 
         # Require full control of the drivetrain
-        if not overwrite:
-            self.addRequirements([self.drivetrain])
+        self.addRequirements([self.drivetrain])
 
         # Timer
         self.backgroundTimer = wpilib.Timer()

rio/robot.py

@@ -47,8 +47,10 @@ def autonomousInit(self) -> None:
         if self.autonomousCommand:
             self.autonomousCommand.schedule()
 
+        # Don't trigger an enabledInit(), auto can choose for itself.
+
         # Trigger an enabledInit()
-        self.container.enabledInit()
+        # self.container.enabledInit()
 
     def autonomousPeriodic(self) -> None:
         """This function is called periodically during autonomous"""