diff --git a/robot.py b/robot.py index 9239568..a808fbb 100644 --- a/robot.py +++ b/robot.py @@ -1,21 +1,13 @@ from commands2 import CommandScheduler -from wpilib import DriverStation, Preferences -from robotcontainer import RobotContainer -from logging import basicConfig, DEBUG, info +from wpilib import Preferences +from wpilib.shuffleboard import Shuffleboard +from robotcontainer import RobotContainer, DummyGyro from commands2.timedcommandrobot import TimedCommandRobot -basicConfig(level=DEBUG) - class Ghost(TimedCommandRobot): __slots__ = ("auto_active", "subsystems") - def info(self, message: str): - if (not DriverStation.isDSAttached()) or DriverStation.isFMSAttached(): - # * don't run if there's no driver station/we're in competition - return - info(message) - # dio- port 0 green, port 1 yellow, port 2 blue, and port 3 white def robotInit(self): @@ -23,7 +15,8 @@ def robotInit(self): # * allows the robot to remember + the dashboard to configure whether or not autonomous should run Preferences.initBoolean("auto_active", False) - self.auto_active = Preferences.getBoolean("auto_active", False) + if type(self.subsystems.gyro) != DummyGyro: + Shuffleboard.getTab("LiveWindow").add("gyro", self.subsystems.gyro) def teleopInit(self): """executed when teleoperated mode is enabled. this function turns on the controller's safety features to stop us from breaking things.""" @@ -34,12 +27,13 @@ def teleopPeriodic(self): ... def teleopExit(self) -> None: self.subsystems.arm.mode = 0 - def autonomousInit(self) -> None: ... + def autonomousInit(self) -> None: + self.auto_active = Preferences.getBoolean("auto_active", False) def autonomousPeriodic(self) -> None: if self.auto_active: self.subsystems.shooter.shoot() - self.subsystems.drivetrain.stop() + # self.subsystems.drivetrain.stop() def autonomousExit(self) -> None: self.subsystems.drivetrain.stop() diff --git a/robotcontainer.py b/robotcontainer.py index 475eae5..d679ac8 100644 --- a/robotcontainer.py +++ b/robotcontainer.py @@ -156,7 +156,17 @@ def configureButtonBindings(self): ) # * toggle arm up and down with x - self.controller.x().onTrue(InstantCommand(lambda: self.arm.toggle(), self.arm)) + self.controller.x().onTrue( + InstantCommand( + self.drivetrain.invert, self.drivetrain + ).andThen(InstantCommand(self.arm.toggle, self.arm)) + ) + + self.controller.y().onTrue( + InstantCommand(lambda: self.arm.motor.set(0.3)) + ).onFalse( + InstantCommand(lambda: self.arm.motor.set(0)) + ) # self.controller.povUp().or_(self.controller.povUpLeft()).or_( # self.controller.povUpLeft() @@ -184,7 +194,15 @@ def reset_arm(): self.arm.motor.stopMotor() self.arm.encoder.reset() - self.controller.back().onTrue(FunctionalCommand(onInit=self.arm.motor.stopMotor, onExecute=self.arm.motor.set(-0.5), onEnd=reset_arm, isFinished=self.arm.encoder.getRate(0))) + self.controller.back().onTrue( + FunctionalCommand( + onInit=self.arm.motor.stopMotor, + onExecute=lambda: self.arm.motor.set(-0.75), + onEnd=reset_arm, + isFinished=lambda: self.arm.encoder.getRate() == 0, + ) + ) + ... def getAutonomousCommand(self): diff --git a/subsystems/arm.py b/subsystems/arm.py index b692494..966fe91 100644 --- a/subsystems/arm.py +++ b/subsystems/arm.py @@ -1,12 +1,10 @@ from commands2.subsystem import Subsystem from rev import CANSparkMax -from wpilib import Encoder, Preferences -from wpilib.shuffleboard import Shuffleboard +from wpilib import Encoder from constants import ArmPID from wpimath.controller import PIDController - class Arm(Subsystem): __slots__ = ("motor", "encoder", "setpoint", "pid") @@ -19,14 +17,19 @@ def __init__(self): self.encoder = Encoder(1, 2, False, Encoder.EncodingType.k4X) self.mode = 0 self.encoder.setDistancePerPulse(1 / 7) - Shuffleboard.getTab("LiveWindow").add(self.encoder) - Shuffleboard.getTab("LiveWindow").add(self.pid) + # Shuffleboard.getTab("LiveWindow").add(self.encoder) + # Shuffleboard.getTab("LiveWindow").add(self.pid) def get_setpoint(self) -> int: - return self.mode * 88 + if self.mode == 2: + return 88 + return self.mode * 176 def toggle(self): - self.mode = (self.mode + 1) % 3 + self.mode = (self.mode + 1) % 2 + + def aim_amp(self): + self.mode = 2 def periodic(self): # return # skip all the other stuff @@ -35,8 +38,13 @@ def periodic(self): # self.motor.set(self.pid.calculate(error, self.get_setpoint())) if abs(error) < 1: self.motor.set(0) + if self.get_setpoint() == 0: + self.encoder.reset() else: - self.motor.set((error * ArmPID.K_P) + (0.3 * error / abs(error))) + if self.mode == 2: # if it's aiming for the amp + self.motor.set(0.75) # lower power + else: + self.motor.set((error * ArmPID.K_P) + (0.3 * error / abs(error))) # self.pid.setReference(self.get_setpoint(), CANSparkMax.ControlType.kPosition) # print(error) # if abs(error) < 1: # * if it's less than 0.1 degree off... diff --git a/subsystems/mecanum.py b/subsystems/mecanum.py index a2e5d0e..44cb63b 100644 --- a/subsystems/mecanum.py +++ b/subsystems/mecanum.py @@ -12,6 +12,9 @@ def __init__(self, front: CANSparkMax, rear: CANSparkMax): self.front: SparkRelativeEncoder = front.getEncoder() self.rear: SparkRelativeEncoder = rear.getEncoder() + self.front.setPosition(0) + self.rear.setPosition(0) + def set_conversion_factor(self, factor: float): self.front.setPositionConversionFactor(factor) self.rear.setPositionConversionFactor(factor) @@ -43,7 +46,7 @@ def __init__(self, max_output: float = 1.0): # self.arm.setIdleMode(CANSparkMax.IdleMode.kCoast) # * start off not inveretd - self.inversion_factor = 1 + self.inversion_factor = -1 # * initialize the motors left_rear = CANSparkMax(1, CANSparkMax.MotorType.kBrushless) @@ -54,11 +57,11 @@ def __init__(self, max_output: float = 1.0): # * when we're not driving, brake # ? do we want this? driving seems much smoother when coasting - left_front.setIdleMode(CANSparkMax.IdleMode.kBrake) - left_rear.setIdleMode(CANSparkMax.IdleMode.kBrake) + left_front.setIdleMode(CANSparkMax.IdleMode.kCoast) + left_rear.setIdleMode(CANSparkMax.IdleMode.kCoast) - right_front.setIdleMode(CANSparkMax.IdleMode.kBrake) - right_rear.setIdleMode(CANSparkMax.IdleMode.kBrake) + right_front.setIdleMode(CANSparkMax.IdleMode.kCoast) + right_rear.setIdleMode(CANSparkMax.IdleMode.kCoast) # * invert the right side (they just are like this) right_front.setInverted(True) @@ -73,6 +76,9 @@ def __init__(self, max_output: float = 1.0): self.left_encoders = EncoderGroup(left_front, left_rear) self.right_encoders = EncoderGroup(right_front, right_rear) + dash = Shuffleboard.getTab("LiveWindow") + dash.add("left front", self.left_encoders.front.getPosition()) + dash.add("right front", self.right_encoders.front.getPosition()) # * make the encoders return meters self.left_encoders.set_conversion_factor(NEO_CPR / WHEEL_CIRCUMFERENCE) self.right_encoders.set_conversion_factor(NEO_CPR / WHEEL_CIRCUMFERENCE) @@ -83,7 +89,6 @@ def __init__(self, max_output: float = 1.0): self.drivetrain.setMaxOutput(max_output) - dash = Shuffleboard.getTab("LiveWindow") dash.add("drivetrain", self.drivetrain) def invert(self): @@ -101,7 +106,7 @@ def drive(self, forward_motion: float, side_motion: float, rotation: float): self.drivetrain.driveCartesian( self.deadzone(forward_motion) * self.inversion_factor, self.deadzone(side_motion) * self.inversion_factor, - self.deadzone(rotation) * self.inversion_factor, + self.deadzone(rotation), ) def stop(self): diff --git a/subsystems/odometry.py b/subsystems/odometry.py index bc80314..ffc072b 100644 --- a/subsystems/odometry.py +++ b/subsystems/odometry.py @@ -6,7 +6,7 @@ MecanumDriveWheelPositions, ) from wpimath.geometry import Translation2d, Rotation2d, Pose2d -from wpilib import Field2d, Timer +from wpilib import Field2d, Timer, DriverStation from wpilib.shuffleboard import Shuffleboard from navx import AHRS @@ -14,6 +14,15 @@ from subsystems.mecanum import Mecanum from constants import LINEAR_SPEED +from logging import basicConfig, DEBUG, info as linfo + +basicConfig(level=DEBUG) + +def info(message: str): + if (not DriverStation.isDSAttached()) or DriverStation.isFMSAttached(): + # * don't run if there's no driver station/we're in competition + return + linfo(message) class Odometry(Subsystem): @@ -46,12 +55,13 @@ def __init__(self, gyro: AHRS, drivetrain: Mecanum, vision: Vision): self.kinematics, self.gyro.getRotation2d(), self.wheel_positions, - Pose2d(5.0, 13.5, Rotation2d()), + robot := Pose2d(5.0, 13.5, Rotation2d()), ) self.field = Field2d() + self.field.setRobotPose(robot) dash = Shuffleboard.getTab("LiveWindow") dash.add("field", self.field) - dash.add("gyro", gyro.getAngle()) + self.update() self.vision_timer.start() @@ -68,6 +78,7 @@ def periodic(self): self.update() return if self.vision_timer.advanceIfElapsed(1): + info(self.gyro.getRotation2d().degrees()) estimated_pose = self.vision.estimate_pose() if estimated_pose: # * if we have a pose estimation from photonvision... pose = estimated_pose.toPose2d() diff --git a/subsystems/shooter.py b/subsystems/shooter.py index df0f560..f480330 100644 --- a/subsystems/shooter.py +++ b/subsystems/shooter.py @@ -15,16 +15,17 @@ def __init__(self): intake_top_left.setInverted(True) # intake_bot_right.setInverted(True) - shooter_left = CANSparkMax(10, CANSparkMax.MotorType.kBrushed) + shooter_left = CANSparkMax(10, CANSparkMax.MotorType.kBrushless) shooter_right = CANSparkMax(11, CANSparkMax.MotorType.kBrushless) shooter_right.setInverted(False) + shooter_left.setInverted(True) shooter_left.setIdleMode(CANSparkMax.IdleMode.kBrake) shooter_right.setIdleMode(CANSparkMax.IdleMode.kBrake) top_group = MotorControllerGroup(intake_top_left, intake_top_right) bottom_group = MotorControllerGroup(intake_bot_left, intake_bot_right) - top_group.setInverted(True) + intake_top_right.setInverted(True) shooter_group = MotorControllerGroup(shooter_left, shooter_right) @@ -40,17 +41,19 @@ def arm_intake(self): def arm_spit(self): # print("Regurgitating the whole note.") - self.bottom_group.set(-0.5) - self.top_group.set(-0.5) + self.bottom_group.set(-0.25) + self.top_group.set(-0.25) def intake(self): self.shooter_group.set(-0.5) def shoot(self): - self.shooter_group.set(1) + self.shooter_group.set(0.75) + self.arm_spit() def stop(self): self.shooter_group.set(0) + self.arm_stop() def set(self, speed: float): self.shooter_group.set(speed) diff --git a/subsystems/vision.py b/subsystems/vision.py index 218d0dd..a54457f 100644 --- a/subsystems/vision.py +++ b/subsystems/vision.py @@ -85,7 +85,10 @@ def estimate_pose(self) -> Optional[Pose3d]: self.driver_mode = False # gets the current best target - pose_result = self.pose_estimator.update(self.latest_result()) + latest_result = self.latest_result() + if not latest_result: + return None + pose_result = self.pose_estimator.update() return pose_result.estimatedPose if pose_result else None def calculate_distance(