coast go brr

rio/commands/flyUntilTrigger.py

@@ -0,0 +1,29 @@
+import logging
+import commands2
+
+
+class FlyUntilTrigger(commands2.Command):
+    def __init__(self, speed, subsystem):
+        super().__init__()
+
+        # local subsystem instance
+        self.subsystem = subsystem
+
+        # requested speed
+        self.speed = speed
+
+    def initialize(self):
+        logging.info("Spinning up...")
+
+    def execute(self):
+        self.subsystem.setFlyWheelSpeed(self.speed)
+        print(self.subsystem.switchPress())
+
+    def isFinished(self):
+        return self.subsystem.switchPress()
+
+    def end(self, interrupted: bool):
+        self.subsystem.setFlyWheelSpeed(0)
+        logging.info(f"Done, up to speed is {self.isFinished()}")
+        # self.subsystem.setFlyWheelSpeed(0)
+        return True

rio/commands/setIntakeSpeed.py

@@ -27,7 +27,9 @@ def execute(self):
         else:
             self.intakeSubsystem.intake(self.speed)
 
+    def isFinished(self):
+        return True
+
     def end(self, interrupted: bool):
-        self.intakeSubsystem.intake(0)
         logging.debug(f"Intake suck done")
         return True

rio/robotcontainer.py

@@ -34,6 +34,7 @@
 from commands.manualRot import manualROT
 from commands.intakeUntilNote import intakeUntilNote
 from commands.setIntakeSpeed import SetIntakeSpeed
+from commands.flyUntilTrigger import FlyUntilTrigger
 
 
 class RobotContainer:
@@ -97,9 +98,11 @@ def configureButtonBindings(self) -> None:
 
         # intaking
 
+
+
         self.opController.x().onTrue(
             commands2.SequentialCommandGroup(
-                RotateIntake(120, self.intake),
+                RotateIntake(115, self.intake),
                 intakeUntilNote(0.5, self.intake),
                 RotateIntake(0, self.intake),
             )
@@ -109,8 +112,19 @@ def configureButtonBindings(self) -> None:
                 RotateIntake(0, self.intake), 
                 FlyWheelSpeed(1.0, self.shooter),    #power flywheels
                 commands2.WaitCommand(3),            #wait for flywheels to get up to speed
-                SetIntakeSpeed(-0.4, self.intake)  ,
+                SetIntakeSpeed(-0.4, self.intake),
+                commands2.WaitCommand(3), 
                 FlyWheelSpeed(0.0, self.shooter),
+                SetIntakeSpeed(0, self.intake)
+            )
+        )
+        self.opController.a().onTrue(
+            commands2.SequentialCommandGroup(       
+                RotateIntake(0, self.intake),  
+                SetIntakeSpeed(-0.4, self.intake),
+                FlyUntilTrigger(0.25,self.shooter)
+
+                   #power flywheels
             )
         )
         # moving intake
@@ -128,7 +142,7 @@ def configureButtonBindings(self) -> None:
         # self.opController.pov(0).whileTrue(ShooterROT(0,self.shooter))  # out
         # self.opController.pov(180).whileTrue(ShooterROT(40,self.shooter))  # out
 
-        self.opController.a().whileTrue(RotateIntake(0, self.intake))
+
         self.opController.b().whileTrue(RotateIntake(60, self.intake))
 
     def disablePIDSubsystems(self) -> None:

rio/subsystems/shooter.py

@@ -1,5 +1,4 @@
 import wpilib
-
 from ntcore import NetworkTableInstance
 from commands2 import Subsystem
 from rev import (
@@ -34,12 +33,21 @@ def __init__(self) -> None:
 
         # PID values
         self.rotatePIDController = self.rotateMotor.getPIDController()
+        self.rotatePIDController.setFeedbackDevice(self.rotateEncoder)
 
         self.rotatePIDController.getP(SuperStrucConstants.kP)
         self.rotatePIDController.getI(SuperStrucConstants.kI)
         self.rotatePIDController.getD(SuperStrucConstants.kD)
         self.rotatePIDController.getFF(SuperStrucConstants.kFF)
 
+        # limit switches
+        self.limtSwitch = self.flyMotor.getReverseLimitSwitch(
+            SparkMaxLimitSwitch.Type.kNormallyOpen
+        )
+
+        self.limtSwitch.enableLimitSwitch(True)
+
+        self.flyMotor.burnFlash()
 
     def periodic(self) -> None:
         self.sd.putNumber("Thermals/rotate", self.rotateMotor.getMotorTemperature())
@@ -48,7 +56,12 @@ def periodic(self) -> None:
 
     def setFlyWheelSpeed(self, speed):
         self.flyMotor.set(speed)
-
+
+    def setIdleBrake(self):
+        self.flyMotor.setIdleMode()
+    def switchPress(self):
+        return self.limtSwitch.get()
+
     def rotateManual(self,speed):
         self.rotateMotor.set(speed)