Feat/auto odometry (#25)

* blah blah blah

* Intake work

* the fog is coming the fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is comingthe fog is coming

---------

Co-authored-by: NotSimon5673 <[email protected]>

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,
                 ),
@@ -141,17 +150,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(
@@ -166,7 +177,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
@@ -191,10 +204,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(
@@ -219,6 +228,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)