diff --git a/rio/commands/climb.py b/rio/commands/climb.py index 1b9f7b4..3a26855 100644 --- a/rio/commands/climb.py +++ b/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): diff --git a/rio/commands/defaultFlywheel.py b/rio/commands/defaultFlywheel.py new file mode 100644 index 0000000..6f9cfa4 --- /dev/null +++ b/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 diff --git a/rio/commands/spool.py b/rio/commands/spool.py index 0c414c6..1dcd7ae 100644 --- a/rio/commands/spool.py +++ b/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) diff --git a/rio/constants.py b/rio/constants.py index 24fb59d..51d1c96 100644 --- a/rio/constants.py +++ b/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 diff --git a/rio/robotcontainer.py b/rio/robotcontainer.py index b288cd0..b5598d0 100644 --- a/rio/robotcontainer.py +++ b/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)) diff --git a/rio/simgui-window.json b/rio/simgui-window.json index 29e2c34..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", diff --git a/rio/simgui.json b/rio/simgui.json index babcdb1..ccf90fe 100644 --- a/rio/simgui.json +++ b/rio/simgui.json @@ -16,6 +16,7 @@ "NTProvider": { "types": { "/FMSInfo": "FMSInfo", + "/SmartDashboard/Auto/Mode": "String Chooser", "/SmartDashboard/Autonomous": "String Chooser", "/SmartDashboard/FieldCentric": "String Chooser" } diff --git a/rio/subsystems/drivesubsystem.py b/rio/subsystems/drivesubsystem.py index 013f4b0..755898d 100644 --- a/rio/subsystems/drivesubsystem.py +++ b/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) diff --git a/rio/subsystems/intake.py b/rio/subsystems/intake.py index 78cab92..ebe9b19 100644 --- a/rio/subsystems/intake.py +++ b/rio/subsystems/intake.py @@ -59,7 +59,7 @@ def __init__(self) -> None: # limit switches self.limtSwitch = self.intakeMotor.getForwardLimitSwitch( - SparkMaxLimitSwitch.Type.kNormallyOpen + SparkMaxLimitSwitch.Type.kNormallyClosed ) self.limtSwitch.enableLimitSwitch(True) @@ -71,7 +71,7 @@ def __init__(self) -> None: CANSparkBase.SoftLimitDirection.kReverse, False ) - self.liftMotor.setSmartCurrentLimit(4) + self.liftMotor.setSmartCurrentLimit(12) self.intakeMotor.burnFlash() @@ -79,7 +79,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) diff --git a/rio/subsystems/shooter.py b/rio/subsystems/shooter.py index 9d60fee..14263b7 100644 --- a/rio/subsystems/shooter.py +++ b/rio/subsystems/shooter.py @@ -67,7 +67,7 @@ def __init__(self) -> None: # limit switches self.magazineSwitch = self.flyMotor.getForwardLimitSwitch( - SparkMaxLimitSwitch.Type.kNormallyOpen + SparkMaxLimitSwitch.Type.kNormallyClosed ) self.magazineSwitch.enableLimitSwitch(False) @@ -90,9 +90,9 @@ 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): + print(speed) self.flyMotor.set(speed) def getAngle(self) -> float: