Merge pull request #28 from FRC-1721/feat/gearReduction

Gear Reduction Refactor

rio/commands/climb.py

@@ -1,10 +1,11 @@
 import commands2
 import rev
 
-from constants import ClimberConstants
+from constants import ClimberConstants, SuperStrucConstants
 
 from subsystems.climber import Climber
 from subsystems.shooter import Shooter
+import logging
 
 
 class Climb(commands2.Command):
@@ -25,6 +26,7 @@ def __init__(
 
         # local var of speed
         self.speed = _speed
+        self.time = 0
 
         # Command requires
         self.addRequirements((self.climber, self.shooter))
@@ -33,7 +35,13 @@ def __init__(
         self.climber.setServoAngle(ClimberConstants.kservoOpen)
 
     def execute(self):
-        self.climber.setClimberMotorSpeed(self.speed)
+        angle = self.shooter.getAngle()
+        softLimit = SuperStrucConstants.ShootPos - 20
+
+        if angle <= softLimit:
+            self.climber.setClimberMotorSpeed(self.speed)
+        else:
+            logging.warn(f"You've gone too far! {angle}/{softLimit}")
         self.shooter.setIdleMode(rev._rev.CANSparkBase.IdleMode.kBrake)
 
     def end(self, interrupted: bool):

rio/commands/defaultFlywheel.py

@@ -0,0 +1,31 @@
+import logging
+import commands2
+
+from subsystems.shooter import Shooter
+
+
+class DefaultFlywheel(commands2.Command):
+    def __init__(self, speed, _shooter: Shooter):
+        super().__init__()
+
+        # local subsystem instance
+        self.shooter = _shooter
+
+        # requested speed
+        self.speed = speed
+
+        # Command requirements
+        self.addRequirements(self.shooter)
+
+    def execute(self):
+        if self.speed() > 0.1:
+            self.shooter.setFlyWheelSpeed(self.speed())
+            logging.debug(f"Flywheel at speed {self.shooter.currentSpeed()}")
+
+    def end(self, interrupted: bool):
+        if not interrupted:
+            logging.info(f"Default is done with speed {self.isFinished()}")
+        else:
+            logging.warn("Default Flywheel was interrupted!")
+
+        return True

rio/commands/spool.py

@@ -42,16 +42,16 @@ def execute(self):
             # Positive numbers imply we're pulling DOWN
             self.shooter.currlimit(1)
             self.climber.setServoAngle(ClimberConstants.kServoLock)
-            self.shooter.rotateManual(
-                -self.speed * ClimberConstants.kClimberShooterForward
-            )
+            # self.shooter.rotateManual(
+            #    -self.speed * ClimberConstants.kClimberShooterForward
+            # )
         else:
             # Negative numbers imply we're relaxing
             self.shooter.currlimit(2)
             self.climber.setServoAngle(ClimberConstants.kservoOpen)
-            self.shooter.rotateManual(
-                -self.speed * ClimberConstants.kClimberShooterBackward
-            )
+            # self.shooter.rotateManual(
+            #    -self.speed * ClimberConstants.kClimberShooterBackward
+            # )
 
         self.climber.setClimberMotorSpeed(self.speed)
 

rio/constants.py

@@ -124,6 +124,7 @@ class OIConstants:
     # driver controller
     kDriverControllerPort = 0
     kDriveDeadband = 0.075
+    kDampeningAmount = 0.15
 
     # operator controller
     kOpControllerPort = 1
@@ -150,6 +151,7 @@ class SuperStrucConstants:
     # angles for shooter
     ShootPos = 321
     LoadPos = 204
+    ClimbPos = 277
 
     # CANSpark IDS
     rotateID = 11
@@ -161,7 +163,7 @@ class SuperStrucConstants:
     krotateInversion = True
 
     # PID values
-    krotateP = 0.02
+    krotateP = 0.01
     krotateI = 0.0
     krotateD = 0.0
     krotateFF = 0
@@ -182,9 +184,9 @@ class IntakeConstants:
     kIntakeInversion = True
 
     # conversion factor
-    kLiftConversion = 360  # Configured feb 12 by joe
-    SuckPos = 120
-    BlowPos = 1
+    kLiftConversion = 1  # Configured feb 12 by joe
+    SuckPos = 31
+    BlowPos = 0
     # lift pid
     kLiftP = 0.075
     kLiftI = 0.00000000000001

rio/robotcontainer.py

@@ -53,6 +53,7 @@
 from commands.ResetYaw import ResetYaw
 from commands.spool import Spool
 from commands.lock import Lock
+from commands.defaultFlywheel import DefaultFlywheel
 
 # auto
 from autonomous.noAuto import NoAuto
@@ -103,21 +104,25 @@ def __init__(self) -> None:
             # Turning is controlled by the X axis of the right stick.
             commands2.cmd.run(
                 lambda: self.robotDrive.drive(
-                    -wpimath.applyDeadband(
-                        self.driverController.getRawAxis(1),
-                        OIConstants.kDriveDeadband,  # TODO: Use constants to set these controls
-                    )
-                    * 0.5,
-                    wpimath.applyDeadband(
-                        self.driverController.getRawAxis(0),
-                        OIConstants.kDriveDeadband,  # TODO: Use constants to set these controls
-                    )
-                    * 0.5,
-                    -wpimath.applyDeadband(
-                        self.driverController.getRawAxis(4),
-                        OIConstants.kDriveDeadband,  # TODO: Use constants to set these controls
-                    )
-                    * 0.5,
+                    (
+                        -wpimath.applyDeadband(
+                            self.driverController.getRawAxis(1),
+                            OIConstants.kDriveDeadband,  # TODO: Use constants to set these controls
+                        )
+                    ),
+                    (
+                        wpimath.applyDeadband(
+                            self.driverController.getRawAxis(0),
+                            OIConstants.kDriveDeadband,  # TODO: Use constants to set these controls
+                        )
+                    ),
+                    (
+                        -wpimath.applyDeadband(
+                            self.driverController.getRawAxis(4),
+                            OIConstants.kDriveDeadband,  # TODO: Use constants to set these controls
+                        )
+                    ),
+                    self.driverController.leftTrigger(),
                     lambda: self.fieldCentricChooser.getSelected() == "Field Centric",
                     True,
                 ),
@@ -126,14 +131,18 @@ def __init__(self) -> None:
         )
 
         self.shooter.setDefaultCommand(
-            commands2.cmd.run(
-                lambda: self.shooter.setFlyWheelSpeed(
-                    self.opController.getRawAxis(2) + 0.16
-                ),
-                self.shooter,
-            )
+            DefaultFlywheel(lambda: self.opController.getRawAxis(2), self.shooter)
         )
 
+        # self.shooter.setDefaultCommand(
+        #     commands2.cmd.run(
+        #         lambda: self.shooter.setFlyWheelSpeed(
+        #             self.opController.getRawAxis(2) + 0.16
+        #         ),
+        #         self.shooter,
+        #     )
+        # )
+
     def configureButtonBindings(self) -> None:
         """
         Use this method to define your button->command mappings. Buttons can be created by
@@ -151,27 +160,16 @@ def configureButtonBindings(self) -> None:
                 ShooterROT(SuperStrucConstants.LoadPos, self.shooter),
                 FlyWheelSpeed(0, self.shooter),  # Stop shooter (if its running)
                 RotateIntake(IntakeConstants.SuckPos, self.intake),  # Put intake down
-                intakeUntilNote(0.5, self.intake),  # Intake till note
+                intakeUntilNote(1, self.intake),  # Intake till note
                 RotateIntake(
                     IntakeConstants.BlowPos, self.intake
                 ),  # Put intake back up
             )
         )
 
-        # Shoot to speaker (button y)
+        # set Shooter to climb position
         self.driverController.y().onTrue(
-            commands2.SequentialCommandGroup(
-                RotateIntake(
-                    IntakeConstants.BlowPos, self.intake
-                ),  # Put intake fully inside (if it wasn't already)
-                FlyWheelSpeed(1.0, self.shooter, False),  # Power up the flywheels (?)
-                SetIntakeSpeed(
-                    -0.6, self.intake
-                ),  # Load magazine? (but without ending)
-                commands2.WaitCommand(3),
-                FlyWheelSpeed(0.0, self.shooter),  # Stop flywheel
-                SetIntakeSpeed(0, self.intake),  # Stop intake
-            )
+            ShooterROT(SuperStrucConstants.ClimbPos, self.shooter)
         )
 
         # Deliver to amp (button a), part a
@@ -193,8 +191,11 @@ def configureButtonBindings(self) -> None:
         # Deliver to amp (button b), part b
         self.driverController.b().onTrue(
             commands2.SequentialCommandGroup(
-                FlyWheelSpeed(0.3, self.shooter, False),  # rotates the Flywheel
-                commands2.WaitCommand(2),
+                ShooterROT(
+                    SuperStrucConstants.ShootPos, self.shooter
+                ),  # Rotate the shooter
+                FlyWheelSpeed(0.5, self.shooter, False),  # rotates the Flywheel
+                commands2.WaitCommand(0.375),
                 ShooterROT(
                     SuperStrucConstants.LoadPos, self.shooter
                 ),  # Rotate the shooter
@@ -205,10 +206,16 @@ def configureButtonBindings(self) -> None:
         self.driverController.start().onTrue(ResetYaw(self.robotDrive))
 
         # Climbing
-        self.driverController.rightBumper().whileTrue(
+        self.driverController.pov(0).whileTrue(
             Climb(
-                lambda: self.opController.getRightTriggerAxis()
-                - self.opController.getLeftTriggerAxis(),
+                1,
+                self.climber,
+                self.shooter,
+            )
+        )
+        self.driverController.pov(180).whileTrue(
+            Climb(
+                -1,
                 self.climber,
                 self.shooter,
             )
@@ -217,21 +224,43 @@ def configureButtonBindings(self) -> None:
         self.driverController.leftBumper().whileTrue(
             sendToObject(self.robotDrive, self.limelight)
         )
-        self.driverController.rightBumper().whileTrue(
+        self.driverController.back().whileTrue(
             commands2.InstantCommand(self.intake.zeroIntake())
         )
 
+        self.driverController.rightTrigger().whileTrue(
+            commands2.ParallelCommandGroup(
+                RotateIntake(
+                    IntakeConstants.BlowPos, self.intake
+                ),  # Put intake fully inside (if it wasn't already)
+                FlyWheelSpeed(1, self.shooter, False),
+            )
+        )
+        self.driverController.rightTrigger().onFalse(
+            commands2.SequentialCommandGroup(
+                FlyWheelSpeed(1, self.shooter, False),
+                SetIntakeSpeed(-1, self.intake),
+                commands2.WaitCommand(3),
+                FlyWheelSpeed(0.0, self.shooter),  # Stop flywheel
+                SetIntakeSpeed(0, self.intake),  # Stop intake
+            )
+        )
+
         # ==============================
         #        Operator Commands
         # ===============180===============
         # intake keybinds
         # intake movement
-        self.opController.button(2).whileTrue(IntakeRotationMAN(1, self.intake))  # out
-        self.opController.button(1).whileTrue(IntakeRotationMAN(-1, self.intake))  # in
+        self.opController.button(2).whileTrue(
+            IntakeRotationMAN(0.3, self.intake)
+        )  # out
+        self.opController.button(1).whileTrue(
+            IntakeRotationMAN(-0.3, self.intake)
+        )  # in
 
         # intake spin
-        self.opController.button(6).whileTrue(SetIntakeSpeed(0.6, self.intake))
-        self.opController.button(9).whileTrue(SetIntakeSpeed(-0.6, self.intake))
+        self.opController.button(6).whileTrue(SetIntakeSpeed(1.0, self.intake))
+        self.opController.button(9).whileTrue(SetIntakeSpeed(-1.0, self.intake))
 
         self.opController.button(9).whileFalse(SetIntakeSpeed(0, self.intake))
 
@@ -241,8 +270,8 @@ def configureButtonBindings(self) -> None:
         self.opController.button(4).whileTrue(manualROT(-0.5, self.shooter))
 
         # climber
-        self.opController.button(5).whileTrue(Spool(0.2, self.climber, self.shooter))
-        self.opController.button(7).whileTrue(Spool(-0.1, self.climber, self.shooter))
+        self.opController.button(5).whileTrue(Spool(0.4, self.climber, self.shooter))
+        self.opController.button(7).whileTrue(Spool(-0.2, self.climber, self.shooter))
 
         # Cancel all
         self.opController.button(8).onTrue(commands2.InstantCommand(self.cancelAll))

rio/simgui-window.json

@@ -26,7 +26,7 @@
     "###FMS": {
       "Collapsed": "0",
       "Pos": "5,540",
-      "Size": "283,140"
+      "Size": "169,184"
     },
     "###Joysticks": {
       "Collapsed": "0",

rio/simgui.json

@@ -16,6 +16,7 @@
   "NTProvider": {
     "types": {
       "/FMSInfo": "FMSInfo",
+      "/SmartDashboard/Auto/Mode": "String Chooser",
       "/SmartDashboard/Autonomous": "String Chooser",
       "/SmartDashboard/FieldCentric": "String Chooser"
     }

rio/subsystems/drivesubsystem.py

@@ -17,7 +17,7 @@
 
 from ntcore import NetworkTableInstance
 
-from constants import DriveConstants, GyroConstants
+from constants import DriveConstants, GyroConstants, OIConstants
 
 import utils.swerveutils as swerveutils
 
@@ -192,6 +192,7 @@ def drive(
         xSpeed: float,
         ySpeed: float,
         rot: float,
+        dampened: bool,
         fieldRelative: typing.Callable[
             [],
             bool,
@@ -207,8 +208,8 @@ def drive(
                               field.
         :param rateLimit:     Whether to enable rate limiting for smoother control.
         """
-        xSpeedCommanded = xSpeed
-        ySpeedCommanded = ySpeed
+        xSpeedCommanded = xSpeed * (OIConstants.kDampeningAmount if dampened else 1)
+        ySpeedCommanded = ySpeed * (OIConstants.kDampeningAmount if dampened else 1)
         self.sd.putNumber("pos/rot", rot)
         if False:  # if rateLimit
             # Convert XY to polar for rate limiting
@@ -271,7 +272,9 @@ def drive(
             self.currentRotation = self.rotLimiter.calculate(rot)
 
         else:
-            self.currentRotation = rot
+            self.currentRotation = rot * (
+                OIConstants.kDampeningAmount if dampened else 1
+            )
 
         # Convert the commanded speeds into the correct units for the drivetrain
         xSpeedDelivered = xSpeedCommanded * DriveConstants.kMaxSpeedMetersPerSecond
@@ -283,7 +286,7 @@ def drive(
                 xSpeedDelivered,
                 ySpeedDelivered,
                 rotDelivered,
-                Rotation2d.fromDegrees(self.gyro.getYaw()),
+                Rotation2d.fromDegrees(-self.gyro.getYaw()),
             )
             if fieldRelative
             else ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered)