diff --git a/Pipfile b/Pipfile new file mode 100644 index 00000000..0757494b --- /dev/null +++ b/Pipfile @@ -0,0 +1,11 @@ +[[source]] +url = "https://pypi.org/simple" +verify_ssl = true +name = "pypi" + +[packages] + +[dev-packages] + +[requires] +python_version = "3.11" diff --git a/rio/autonomous/flywithwires.py b/rio/autonomous/flywithwires.py new file mode 100644 index 00000000..1fa572dd --- /dev/null +++ b/rio/autonomous/flywithwires.py @@ -0,0 +1,48 @@ +import commands2 +import wpilib + +from subsystems.drivesubsystem import DriveSubsystem + + +class FlyWithWires(commands2.CommandBase): + def __init__( + self, driveSubsystem: DriveSubsystem, xSpeed=0, ySpeed=0, rot=0, time=-1 + ) -> None: + super().__init__() + + self.driveSubsystem = driveSubsystem + self.addRequirements((self.driveSubsystem)) + + # time and derection vars + self.time = time + self.xSpeed = xSpeed + self.ySpeed = ySpeed + self.rot = rot + + # Timer + self.backgroundTimer = wpilib.Timer() + self.backgroundTimer.start() + + print("initalized") + + def initialize(self) -> None: + self.backgroundTimer.reset() + + def execute(self) -> None: + self.driveSubsystem.drive( + self.xSpeed, + self.ySpeed, + self.rot, + False, + True, + ) + print("running") + + def end(self, interrupted: bool) -> None: + self.driveSubsystem.drive(0, 0, 0, False, True) + print("ended") + + def isFinished(self) -> bool: + if self.time != -1 and self.backgroundTimer.hasElapsed(self.time): + return True + print("finished") diff --git a/rio/autonomous/forwardDrive.py b/rio/autonomous/forwardDrive.py new file mode 100644 index 00000000..369064d8 --- /dev/null +++ b/rio/autonomous/forwardDrive.py @@ -0,0 +1,39 @@ +import commands2 + +from subsystems.drivesubsystem import DriveSubsystem +from subsystems.intake import IntakeSubsystem +from subsystems.shooter import Shooter + +from commands.FlyWheelSpeed import FlyWheelSpeed +from commands.setIntakeSpeed import SetIntakeSpeed + +from autonomous.flywithwires import FlyWithWires + + +class ForwardDrive(commands2.SequentialCommandGroup): + def __init__( + self, + driveSubsystem: DriveSubsystem, + shooterSubsytem: Shooter, + intakeSubsystem: IntakeSubsystem, + ) -> None: + """ + This shoots a note and drives backwards + this wants the note to start in the intake (folded up) + with the limit switch pressed + """ + + super().__init__( + FlyWheelSpeed(1, shooterSubsytem, False), # Power up the flywheels + SetIntakeSpeed(-0.6, intakeSubsystem), # Load magazine + commands2.WaitCommand( + 3 + ), # this needs to be here because it won't hold speed otherwise + FlyWheelSpeed(0.0, shooterSubsytem), # Stop flywheel + SetIntakeSpeed(0, intakeSubsystem).withTimeout(3), # Stop intake + FlyWithWires(driveSubsystem, 1, 0, 0, 2), # driving + ) + + +NAME = "Forward Drive" +load = lambda bot: ForwardDrive(bot.robotDrive, bot.shooter, bot.intake) diff --git a/rio/autonomous/grabNote.py b/rio/autonomous/grabNote.py index 45de333a..887112ae 100644 --- a/rio/autonomous/grabNote.py +++ b/rio/autonomous/grabNote.py @@ -63,3 +63,7 @@ def __init__( # ================= # PoseReset(_drive), ) + + +NAME = "Grab Note" +load = lambda bot: GrabNote(bot.limelight, bot.shooter, bot.intake, bot.robotDrive) diff --git a/rio/autonomous/noAuto.py b/rio/autonomous/noAuto.py index 4144530c..18c166d5 100644 --- a/rio/autonomous/noAuto.py +++ b/rio/autonomous/noAuto.py @@ -5,5 +5,12 @@ class NoAuto(commands2.SequentialCommandGroup): def __init__(self, _drive: DriveSubsystem): - self.drivetrain = _drive - super().__init__(ResetYaw(_drive)) + """ + This doesn't do anything besides + reset yaw + """ + super().__init__(ResetYaw(_drive, 0)) + + +NAME = "No Auto" +load = lambda bot: NoAuto(bot.robotDrive) diff --git a/rio/autonomous/shoot.py b/rio/autonomous/shoot.py new file mode 100644 index 00000000..37ce2f3d --- /dev/null +++ b/rio/autonomous/shoot.py @@ -0,0 +1,49 @@ +import commands2 + +from subsystems.drivesubsystem import DriveSubsystem +from subsystems.intake import IntakeSubsystem +from subsystems.shooter import Shooter + +from commands.rotateIntake import RotateIntake +from commands.FlyWheelSpeed import FlyWheelSpeed +from commands.setIntakeSpeed import SetIntakeSpeed +from commands.ResetYaw import ResetYaw +from commands.zeroPose import zeroPose + +from constants import IntakeConstants + + +class Shoot(commands2.SequentialCommandGroup): + def __init__( + self, + _drive: DriveSubsystem, + _intake: IntakeSubsystem, + _shooter: Shooter, + ): + """ + This just shoots the note, + useful for being with teams + with super autos + """ + super().__init__( + # Resets Yaw relative to the robot's starting position + ResetYaw(_drive), + zeroPose(_drive), + # ============ # + # SPEAKER SHOT # + # ============ # + 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"), + SetIntakeSpeed(-0.6, _intake), # Load magazine? (but without ending) + commands2.WaitCommand(1), + FlyWheelSpeed(0.0, _shooter), # Stop flywheel + SetIntakeSpeed(0, _intake), # Stop intake) + ) + + +NAME = "Shoot" +load = lambda bot: Shoot(bot.robotDrive, bot.intake, bot.shooter) diff --git a/rio/commands/ResetYaw.py b/rio/commands/ResetYaw.py index 5e08e957..753485ca 100644 --- a/rio/commands/ResetYaw.py +++ b/rio/commands/ResetYaw.py @@ -2,6 +2,7 @@ import logging from subsystems.drivesubsystem import DriveSubsystem +from ntcore import NetworkTableInstance class ResetYaw(commands2.Command): @@ -12,12 +13,16 @@ def __init__( ): super().__init__() + # Configure networktables + self.nt = NetworkTableInstance.getDefault() + self.sd = self.nt.getTable("SmartDashboard") + # local subsystem instance self.subsystem = _drivetrain - self.angle = angle def initialize(self): - self.subsystem.resetYaw(self.angle) + print("i b setn' 0 t' " + str(self.sd.getNumber("Auto/Angle", 1))) + self.subsystem.resetYaw(self.sd.getNumber("Auto/Angle", 1)) def execute(self): pass diff --git a/rio/commands/crashDrive.py b/rio/commands/crashDrive.py deleted file mode 100644 index cb2e5045..00000000 --- a/rio/commands/crashDrive.py +++ /dev/null @@ -1,39 +0,0 @@ -import commands2 -import logging - -from subsystems.drivesubsystem import DriveSubsystem - -from wpimath.controller import PIDController - - -class crashDrive(commands2.Command): - def __init__( - self, - _drivetrain=DriveSubsystem, - ): - super().__init__() - - # local subsystem instance - self.subsystem = _drivetrain - self.addRequirements(self.subsystem) - - self.fixHeadingPID = PIDController(0.004, 0.0, 0.0) - - def initialize(self): - pass - - def execute(self): - # Fix our heading with a small pid loop - yawAdjustment = self.fixHeadingPID.calculate(self.subsystem.getHeading(), 180) - logging.info( - f"Calculated yaw adjustment to be {yawAdjustment}, error was {self.subsystem.getHeading()}" - ) - - self.subsystem.drive(-0.2, 0, yawAdjustment, False, False) - - def isFinished(self): - return self.subsystem.getAcc() - - def end(self, interrupted: bool): - logging.info("acceleration iz zer0 mee bio") - return True diff --git a/rio/commands/loadMagazine.py b/rio/commands/loadMagazine.py index 223f7ad3..3c13819e 100644 --- a/rio/commands/loadMagazine.py +++ b/rio/commands/loadMagazine.py @@ -1,5 +1,6 @@ import logging import commands2 +import rev from subsystems.shooter import Shooter from subsystems.intake import IntakeSubsystem @@ -22,7 +23,7 @@ def __init__(self, _shooter: Shooter, _intake: IntakeSubsystem): def initialize(self): logging.info("Spinning up...") - self.shooter.setIdleBrake() + self.shooter.setIdleMode(rev._rev.CANSparkBase.IdleMode.kBrake) def execute(self): self.intake.intake(-0.6) # Slowly eject @@ -41,7 +42,7 @@ def isFinished(self): def end(self, interrupted: bool): self.intake.intake(0) # self.shooter.setFlyWheelSpeed(0) - self.shooter.setIdleCoast() + self.shooter.setIdleMode(rev._rev.CANSparkBase.IdleMode.kCoast) if not interrupted: logging.info(f"Done, magazine loaded") diff --git a/rio/commands/poseReset.py b/rio/commands/poseReset.py index e377a6f2..f3465eb7 100644 --- a/rio/commands/poseReset.py +++ b/rio/commands/poseReset.py @@ -14,9 +14,10 @@ def __init__( # local subsystem instance self.drivetrain = _drivetrain + self.addRequirements((self.drivetrain)) def initialize(self): - # what do I put here? :3 + # what do I put here? :3c pass def execute(self): diff --git a/rio/constants.py b/rio/constants.py index 42528c01..24fb59d6 100644 --- a/rio/constants.py +++ b/rio/constants.py @@ -39,10 +39,10 @@ class DriveConstants: # Distance between front and back wheels on robot kModulePositions = [ - Translation2d(kWheelBase / 2, kTrackWidth / 2), - Translation2d(kWheelBase / 2, -kTrackWidth / 2), Translation2d(-kWheelBase / 2, kTrackWidth / 2), Translation2d(-kWheelBase / 2, -kTrackWidth / 2), + Translation2d(kWheelBase / 2, kTrackWidth / 2), + Translation2d(kWheelBase / 2, -kTrackWidth / 2), ] kDriveKinematics = SwerveDrive4Kinematics(*kModulePositions) @@ -63,7 +63,7 @@ class DriveConstants: kFrontRightTurningCanId = 2 kRearRightTurningCanId = 4 - kGyroReversed = False + kGyroReversed = True class ModuleConstants: @@ -179,11 +179,11 @@ class IntakeConstants: # inversion kLiftInversion = False - kIntakeInversion = False + kIntakeInversion = True # conversion factor kLiftConversion = 360 # Configured feb 12 by joe - SuckPos = 197 # 347 + SuckPos = 120 BlowPos = 1 # lift pid kLiftP = 0.075 diff --git a/rio/robotcontainer.py b/rio/robotcontainer.py index ef097350..b288cd04 100644 --- a/rio/robotcontainer.py +++ b/rio/robotcontainer.py @@ -4,6 +4,7 @@ import logging import os import importlib +import commands2 # wpimath import wpimath @@ -52,9 +53,11 @@ from commands.ResetYaw import ResetYaw from commands.spool import Spool from commands.lock import Lock -from commands.crashDrive import crashDrive + +# auto from autonomous.noAuto import NoAuto from autonomous.grabNote import GrabNote +from autonomous.shoot import Shoot # NetworkTables from ntcore import NetworkTableInstance @@ -100,25 +103,22 @@ def __init__(self) -> None: # Turning is controlled by the X axis of the right stick. commands2.cmd.run( lambda: self.robotDrive.drive( - 2 - ** -wpimath.applyDeadband( + -wpimath.applyDeadband( self.driverController.getRawAxis(1), OIConstants.kDriveDeadband, # TODO: Use constants to set these controls ) - - 1, - 2 - ** -wpimath.applyDeadband( + * 0.5, + wpimath.applyDeadband( self.driverController.getRawAxis(0), OIConstants.kDriveDeadband, # TODO: Use constants to set these controls ) - - 1, - 2 - ** -wpimath.applyDeadband( + * 0.5, + -wpimath.applyDeadband( self.driverController.getRawAxis(4), OIConstants.kDriveDeadband, # TODO: Use constants to set these controls ) - - 1, - True, + * 0.5, + lambda: self.fieldCentricChooser.getSelected() == "Field Centric", True, ), self.robotDrive, @@ -177,8 +177,8 @@ def configureButtonBindings(self) -> None: # Deliver to amp (button a), part a self.driverController.a().onTrue( commands2.SequentialCommandGroup( - RotateIntake( - IntakeConstants.BlowPos, self.intake + RotateIntake(IntakeConstants.BlowPos, self.intake).withTimeout( + 1 ), # Rotate to fully closed # SetIntakeSpeed(-0.6, self.intake), # Eject slowly LoadMagazine(self.shooter, self.intake), # Load the magazine @@ -193,7 +193,7 @@ def configureButtonBindings(self) -> None: # Deliver to amp (button b), part b self.driverController.b().onTrue( commands2.SequentialCommandGroup( - FlyWheelSpeed(0.25, self.shooter, False), # rotates the Flywheel + FlyWheelSpeed(0.3, self.shooter, False), # rotates the Flywheel commands2.WaitCommand(2), ShooterROT( SuperStrucConstants.LoadPos, self.shooter @@ -214,11 +214,16 @@ def configureButtonBindings(self) -> None: ) ) - self.driverController.leftBumper().whileTrue(crashDrive(self.robotDrive)) + self.driverController.leftBumper().whileTrue( + sendToObject(self.robotDrive, self.limelight) + ) + self.driverController.rightBumper().whileTrue( + commands2.InstantCommand(self.intake.zeroIntake()) + ) # ============================== # Operator Commands - # ============================== + # ===============180=============== # intake keybinds # intake movement self.opController.button(2).whileTrue(IntakeRotationMAN(1, self.intake)) # out @@ -228,7 +233,7 @@ 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)) + self.opController.button(9).whileFalse(SetIntakeSpeed(0, self.intake)) # shooter keybinds # shooter movement @@ -274,8 +279,10 @@ def configureNetworktables(self): # Use sendable choosers for some settings self.fieldCentricChooser = wpilib.SendableChooser() + # TODO Make this a boolean self.fieldCentricChooser.setDefaultOption("Field Centric", True) self.fieldCentricChooser.addOption("Robot Centric", False) + self.sd.putNumber("Auto/Angle", 0) wpilib.SmartDashboard.putData("FieldCentric", self.fieldCentricChooser) def configureAutonomous(self) -> None: @@ -292,7 +299,12 @@ def configureAutonomous(self) -> None: GrabNote(self.limelight, self.shooter, self.intake, self.robotDrive), ) - wpilib.SmartDashboard.putData("Autonomous", self.autoChooser) + self.autoChooser.addOption( + "shoot Auto", + Shoot(self.robotDrive, self.intake, self.shooter), + ) + + wpilib.SmartDashboard.putData("Auto/Mode", self.autoChooser) def getAutonomousCommand(self) -> commands2.Command: """Use this to pass the autonomous command to the main {@link Robot} class. diff --git a/rio/simgui-window.json b/rio/simgui-window.json index c93cc85e..29e2c346 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", diff --git a/rio/subsystems/drivesubsystem.py b/rio/subsystems/drivesubsystem.py index cd8f1656..013f4b02 100644 --- a/rio/subsystems/drivesubsystem.py +++ b/rio/subsystems/drivesubsystem.py @@ -210,7 +210,7 @@ def drive( xSpeedCommanded = xSpeed ySpeedCommanded = ySpeed self.sd.putNumber("pos/rot", rot) - if rateLimit: + if False: # if rateLimit # Convert XY to polar for rate limiting inputTranslationDir = math.atan2(ySpeed, xSpeed) inputTranslationMag = math.hypot(xSpeed, ySpeed) diff --git a/rio/subsystems/intake.py b/rio/subsystems/intake.py index 63090385..78cab928 100644 --- a/rio/subsystems/intake.py +++ b/rio/subsystems/intake.py @@ -40,22 +40,18 @@ def __init__(self) -> None: self.intakeMotor.setInverted(True) # encoders - self.liftEncoder = self.liftMotor.getAbsoluteEncoder( - SparkAbsoluteEncoder.Type.kDutyCycle - ) + self.liftEncoder = self.liftMotor.getEncoder() - self.liftEncoder.setPositionConversionFactor(IntakeConstants.kLiftConversion) + self.liftEncoder.setPositionConversionFactor(1.0) self.intakeEncoder = self.intakeMotor.getEncoder() # setting inverted self.liftMotor.setInverted(IntakeConstants.kLiftInversion) - # self.intakeMotor.setInverted(IntakeConstants.kIntakeInversion) + self.intakeMotor.setInverted(IntakeConstants.kIntakeInversion) # 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) @@ -75,12 +71,15 @@ def __init__(self) -> None: CANSparkBase.SoftLimitDirection.kReverse, False ) + self.liftMotor.setSmartCurrentLimit(4) + self.intakeMotor.burnFlash() 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) @@ -97,3 +96,6 @@ def manualLift(self, speed): def lift(self, angle: float): self.liftPID.setReference(angle, CANSparkMax.ControlType.kPosition) + + def zeroIntake(self): + self.liftEncoder.setPosition(0) diff --git a/rio/subsystems/mikeswervemodule.py b/rio/subsystems/mikeswervemodule.py index bb06b42b..f870d03b 100644 --- a/rio/subsystems/mikeswervemodule.py +++ b/rio/subsystems/mikeswervemodule.py @@ -148,7 +148,7 @@ def setDesiredState(self, desiredState: SwerveModuleState) -> None: # Apply chassis angular offset to the desired state. correctedDesiredState = SwerveModuleState() correctedDesiredState.speed = desiredState.speed - correctedDesiredState.angle = -desiredState.angle + Rotation2d( + correctedDesiredState.angle = desiredState.angle + Rotation2d( self.chassisAngularOffset ) @@ -165,7 +165,7 @@ def setDesiredState(self, desiredState: SwerveModuleState) -> None: optimizedDesiredState.angle.radians(), CANSparkMax.ControlType.kPosition ) - self.desiredState = desiredState + self.desiredState = optimizedDesiredState def resetEncoders(self) -> None: """ diff --git a/rio/subsystems/shooter.py b/rio/subsystems/shooter.py index 3ea82743..9d60feea 100644 --- a/rio/subsystems/shooter.py +++ b/rio/subsystems/shooter.py @@ -90,6 +90,7 @@ def __init__(self) -> None: def periodic(self) -> None: self.sd.putNumber("Thermals/rotate", self.rotateMotor.getMotorTemperature()) self.sd.putNumber("Thermals/fly", self.flyMotor.getMotorTemperature()) + # print(self.rotateEncoder.getPosition()) def setFlyWheelSpeed(self, speed): self.flyMotor.set(speed)