holo

rio/commands/loadMagazine.py

@@ -24,6 +24,9 @@ def initialize(self):
 
     def execute(self):
         self.shooter.setFlyWheelSpeed(self.speed)
+        if self.shooter.isMagazineLoaded():
+            self.shooter.zeroFly()
+            self.shooter.setFlyAngle(0)
 
     def isFinished(self):
         return self.shooter.isMagazineLoaded()

rio/commands/shooterROT.py

@@ -1,17 +1,21 @@
 import commands2
 import logging
 
+from subsystems.shooter import Shooter
+
 
 class ShooterROT(commands2.Command):
-    def __init__(self, angle, subsystem):
+    def __init__(self, angle: float, _shooter=Shooter):
         super().__init__()
 
         # local subsystem instance
-        self.subsystem = subsystem
+        self.subsystem = _shooter
 
         # requested speed
         self.angle = angle
 
+        self.addRequirements(self.subsystem)
+
     def initialize(self):
         logging.info(f"Moving shooter to {self.angle}")
 

rio/constants.py

@@ -154,12 +154,18 @@ class SuperStrucConstants:
 
     kRotConversion = 360  # Configured feb 12 by joe
 
-    # PID values
-    kP = 0.8
-    kI = 0.00001
-    kD = 1.6
-    kFF = 0
+    krotateInversion = True
 
+    # PID values
+    krotateP = 0.04
+    krotateI = 0.00001
+    krotateD = 0.04
+    krotateFF = 0
+
+    kflyP = 0.1
+    kflyI = 0.0001
+    kflyD = 0.1
+    kflyFF = 0
 
 
 class IntakeConstants:

rio/robotcontainer.py

@@ -52,7 +52,7 @@ def __init__(self) -> None:
         self.intake = IntakeSubsystem()
 
         # The driver's controller
-        self.driverController = CommandJoystick(0)
+        self.driverController = CommandXboxController(0)
 
         # the operators controller
         self.opController = CommandXboxController(OIConstants.kOpControllerPort)
@@ -97,7 +97,7 @@ def configureButtonBindings(self) -> None:
         # _____INTAKE_KEYBINDS_____
 
         # Drop intake (controller x)
-        self.opController.x().onTrue(
+        self.driverController.x().onTrue(
             commands2.SequentialCommandGroup(
                 FlyWheelSpeed(0, self.shooter),  # Stop shooter (if its running)
                 RotateIntake(120, self.intake),  # Put intake down
@@ -107,15 +107,15 @@ def configureButtonBindings(self) -> None:
         )
 
         # Shoot to speaker (button y)
-        self.opController.y().onTrue(
+        self.driverController.y().onTrue(
             commands2.SequentialCommandGroup(
                 RotateIntake(
                     0, self.intake
                 ),  # Put intake fully inside (if it wasn't already)
                 FlyWheelSpeed(1.0, self.shooter),  # Power up the flywheels (?)
                 commands2.WaitCommand(3),  # Wait for full speed TODO: REMOVE ME
                 SetIntakeSpeed(
-                    -0.4, self.intake
+                    -0.6, self.intake
                 ),  # Load magazine? (but without ending)
                 commands2.WaitCommand(3),  # Wait again.... TODO: REMOVE ME
                 FlyWheelSpeed(0.0, self.shooter),  # Stop flywheel
@@ -124,7 +124,7 @@ def configureButtonBindings(self) -> None:
         )
 
         # Shoot to amp (button a)
-        self.opController.a().onTrue(
+        self.driverController.a().onTrue(
             commands2.SequentialCommandGroup(
                 RotateIntake(0, self.intake),  # Rotate to fully closed
                 SetIntakeSpeed(-0.6, self.intake),  # Eject slowly
@@ -144,6 +144,8 @@ def configureButtonBindings(self) -> None:
         self.opController.pov(0).whileTrue(manualROT(0.5, self.shooter))
         self.opController.pov(180).whileTrue(manualROT(-0.5, self.shooter))
 
+        self.driverController.pov(0).whileTrue(ShooterROT(60, self.shooter))
+
         # PID shooter rotation (NOT CURRENTLY WORKING)
 
         # self.opController.pov(0).whileTrue(ShooterROT(0,self.shooter))  # out

rio/subsystems/shooter.py

@@ -39,14 +39,27 @@ def __init__(self) -> None:
             SuperStrucConstants.kRotConversion
         )
 
+        self.rotateEncoder.setInverted(SuperStrucConstants.krotateInversion)
+
         # PID values
+        # rotate pid
         self.rotatePIDController = self.rotateMotor.getPIDController()
         self.rotatePIDController.setFeedbackDevice(self.rotateEncoder)
+        self.rotatePIDController.setPositionPIDWrappingEnabled(False)
+
+        self.rotatePIDController.setP(SuperStrucConstants.krotateP)
+        self.rotatePIDController.setI(SuperStrucConstants.krotateI)
+        self.rotatePIDController.setD(SuperStrucConstants.krotateD)
+        self.rotatePIDController.setFF(SuperStrucConstants.krotateFF)
+
+        # fly pid
+        self.flyPIDController = self.flyMotor.getPIDController()
+        self.flyPIDController.setFeedbackDevice(self.flyEncoder)
 
-        self.rotatePIDController.setP(SuperStrucConstants.kP)
-        self.rotatePIDController.setI(SuperStrucConstants.kI)
-        self.rotatePIDController.setD(SuperStrucConstants.kD)
-        self.rotatePIDController.setFF(SuperStrucConstants.kFF)
+        # self.rotatePIDController.setP(SuperStrucConstants.kflyP)
+        # self.rotatePIDController.setI(SuperStrucConstants.kflyI)
+        # self.rotatePIDController.setD(SuperStrucConstants.kflyD)
+        # self.rotatePIDController.setFF(SuperStrucConstants.kflyFF)
 
         # limit switches
         self.magazineSwitch = self.flyMotor.getForwardLimitSwitch(
@@ -65,6 +78,7 @@ def __init__(self) -> None:
     def periodic(self) -> None:
         self.sd.putNumber("Thermals/rotate", self.rotateMotor.getMotorTemperature())
         self.sd.putNumber("Thermals/fly", self.flyMotor.getMotorTemperature())
+        print(self.rotateEncoder.getPosition())
 
     def setFlyWheelSpeed(self, speed):
         self.flyMotor.set(speed)
@@ -96,3 +110,9 @@ def setIdleBrake(self):
     def setIdleCoast(self):
         self.flyMotor.setIdleMode(CANSparkBase.IdleMode.kCoast)
         self.flyMotor.burnFlash()
+
+    def setFlyAngle(self, angle: float):
+        self.flyPIDController.setReference(angle, CANSparkMax.ControlType.kPosition)
+
+    def zeroFly(self):
+        self.flyEncoder.setPosition(0)