Merge pull request #16 from FRC-1721/feat/intakeTuning

Feat/intake tuning

Pipfile

@@ -0,0 +1,11 @@
+[[source]]
+url = "https://pypi.org/simple"
+verify_ssl = true
+name = "pypi"
+
+[packages]
+
+[dev-packages]
+
+[requires]
+python_version = "3.11"

rio/Makefile

@@ -14,7 +14,7 @@ sim:	## Run the simulator
 	python -m robotpy sim
 
 deploy:	## Deploy to the robot (with netconsole)
-	python -m robotpy deploy --nc
+	python -m robotpy deploy --nc --no-install --skip-test
 
 push:	## Deploy to the robot (push only, no netconsole)
 	python -m robotpy deploy

rio/Pipfile

@@ -15,3 +15,6 @@ robotpy-commands-v2 = "2024.2.1"
 
 [requires]
 python_version = "3.11"
+
+[pipenv]
+allow_prereleases = true

rio/commands/FlyWheelSpeed.py

@@ -0,0 +1,19 @@
+import commands2
+
+
+class FlyWheelSpeed(commands2.Command):
+    def __init__(self, speed, subsystem):
+        super().__init__()
+
+        # local subsystem instance
+        self.subsystem = subsystem
+
+        # requested speed
+        self.speed = speed
+
+    def initialize(self):
+        self.subsystem.setFlyWheelSpeed(self.speed)
+
+    def end(self, interrupted: bool):
+        self.subsystem.setFlyWheelSpeed(0)
+        return True

rio/commands/intakeRotationMAN.py

@@ -16,9 +16,6 @@ def __init__(self, angle: float, subsystem):
         # requested speed
         self.angle = angle
 
-        # TODO change current limit later in amps
-        self.intakeSubsystem.liftCurrentLimit(1)
-
     def execute(self):
         self.intakeSubsystem.manualLift(self.angle)
 

rio/commands/intakeSuck.py → rio/commands/intakeUntilNote.py

@@ -1,9 +1,10 @@
+import logging
 import commands2
 
 from subsystems.intake import IntakeSubsystem
 
 
-class IntakeSuck(commands2.Command):
+class intakeUntilNote(commands2.Command):
     def __init__(self, speed: float, subsystem: IntakeSubsystem):
         """
         takes in the rings
@@ -16,12 +17,16 @@ def __init__(self, speed: float, subsystem: IntakeSubsystem):
         # requested speed
         self.speed = speed
 
-        # TODO change current limit later in amps
-        self.intakeSubsystem.intakeCurrentLimit(30)
+    def initialize(self):
+        logging.debug(f"Running command Intake Suck (manual) with speed {self.speed}")
 
     def execute(self):
         self.intakeSubsystem.intake(self.speed)
 
+    def isFinished(self):
+        return self.intakeSubsystem.switchPress()
+
     def end(self, interrupted: bool):
         self.intakeSubsystem.intake(0)
+        logging.debug(f"Intake suck done")
         return True

rio/commands/rotateIntake.py

@@ -0,0 +1,38 @@
+import commands2
+import logging
+
+from subsystems.intake import IntakeSubsystem
+
+
+class RotateIntake(commands2.Command):
+    def __init__(self, angle: float, subsystem: IntakeSubsystem):
+        """
+        rotates the intake
+        supposed to be used with
+        presets
+        """
+        super().__init__()
+
+        # local subsystem instance
+        self.intakeSubsystem = subsystem
+
+        # requested speed
+        self.angle = angle
+
+    def initialize(self):
+        logging.info(f"Moving intake to {self.angle}")
+
+    def execute(self):
+        self.intakeSubsystem.lift(self.angle)
+        logging.info(
+            f"Still moving... {self.intakeSubsystem.getAngle()} to {self.angle}"
+        )
+
+    def isFinished(self):
+        e = 1  # How close before we're done
+
+        return abs(self.intakeSubsystem.getAngle() - self.angle) < e
+
+    def end(self, interrupted: bool):
+        logging.info(f"Done moving! Finished normally: {interrupted}")
+        return True

rio/commands/setIntakeSpeed.py

@@ -0,0 +1,33 @@
+import logging
+import commands2
+
+from subsystems.intake import IntakeSubsystem
+
+
+class SetIntakeSpeed(commands2.Command):
+    def __init__(self, speed: float, subsystem: IntakeSubsystem):
+        """
+        takes in the rings
+        """
+        super().__init__()
+
+        # local subsystem instance
+        self.intakeSubsystem = subsystem
+
+        # requested speed
+        self.speed = speed
+
+    def initialize(self):
+        logging.debug(f"Running command Intake Suck (manual) with speed {self.speed}")
+
+    def execute(self):
+        if self.speed > 0:
+            if not self.intakeSubsystem.switchPress():
+                self.intakeSubsystem.intake(self.speed)
+        else:
+            self.intakeSubsystem.intake(self.speed)
+
+    def end(self, interrupted: bool):
+        self.intakeSubsystem.intake(0)
+        logging.debug(f"Intake suck done")
+        return True

rio/commands/shooterROT.py

@@ -0,0 +1,18 @@
+import commands2
+
+
+class ShooterROT(commands2.Command):
+    def __init__(self, angle, subsystem):
+        super().__init__()
+
+        # local subsystem instance
+        self.subsystem = subsystem
+
+        # requested speed
+        self.angle = angle
+
+    def initialize(self):
+        self.subsystem.setRotateAngle(self.angle)
+
+    def end(self, interrupted: bool):
+        return True

rio/constants.py

@@ -35,7 +35,7 @@ class DriveConstants:
     # Chassis configuration
     kTrackWidth = units.inchesToMeters(20.3937)
     # Distance between centers of right and left wheels on robot
-    kWheelBase = units.inchesToMeters(20.25)
+    kWheelBase = units.inchesToMeters(20.5)
 
     # Distance between front and back wheels on robot
     kModulePositions = [
@@ -146,6 +146,24 @@ class AutoConstants:
     )
 
 
+class SuperStrucConstants:
+    # CANSpark IDS
+    rotateID = 11
+    flyID = 12
+    guideID = 13
+
+    # PID values
+    kP = 0
+    kI = 0
+    kD = 0
+    kFF = 0
+
+    # current limits
+    rotateCurrentLimit = 1
+    flyCurrentLimit = 1
+    guideCurrentLimit = 1
+
+
 class IntakeConstants:
     # CANSparkMax ports
     kliftCanId = 9
@@ -159,7 +177,7 @@ class IntakeConstants:
     kLiftConversion = 1  # Configured feb 12 by joe
 
     # lift pid
-    kLiftP = 0
-    kLiftI = 0
-    kLiftD = 0
+    kLiftP = 1.2
+    kLiftI = 0.0001
+    kLiftD = 0.5
     kLiftFF = 0

rio/robot.py

@@ -7,6 +7,7 @@
 
 import commands2
 import wpilib
+import logging
 
 from robotcontainer import RobotContainer
 
@@ -18,6 +19,12 @@ def robotInit(self):
         self.container = RobotContainer()
         self.autonomousCommand = None
 
+        if not wpilib.RobotBase.isReal():
+            # Do some things if the robot is NOT real
+
+            # Ovverride default logging
+            logging.basicConfig(level=logging.DEBUG)
+
     def autonomousInit(self) -> None:
         self.autonomousCommand = self.container.getAutonomousCommand()