From 83145cf8e5bf7f2a9ea6284c5c313691bb04054a Mon Sep 17 00:00:00 2001 From: NotSimon5673 Date: Thu, 14 Mar 2024 17:08:41 -0400 Subject: [PATCH 1/3] blah blah blah --- rio/autonomous/grabNote.py | 3 +- rio/commands/poseReset.py | 49 ++++++++++++++++++++++++++++++++ rio/robotcontainer.py | 4 --- rio/simgui-window.json | 4 +-- rio/subsystems/drivesubsystem.py | 15 +++++++--- rio/subsystems/intake.py | 8 +++--- 6 files changed, 68 insertions(+), 15 deletions(-) create mode 100644 rio/commands/poseReset.py diff --git a/rio/autonomous/grabNote.py b/rio/autonomous/grabNote.py index 0667cb9..5fae822 100644 --- a/rio/autonomous/grabNote.py +++ b/rio/autonomous/grabNote.py @@ -13,7 +13,7 @@ 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 class GrabNote(commands2.SequentialCommandGroup): @@ -53,4 +53,5 @@ def __init__( # ================= # # RETURN TO SPEAKER # # ================= # + PoseReset(_drive), ) diff --git a/rio/commands/poseReset.py b/rio/commands/poseReset.py new file mode 100644 index 0000000..9f99503 --- /dev/null +++ b/rio/commands/poseReset.py @@ -0,0 +1,49 @@ +import commands2 +import logging + +from subsystems.drivesubsystem import DriveSubsystem + + +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[0] > 0: + driveX = 0.1 + elif Pose[0] < 0: + driveX = -0.1 + + if Pose[1] > 0: + driveY = 0.1 + elif Pose[1] < 0: + driveY = -0.1 + + if Pose[2] > 0: + rotZ = 0.1 + elif Pose[2] < 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 diff --git a/rio/robotcontainer.py b/rio/robotcontainer.py index a345302..cf62db8 100644 --- a/rio/robotcontainer.py +++ b/rio/robotcontainer.py @@ -182,10 +182,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( diff --git a/rio/simgui-window.json b/rio/simgui-window.json index c93cc85..785949e 100644 --- a/rio/simgui-window.json +++ b/rio/simgui-window.json @@ -26,7 +26,7 @@ "###FMS": { "Collapsed": "0", "Pos": "5,540", - "Size": "169,184" + "Size": "283,140" }, "###Joysticks": { "Collapsed": "0", @@ -61,7 +61,7 @@ "###Timing": { "Collapsed": "0", "Pos": "5,150", - "Size": "135,150" + "Size": "133,127" }, "Debug##Default": { "Collapsed": "0", diff --git a/rio/subsystems/drivesubsystem.py b/rio/subsystems/drivesubsystem.py index 7b34757..cd8f165 100644 --- a/rio/subsystems/drivesubsystem.py +++ b/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. diff --git a/rio/subsystems/intake.py b/rio/subsystems/intake.py index 2570e47..23ed30d 100644 --- a/rio/subsystems/intake.py +++ b/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) @@ -77,6 +79,7 @@ def periodic(self) -> None: self.sd.putNumber("Thermals/Intake", self.intakeMotor.getMotorTemperature()) self.sd.putNumber("Intake/IntakeAngle", self.intakeEncoder.getPosition()) self.sd.putNumber("Thermals/Lift", self.liftMotor.getMotorTemperature()) + # print(self.liftEncoder.getPosition()) def intake(self, speed): self.intakeMotor.set(speed) @@ -93,6 +96,3 @@ def manualLift(self, speed): def lift(self, angle: float): self.liftPID.setReference(angle, CANSparkMax.ControlType.kPosition) - - def zeroIntake(self): - self.liftEncoder.setPosition(0) From 84d281504f5e2dfe0caefb9334a70d7e8aebb50a Mon Sep 17 00:00:00 2001 From: NotSimon5673 Date: Thu, 14 Mar 2024 19:09:36 -0400 Subject: [PATCH 2/3] Intake work --- rio/autonomous/grabNote.py | 2 +- rio/constants.py | 11 ++++++----- rio/robotcontainer.py | 18 ++++++++++++++---- rio/subsystems/intake.py | 4 +++- 4 files changed, 24 insertions(+), 11 deletions(-) diff --git a/rio/autonomous/grabNote.py b/rio/autonomous/grabNote.py index 5fae822..789c47c 100644 --- a/rio/autonomous/grabNote.py +++ b/rio/autonomous/grabNote.py @@ -47,7 +47,7 @@ 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(199, _intake), # Put intake down (with a lil extra squeeze) intakeUntilNote(0.5, _intake), # Intake till note RotateIntake(0, _intake), # Put intake back up # ================= # diff --git a/rio/constants.py b/rio/constants.py index 4714b01..33b43ac 100644 --- a/rio/constants.py +++ b/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 = 350 # 347 + BlowPos = 150 # lift pid - kLiftP = 0.8 - kLiftI = 0.00001 - kLiftD = 1.6 + kLiftP = 0.075 + kLiftI = 0.00000000000001 + kLiftD = 0.2 kLiftFF = 0 diff --git a/rio/robotcontainer.py b/rio/robotcontainer.py index cf62db8..7959c3f 100644 --- a/rio/robotcontainer.py +++ b/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 @@ -132,9 +138,11 @@ 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 ) ) @@ -142,7 +150,7 @@ def configureButtonBindings(self) -> None: 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( @@ -206,6 +214,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)) diff --git a/rio/subsystems/intake.py b/rio/subsystems/intake.py index 23ed30d..365b40c 100644 --- a/rio/subsystems/intake.py +++ b/rio/subsystems/intake.py @@ -54,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) @@ -79,7 +81,7 @@ def periodic(self) -> None: self.sd.putNumber("Thermals/Intake", self.intakeMotor.getMotorTemperature()) self.sd.putNumber("Intake/IntakeAngle", self.intakeEncoder.getPosition()) self.sd.putNumber("Thermals/Lift", self.liftMotor.getMotorTemperature()) - # print(self.liftEncoder.getPosition()) + print(self.liftEncoder.getPosition()) def intake(self, speed): self.intakeMotor.set(speed) From e0c01ec625bbe1051836fbece47ca4656dbf9726 Mon Sep 17 00:00:00 2001 From: NotSimon5673 Date: Fri, 15 Mar 2024 09:36:34 -0400 Subject: [PATCH 3/3] 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 --- rio/autonomous/grabNote.py | 14 +++++++++++--- rio/commands/poseReset.py | 13 +++++++------ rio/commands/zeroPose.py | 29 +++++++++++++++++++++++++++++ rio/constants.py | 4 ++-- rio/robotcontainer.py | 19 ++++++++++++------- rio/simgui-window.json | 4 ++-- rio/subsystems/intake.py | 1 - 7 files changed, 63 insertions(+), 21 deletions(-) create mode 100644 rio/commands/zeroPose.py diff --git a/rio/autonomous/grabNote.py b/rio/autonomous/grabNote.py index 789c47c..45de333 100644 --- a/rio/autonomous/grabNote.py +++ b/rio/autonomous/grabNote.py @@ -14,6 +14,9 @@ from commands.shooterROT import ShooterROT from commands.intakeUntilNote import intakeUntilNote 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,9 +53,11 @@ def __init__( sendToObject(_drive, _limelight), ShooterROT(SuperStrucConstants.LoadPos, _shooter), FlyWheelSpeed(0, _shooter), # Stop shooter (if its running) - RotateIntake(199, _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 # # ================= # diff --git a/rio/commands/poseReset.py b/rio/commands/poseReset.py index 9f99503..e377a6f 100644 --- a/rio/commands/poseReset.py +++ b/rio/commands/poseReset.py @@ -2,6 +2,7 @@ import logging from subsystems.drivesubsystem import DriveSubsystem +from wpimath.geometry import Rotation2d class PoseReset(commands2.Command): @@ -24,19 +25,19 @@ def execute(self): rotZ = 0 Pose = self.drivetrain.getPose() - if Pose[0] > 0: + if Pose.X() > 0: driveX = 0.1 - elif Pose[0] < 0: + elif Pose.X() < 0: driveX = -0.1 - if Pose[1] > 0: + if Pose.Y() > 0: driveY = 0.1 - elif Pose[1] < 0: + elif Pose.Y() < 0: driveY = -0.1 - if Pose[2] > 0: + if Pose.rotation().degrees() > 0: rotZ = 0.1 - elif Pose[2] < 0: + elif Pose.rotation().degrees() < 0: rotZ = -0.1 self.drivetrain.drive(driveX, driveY, rotZ) diff --git a/rio/commands/zeroPose.py b/rio/commands/zeroPose.py new file mode 100644 index 0000000..f1d5968 --- /dev/null +++ b/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 diff --git a/rio/constants.py b/rio/constants.py index 33b43ac..42528c0 100644 --- a/rio/constants.py +++ b/rio/constants.py @@ -183,8 +183,8 @@ class IntakeConstants: # conversion factor kLiftConversion = 360 # Configured feb 12 by joe - SuckPos = 350 # 347 - BlowPos = 150 + SuckPos = 197 # 347 + BlowPos = 1 # lift pid kLiftP = 0.075 kLiftI = 0.00000000000001 diff --git a/rio/robotcontainer.py b/rio/robotcontainer.py index 7959c3f..9bcc021 100644 --- a/rio/robotcontainer.py +++ b/rio/robotcontainer.py @@ -100,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, ), @@ -165,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 diff --git a/rio/simgui-window.json b/rio/simgui-window.json index 785949e..c93cc85 100644 --- a/rio/simgui-window.json +++ b/rio/simgui-window.json @@ -26,7 +26,7 @@ "###FMS": { "Collapsed": "0", "Pos": "5,540", - "Size": "283,140" + "Size": "169,184" }, "###Joysticks": { "Collapsed": "0", @@ -61,7 +61,7 @@ "###Timing": { "Collapsed": "0", "Pos": "5,150", - "Size": "133,127" + "Size": "135,150" }, "Debug##Default": { "Collapsed": "0", diff --git a/rio/subsystems/intake.py b/rio/subsystems/intake.py index 365b40c..6309038 100644 --- a/rio/subsystems/intake.py +++ b/rio/subsystems/intake.py @@ -81,7 +81,6 @@ def periodic(self) -> None: self.sd.putNumber("Thermals/Intake", self.intakeMotor.getMotorTemperature()) self.sd.putNumber("Intake/IntakeAngle", self.intakeEncoder.getPosition()) self.sd.putNumber("Thermals/Lift", self.liftMotor.getMotorTemperature()) - print(self.liftEncoder.getPosition()) def intake(self, speed): self.intakeMotor.set(speed)