Feat/auto odometry

rio/autonomous/grabNote.py

@@ -13,7 +13,10 @@
 from commands.NavToObj import sendToObject
 from commands.shooterROT import ShooterROT
 from commands.intakeUntilNote import intakeUntilNote
-from commands.SendToPos import sendToFieldPos
+from commands.poseReset import PoseReset
+from commands.zeroPose import zeroPose
+
+from constants import IntakeConstants
 
 
 class GrabNote(commands2.SequentialCommandGroup):
@@ -30,10 +33,13 @@ def __init__(
         super().__init__(
             # Resets Yaw relative to the robot's starting position
             ResetYaw(_drive),
+            zeroPose(_drive),
             # ============ #
             # SPEAKER SHOT #
             # ============ #
-            RotateIntake(0, _intake),  # Put intake fully inside (if it wasn't already)
+            RotateIntake(
+                IntakeConstants.BlowPos, _intake
+            ),  # Put intake fully inside (if it wasn't already)
             print("intake rotatered"),
             FlyWheelSpeed(1.0, _shooter, False),  # Power up the flywheels (?)
             print("wheels r fly"),
@@ -47,10 +53,13 @@ def __init__(
             sendToObject(_drive, _limelight),
             ShooterROT(SuperStrucConstants.LoadPos, _shooter),
             FlyWheelSpeed(0, _shooter),  # Stop shooter (if its running)
-            RotateIntake(130, _intake),  # Put intake down (with a lil extra squeeze)
+            RotateIntake(
+                IntakeConstants.SuckPos, _intake
+            ),  # Put intake down (with a lil extra squeeze)
             intakeUntilNote(0.5, _intake),  # Intake till note
-            RotateIntake(0, _intake),  # Put intake back up
+            RotateIntake(IntakeConstants.BlowPos, _intake),  # Put intake back up
             # ================= #
             # RETURN TO SPEAKER #
             # ================= #
+            PoseReset(_drive),
         )

rio/commands/poseReset.py

@@ -0,0 +1,50 @@
+import commands2
+import logging
+
+from subsystems.drivesubsystem import DriveSubsystem
+from wpimath.geometry import Rotation2d
+
+
+class PoseReset(commands2.Command):
+    def __init__(
+        self,
+        _drivetrain=DriveSubsystem,
+    ):
+        super().__init__()
+
+        # local subsystem instance
+        self.drivetrain = _drivetrain
+
+    def initialize(self):
+        # what do I put here? :3
+        pass
+
+    def execute(self):
+        driveX = 0
+        driveY = 0
+        rotZ = 0
+        Pose = self.drivetrain.getPose()
+
+        if Pose.X() > 0:
+            driveX = 0.1
+        elif Pose.X() < 0:
+            driveX = -0.1
+
+        if Pose.Y() > 0:
+            driveY = 0.1
+        elif Pose.Y() < 0:
+            driveY = -0.1
+
+        if Pose.rotation().degrees() > 0:
+            rotZ = 0.1
+        elif Pose.rotation().degrees() < 0:
+            rotZ = -0.1
+
+        self.drivetrain.drive(driveX, driveY, rotZ)
+
+    def isFinished(self):
+        return self.drivetrain.isPoseZero()
+
+    def end(self, interrupted: bool):
+        logging.info("Yaw iz zero mah boi")
+        return True

rio/commands/zeroPose.py

@@ -0,0 +1,29 @@
+import commands2
+import logging
+
+from subsystems.drivesubsystem import DriveSubsystem
+from wpimath.geometry import Pose2d, Rotation2d
+
+
+class zeroPose(commands2.Command):
+    def __init__(
+        self,
+        _drivetrain=DriveSubsystem,
+    ):
+        super().__init__()
+
+        # local subsystem instance
+        self.subsystem = _drivetrain
+
+    def initialize(self):
+        self.subsystem.resetOdometry(Pose2d.fromFeet(0, 0, Rotation2d.fromDegrees(0)))
+
+    def execute(self):
+        pass
+
+    def isFinished(self):
+        return True
+
+    def end(self, interrupted: bool):
+        logging.info("Pose iz zero mah boi")
+        return True

rio/constants.py

@@ -182,12 +182,13 @@ class IntakeConstants:
     kIntakeInversion = False
 
     # conversion factor
-    kLiftConversion = 1  # Configured feb 12 by joe
-
+    kLiftConversion = 360  # Configured feb 12 by joe
+    SuckPos = 197  # 347
+    BlowPos = 1
     # lift pid
-    kLiftP = 0.8
-    kLiftI = 0.00001
-    kLiftD = 1.6
+    kLiftP = 0.075
+    kLiftI = 0.00000000000001
+    kLiftD = 0.2
     kLiftFF = 0
 
 

rio/robotcontainer.py

@@ -20,7 +20,13 @@
 from commands2 import cmd
 from commands2.button import CommandXboxController, CommandGenericHID
 
-from constants import AutoConstants, DriveConstants, OIConstants, SuperStrucConstants
+from constants import (
+    AutoConstants,
+    DriveConstants,
+    OIConstants,
+    SuperStrucConstants,
+    IntakeConstants,
+)
 
 # Subsystems
 from subsystems.drivesubsystem import DriveSubsystem
@@ -94,21 +100,24 @@ def __init__(self) -> None:
             # Turning is controlled by the X axis of the right stick.
             commands2.cmd.run(
                 lambda: self.robotDrive.drive(
-                    -wpimath.applyDeadband(
+                    2
+                    ** -wpimath.applyDeadband(
                         self.driverController.getRawAxis(1),
                         OIConstants.kDriveDeadband,  # TODO: Use constants to set these controls
                     )
-                    * 0.5,
-                    -wpimath.applyDeadband(
+                    - 1,
+                    2
+                    ** -wpimath.applyDeadband(
                         self.driverController.getRawAxis(0),
                         OIConstants.kDriveDeadband,  # TODO: Use constants to set these controls
                     )
-                    * 0.5,
-                    -wpimath.applyDeadband(
+                    - 1,
+                    2
+                    ** -wpimath.applyDeadband(
                         self.driverController.getRawAxis(4),
                         OIConstants.kDriveDeadband,  # TODO: Use constants to set these controls
                     )
-                    * 0.5,
+                    - 1,
                     True,
                     True,
                 ),
@@ -132,17 +141,19 @@ def configureButtonBindings(self) -> None:
             commands2.SequentialCommandGroup(
                 ShooterROT(SuperStrucConstants.LoadPos, self.shooter),
                 FlyWheelSpeed(0, self.shooter),  # Stop shooter (if its running)
-                RotateIntake(120, self.intake),  # Put intake down
+                RotateIntake(IntakeConstants.SuckPos, self.intake),  # Put intake down
                 intakeUntilNote(0.5, self.intake),  # Intake till note
-                RotateIntake(0, self.intake),  # Put intake back up
+                RotateIntake(
+                    IntakeConstants.BlowPos, self.intake
+                ),  # Put intake back up
             )
         )
 
         # Shoot to speaker (button y)
         self.driverController.y().onTrue(
             commands2.SequentialCommandGroup(
                 RotateIntake(
-                    0, self.intake
+                    IntakeConstants.BlowPos, self.intake
                 ),  # Put intake fully inside (if it wasn't already)
                 FlyWheelSpeed(1.0, self.shooter, False),  # Power up the flywheels (?)
                 SetIntakeSpeed(
@@ -157,7 +168,9 @@ def configureButtonBindings(self) -> None:
         # Deliver to amp (button a), part a
         self.driverController.a().onTrue(
             commands2.SequentialCommandGroup(
-                RotateIntake(0, self.intake),  # Rotate to fully closed
+                RotateIntake(
+                    IntakeConstants.BlowPos, self.intake
+                ),  # Rotate to fully closed
                 # SetIntakeSpeed(-0.6, self.intake),  # Eject slowly
                 LoadMagazine(self.shooter, self.intake),  # Load the magazine
                 # SetIntakeSpeed(0, self.intake),  # Stop ejecting
@@ -182,10 +195,6 @@ def configureButtonBindings(self) -> None:
 
         self.driverController.start().onTrue(ResetYaw(self.robotDrive))
 
-        self.driverController.back().onTrue(
-            commands2.InstantCommand(self.intake.zeroIntake)
-        )
-
         # Climbing
         self.driverController.rightBumper().whileTrue(
             Climb(
@@ -210,6 +219,8 @@ def configureButtonBindings(self) -> None:
         self.opController.button(6).whileTrue(SetIntakeSpeed(0.6, self.intake))
         self.opController.button(9).whileTrue(SetIntakeSpeed(-0.6, self.intake))
 
+        self.opController.button(6).whileFalse(SetIntakeSpeed(0, self.intake))
+
         # shooter keybinds
         # shooter movement
         self.opController.button(3).whileTrue(manualROT(0.5, self.shooter))

rio/subsystems/drivesubsystem.py

@@ -92,23 +92,23 @@ def __init__(self) -> None:
                 self.rearRight.getPosition(),
             ),
         )
+        self.gyroAngle = self.gyro.getYaw()
 
         # self.gyro.setAngleAdjustment(90)
 
     def periodic(self) -> None:
-        self.sd.putNumberArray("Accel", self.gyro.getBiasedAccelerometer()[1])
         # Update the odometry in the periodic block
         # print(self.getHeading())
         # print(self.gyro.getBiasedAccelerometer())
-        """self.odometry.update(
-            Rotation2d.fromDegrees(self.gyro.accel),
+        self.odometry.update(
+            Rotation2d.fromDegrees(self.gyroAngle),
             (
                 self.frontLeft.getPosition(),
                 self.frontRight.getPosition(),
                 self.rearLeft.getPosition(),
                 self.rearRight.getPosition(),
             ),
-        )"""
+        )
 
         # Desired Positions
         self.sd.putNumber(
@@ -163,6 +163,13 @@ def getPose(self) -> Pose2d:
         """
         return self.odometry.getPose()
 
+    def isPoseZero(self):
+        Pose = self.odometry.getPose()
+        for item in Pose:
+            if item != 0:
+                return False
+        return True
+
     def resetOdometry(self, pose: Pose2d) -> None:
         """Resets the odometry to the specified pose.
 

rio/subsystems/intake.py

@@ -40,7 +40,9 @@ def __init__(self) -> None:
         self.intakeMotor.setInverted(True)
 
         # encoders
-        self.liftEncoder = self.liftMotor.getEncoder()
+        self.liftEncoder = self.liftMotor.getAbsoluteEncoder(
+            SparkAbsoluteEncoder.Type.kDutyCycle
+        )
 
         self.liftEncoder.setPositionConversionFactor(IntakeConstants.kLiftConversion)
 
@@ -52,6 +54,8 @@ def __init__(self) -> None:
 
         # pids
         self.liftPID = self.liftMotor.getPIDController()
+        self.liftPID.setFeedbackDevice(self.liftEncoder)
+        self.liftPID.setPositionPIDWrappingEnabled(False)
         self.liftPID.setP(IntakeConstants.kLiftP)
         self.liftPID.setI(IntakeConstants.kLiftI)
         self.liftPID.setD(IntakeConstants.kLiftD)
@@ -93,6 +97,3 @@ def manualLift(self, speed):
 
     def lift(self, angle: float):
         self.liftPID.setReference(angle, CANSparkMax.ControlType.kPosition)
-
-    def zeroIntake(self):
-        self.liftEncoder.setPosition(0)