From 81e54eb3175d14c825fb95aa6b2983116edb3181 Mon Sep 17 00:00:00 2001 From: Guilherme Samuel Date: Sat, 28 Feb 2026 11:34:45 -0300 Subject: [PATCH 01/14] refactor: remove legacy SPI class methods --- camera.py | 10 +++++++--- crest.py | 4 ++-- physics.py | 0 pixy2py/links/spilink.py | 7 ++++--- robot.py | 9 +++------ 5 files changed, 16 insertions(+), 14 deletions(-) delete mode 100644 physics.py diff --git a/camera.py b/camera.py index b5afcbf..3e14d9d 100644 --- a/camera.py +++ b/camera.py @@ -11,6 +11,7 @@ from pixy2py.pixy2 import Pixy2 from pixy2py.pixy2ccc import Pixy2CCC from wpilib import RobotBase, SerialPort, CameraServer +from commands2 import Subsystem class AprilTagCamera(ABC): @@ -88,15 +89,15 @@ def getYawFromTag(self, tag: int) -> float: def getYawAndRangeFromTag(self, tag: int) -> Tuple[float, float]: return 0, 0 -class PixyFuelDetector: +class PixyFuelDetector(Subsystem): def __init__(self) -> None: self.pixy = Pixy2(Pixy2.LinkType.SPI) self.pixy.init() self.pixy.setLamp(1, 1) - self.pixy.setLED(255, 255, 255) + self.pixy.setLED(red=255, green=255, blue=255) def getBiggestBlock(self) -> Optional[Pixy2CCC.Block]: - blockCount: int = pixy.getCCC().getBlocks(False, Pixy2CCC.CCC_SIG1, 25) + blockCount: int = self.pixy.getCCC().getBlocks(False, Pixy2CCC.CCC_SIG1, 25) print("Found " + str(blockCount) + " blocks!") if blockCount <= 0: return None @@ -111,3 +112,6 @@ def getBiggestBlock(self) -> Optional[Pixy2CCC.Block]: largestBlock = block return largestBlock + + def periodic(self) -> None: + self.getBiggestBlock() diff --git a/crest.py b/crest.py index ab75ada..2aac4d3 100644 --- a/crest.py +++ b/crest.py @@ -16,7 +16,7 @@ def __init__(self): self.pid.setTolerance(0.1) self.encoder.setPosition(0) - + def getPosition(self) -> float: return self.encoder.getPosition() * gear_ratio * 360 @@ -52,4 +52,4 @@ def descer(self): self.motor.set(-0.05) def stop(self): - self.motor.stopMotor() \ No newline at end of file + self.motor.stopMotor() diff --git a/physics.py b/physics.py deleted file mode 100644 index e69de29..0000000 diff --git a/pixy2py/links/spilink.py b/pixy2py/links/spilink.py index 5bde1cd..c687cec 100644 --- a/pixy2py/links/spilink.py +++ b/pixy2py/links/spilink.py @@ -6,6 +6,7 @@ """ import wpilib import pixy2py.links.link +import hal class SPILink(pixy2py.links.link.Link): """Link for communicating over Serial Peripheral Interface (SPI).""" @@ -29,9 +30,9 @@ def __init__(self, link_arg): # Use the value to open the port and configure it. self.spi = wpilib.SPI(spi_port) self.spi.setClockRate(SPILink.PIXY_SPI_CLOCKRATE) - self.spi.setMSBFirst() - self.spi.setSampleDataOnTrailingEdge() - self.spi.setClockActiveLow() + #self.spi.setMSBFirst() + #self.spi.setSampleDataOnTrailingEdge() + #self.spi.setClockActiveLow() self.spi.setChipSelectActiveLow() # def open(self, link_arg): diff --git a/robot.py b/robot.py index b98ed23..1731638 100644 --- a/robot.py +++ b/robot.py @@ -11,7 +11,7 @@ from wpilib import SmartDashboard, SendableChooser, DriverStation from commands2.sysid import SysIdRoutine from shooter import Shooter -from camera import Pixy2, LimelightCamera, PhotonVisionCamera +from camera import PixyFuelDetector, LimelightCamera, PhotonVisionCamera class Robot(TimedCommandRobot): autonomous: Optional[Command] = None @@ -22,6 +22,8 @@ class Robot(TimedCommandRobot): shooter: Shooter = Shooter() def robotInit(self) -> None: + self.pixy: PixyFuelDetector = PixyFuelDetector() + self.autoChooser.addOption( "LTV Controller Test Auto", AutoLTVController(self.drivetrain) ) @@ -70,8 +72,3 @@ def autonomousPeriodic(self) -> None: def autonomousExit(self) -> None: CommandScheduler.getInstance().cancelAll() - - def testInit(self) -> None: - JoystickButton(self.driverJoystick, 1).onTrue( - self.shooter.setFlywheelBySetpointCommand() - ) From b338eb87e2f0ace5617a1246dd5bae761a6ac9f1 Mon Sep 17 00:00:00 2001 From: Guilherme Samuel Date: Sat, 28 Feb 2026 22:37:29 -0300 Subject: [PATCH 02/14] refactor: remove class-only attributes --- climber.py | 3 +-- crest.py | 55 --------------------------------------------- intake.py | 10 ++++----- led.py | 13 ++++++----- robot.py | 11 ++++----- shooter.py | 66 +++++++++++++++++++++++++++++++++++++++++++++--------- utils.py | 5 +++++ 7 files changed, 80 insertions(+), 83 deletions(-) delete mode 100644 crest.py diff --git a/climber.py b/climber.py index 4f000a7..34fef8b 100644 --- a/climber.py +++ b/climber.py @@ -4,9 +4,8 @@ class Climber(Subsystem): - climber: WPI_VictorSPX = WPI_VictorSPX(1) - def __init__(self) -> None: + self.climber = WPI_VictorSPX(1) self.setDefaultCommand(run(lambda: self.stop())) def clockwise(self) -> None: diff --git a/crest.py b/crest.py deleted file mode 100644 index 2aac4d3..0000000 --- a/crest.py +++ /dev/null @@ -1,55 +0,0 @@ -import rev -import wpimath.controller -from wpilib import SmartDashboard - -gear_ratio = 11.52 - -def clamp(value, min_value, max_value): - return max(min(value, max_value), min_value) - -class Crest: - def __init__(self): - self.motor = rev.SparkMax(53, rev.SparkMax.MotorType.kBrushless) - self.encoder = self.motor.getEncoder() - - self.pid = wpimath.controller.PIDController(0.006, 0.0, 0.0) - self.pid.setTolerance(0.1) - self.encoder.setPosition(0) - - - def getPosition(self) -> float: - return self.encoder.getPosition() * gear_ratio * 360 - - def update_dashboard(self): - SmartDashboard.putData("PID", self.pid) - SmartDashboard.putNumber("crest encoder", self.encoder.getPosition()) - SmartDashboard.putNumber("crest position", float(self.getPosition())) - # self.pid.setSetpoint(self.setpoint) - print(self.pid.getSetpoint()) - - def move_to_setpoint(self): - current_position = self.getPosition() - motor_value = self.pid.calculate(current_position) - motor_value = clamp(motor_value, -0.4, 0.4) - self.motor.set(motor_value) - - def move_to(self, setpoint): - current_position = self.getPosition() - motor_value = self.pid.calculate(current_position, setpoint) - motor_value = clamp(motor_value, -0.4, 0.4) - self.motor.set(motor_value) - - def up(self): - self.move_to(20) - - def down(self): - self.move_to(0) - - def subir(self): - self.motor.set(0.2) - - def descer(self): - self.motor.set(-0.05) - - def stop(self): - self.motor.stopMotor() diff --git a/intake.py b/intake.py index f7c93c8..7773f38 100644 --- a/intake.py +++ b/intake.py @@ -5,12 +5,12 @@ class Intake(Subsystem): - pivot: WPI_VictorSPX = WPI_VictorSPX(constants.kIntakeAngleId) - roller: WPI_VictorSPX = WPI_VictorSPX(constants.kIntakeTrackId) - pivotUp: bool = False - lastBurstTime: float = 0.0 - def __init__(self) -> None: + self.pivot = WPI_VictorSPX(constants.kIntakeAngleId) + self.roller = WPI_VictorSPX(constants.kIntakeTrackId) + self.pivotUp = False + self.lastBurstTime = 0.0 + SmartDashboard.putNumber("up", 0.5) SmartDashboard.putNumber("down", 0.5) self.pivot.setInverted(True) diff --git a/led.py b/led.py index 897be66..8021ddf 100644 --- a/led.py +++ b/led.py @@ -1,10 +1,10 @@ -from wpilib import SerialPort -from dataclasses import dataclass +from wpilib import SerialPort, Solenoid, PneumaticsModuleType import constants -@dataclass class LEDController: - arduino: SerialPort = SerialPort(constants.kBaudRate, constants.kLEDUSBPort) + def __init__(self) -> None: + self.arduino = SerialPort(constants.kBaudRate, constants.kLEDUSBPort) + self.extraLED = Solenoid(PneumaticsModuleType.CTREPCM, 4) def red(self) -> None: self.changeColor("r") @@ -15,6 +15,9 @@ def green(self) -> None: def blue(self) -> None: self.changeColor("b") - def changeColor(char: str) -> None: + def setExtraLED(self, status: bool) -> None: + self.extraLED.set(status) + + def changeColor(self, char: str) -> None: byte_obj = char.encode("ascii") self.arduino.write(byte_obj) diff --git a/robot.py b/robot.py index 1731638..9c1bbd1 100644 --- a/robot.py +++ b/robot.py @@ -15,13 +15,14 @@ class Robot(TimedCommandRobot): autonomous: Optional[Command] = None - drivetrain: Drivetrain = Drivetrain() - intake: Intake = Intake() - autoChooser: SendableChooser = AutoBuilder.buildAutoChooser() - driverJoystick: GenericJoystick = GenericJoystick(constants.kJoystickDriverPort) - shooter: Shooter = Shooter() def robotInit(self) -> None: + self.drivetrain = Drivetrain() + self.intake = Intake() + self.autoChooser = AutoBuilder.buildAutoChooser() + self.driverJoystick = GenericJoystick(constants.kJoystickDriverPort) + self.shooter = Shooter() + self.pixy: PixyFuelDetector = PixyFuelDetector() self.autoChooser.addOption( diff --git a/shooter.py b/shooter.py index 2e80ce7..200a0cb 100644 --- a/shooter.py +++ b/shooter.py @@ -16,15 +16,24 @@ ) from wpimath.units import rotationsPerMinuteToRadiansPerSecond import constants +from utils import Utils +gear_ratio = 11.52 class Shooter(Subsystem): - flywheel: SparkMax = SparkMax( - constants.kFlywheelId, SparkLowLevel.MotorType.kBrushless - ) - bangBangController: BangBangController = BangBangController() - def __init__(self) -> None: + self.flywheel = SparkMax( + constants.kFlywheelId, SparkLowLevel.MotorType.kBrushless + ) + self.flywheel_encoder = self.flywheel.getEncoder() + self.bangBangController = BangBangController() + self.hood = rev.SparkMax(53, rev.SparkMax.MotorType.kBrushless) + self.hood_encoder = self.hood.getEncoder() + + self.hood_pid = wpimath.controller.PIDController(0.006, 0.0, 0.0) + self.hood_pid.setTolerance(0.1) + self.hood_encoder.setPosition(0) + config = SparkMaxConfig() config.smartCurrentLimit(40) @@ -32,10 +41,10 @@ def __init__(self) -> None: config.closedLoop.feedForward.sva(*constants.kFlywheelFeedForward) config.closedLoop.pid(*constants.kFlywheelPID) - self.feedforward = SimpleMotorFeedforwardMeters( + self.flywheel_feedforward = SimpleMotorFeedforwardMeters( *constants.kFlywheelFeedForward[:3] ) - self.pid = PIDController(*constants.kFlywheelPID[:3]) + self.flywheel_pid = PIDController(*constants.kFlywheelPID[:3]) self.flywheel.configure( config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters @@ -43,7 +52,6 @@ def __init__(self) -> None: self.closedLoopController = self.flywheel.getClosedLoopController() - self.encoder = self.flywheel.getEncoder() SmartDashboard.putData(self.bangBangController) SmartDashboard.putData("Shooter BangBang Controller", self.bangBangController) @@ -67,8 +75,44 @@ def periodic(self) -> None: self.feedforward.setKv(feedforward[1]) self.feedforward.setKa(feedforward[2]) + SmartDashboard.putData("PID", self.hood_pid) + SmartDashboard.putNumber("crest encoder", self.hood_encoder.getPosition()) + SmartDashboard.putNumber("crest position", float(self.getPosition())) + # self.pid.setSetpoint(self.setpoint) + print(self.hood_pid.getSetpoint()) + + def getPosition(self) -> float: + return self.hood_encoder.getPosition() * gear_ratio * 360 + + def move_to_setpoint(self): + current_position = self.getPosition() + hood_value = self.hood_pid.calculate(current_position) + hood_value = Utils.clamp(hood_value, -0.4, 0.4) + self.hood.set(hood_value) + + def move_to(self, setpoint): + current_position = self.getPosition() + hood_value = self.hood_pid.calculate(current_position, setpoint) + hood_value = Utils.clamp(hood_value, -0.4, 0.4) + self.hood.set(hood_value) + + def up(self): + self.move_to(20) + + def down(self): + self.move_to(0) + + def subir(self): + self.hood.set(0.2) + + def descer(self): + self.hood.set(-0.05) + + def stop(self): + self.hood.stopMotor() + def setFlywheelBySetpoint(self, setpoint: float) -> None: - output = self.pid.calculate(self.encoder.getPosition(), setpoint) + output = self.flywheel_pid.calculate(self.flywheel_encoder.getPosition(), setpoint) self.flywheel.set(output) def setFlywheelBySetpointCommand(self) -> Command: @@ -84,10 +128,10 @@ def deactivate(self) -> None: def bangBangActivate(self, maxSetpoint: float) -> None: setpoint = max(0, rotationsPerMinuteToRadiansPerSecond(maxSetpoint)) output = ( - self.bangBangController.calculate(self.encoder.getPosition(), setpoint) + self.bangBangController.calculate(self.flywheel_encoder.getPosition(), setpoint) * 12.0 ) self.closedLoopController.setReference( - output + 0.9 * self.feedforward.calculate(setpoint), + output + 0.9 * self.flywheel_feedforward.calculate(setpoint), SparkMax.ControlType.kVoltage, ) diff --git a/utils.py b/utils.py index 3929442..de7ad62 100644 --- a/utils.py +++ b/utils.py @@ -20,3 +20,8 @@ def readString(port: SerialPort) -> str: buffer = bytearray(port_bytes) converted = port.read() return buffer[:converted].decode("ascii") + + @staticmethod + def clamp(value, min_value, max_value): + return max(min(value, max_value), min_value) + From a70a260fbb3695a3293fef6ae18e19aaf91d2d67 Mon Sep 17 00:00:00 2001 From: Guilherme Samuel dos Santos Date: Sat, 28 Feb 2026 22:50:15 -0300 Subject: [PATCH 03/14] Update .gitignore to include RobotPy configurations --- .gitignore | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/.gitignore b/.gitignore index b6423d0..fabbf78 100644 --- a/.gitignore +++ b/.gitignore @@ -1,17 +1,14 @@ # Vim temporarily files *.swp -# WPILib default config +# RobotPy default config tests/ .wpilib/ -# WPILib default simulator -simgui-ds.json - # CTRE Simulator ctre_sim -# Those were created when running `python -m robotpy sim` +# WPILib default simulator, those were created when running `python -m robotpy sim` networktables.json simgui-ds.json simgui-window.json @@ -191,5 +188,4 @@ cython_debug/ # PyPI configuration file .pypirc -simgui-window.json -simgui-ds.json + From 2e01e83800677714f5eb93b4c86124dc544e2134 Mon Sep 17 00:00:00 2001 From: Guilherme Samuel Date: Mon, 2 Mar 2026 15:10:59 -0300 Subject: [PATCH 04/14] feat: add indexer subsystem main code --- indexer.py | 14 ++++++++++++++ 1 file changed, 14 insertions(+) create mode 100644 indexer.py diff --git a/indexer.py b/indexer.py new file mode 100644 index 0000000..03993e3 --- /dev/null +++ b/indexer.py @@ -0,0 +1,14 @@ +from commands2 import ParallelCommandGroup +from commands2.cmd import run +from phoenix5 import WPI_VictorSPX + +class Indexer: + def __init__(self) -> None: + self.front_roller = WPI_VictorSPX(7) + self.back_roller = WPI_VictorSPX(8) + + def feed(self) -> Command: + return ParallelCommandGroup( + run(lambda: self.front_roller.set(-1)), + run(lambda: self.back_roller.set(1)) + ) From f8cfd3863756b03df22c9f287399a7b9a6ebb40b Mon Sep 17 00:00:00 2001 From: Guilherme Samuel Date: Mon, 2 Mar 2026 15:11:18 -0300 Subject: [PATCH 05/14] refactor: revert arduino ws2812b led code --- .../ws2812b_pixy2.ino => ws2812b/ws2812b.ino | 24 ++----------------- 1 file changed, 2 insertions(+), 22 deletions(-) rename led-arduino/ws2812b_pixy2.ino => ws2812b/ws2812b.ino (59%) diff --git a/led-arduino/ws2812b_pixy2.ino b/ws2812b/ws2812b.ino similarity index 59% rename from led-arduino/ws2812b_pixy2.ino rename to ws2812b/ws2812b.ino index 666730a..ff80506 100644 --- a/led-arduino/ws2812b_pixy2.ino +++ b/ws2812b/ws2812b.ino @@ -1,11 +1,9 @@ #include -#include -#define LED_PIN 7 +#define LED_PIN 3 #define NUM_LEDS 30 CRGB leds[NUM_LEDS]; -Pixy2 pixy; void changeColor(int red, int green, int blue) { for (int i = 0; i < NUM_LEDS; i++) @@ -13,32 +11,15 @@ void changeColor(int red, int green, int blue) { FastLED.show(); } -void readPixyData() { - int i; - pixy.ccc.getBlocks(); - - if (pixy.ccc.numBlocks) { - Serial.print("Detected "); - Serial.println(pixy.ccc.numBlocks); - for (i = 0; i < pixy.ccc.numBlocks; i++) { - Serial.print(" block "); - Serial.print(i); - Serial.print(": "); - pixy.ccc.blocks[i].print(); - } - } -} - void updateSelectedColor() { if (Serial.available() > 0) { byte received = Serial.read(); - if (received == 'r') { + if (received == 'r') changeColor(255, 0, 0); else if (received == 'g') changeColor(0, 255, 0); else if (received == 'b') changeColor(0, 0, 255); - } } } @@ -46,7 +27,6 @@ void setup() { Serial.begin(9600); FastLED.addLeds(leds, NUM_LEDS); changeColor(255, 0, 0); - pixy.init(); } void loop() { From e78c2c98b463332e3c38a838d727ed9550ba9736 Mon Sep 17 00:00:00 2001 From: Guilherme Samuel Date: Mon, 2 Mar 2026 16:45:59 -0300 Subject: [PATCH 06/14] refactor: add more LEDs to be flashlight by serial port --- ws2812b/ws2812b.ino | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ws2812b/ws2812b.ino b/ws2812b/ws2812b.ino index ff80506..31dfb4f 100644 --- a/ws2812b/ws2812b.ino +++ b/ws2812b/ws2812b.ino @@ -1,7 +1,7 @@ #include -#define LED_PIN 3 -#define NUM_LEDS 30 +#define LED_PIN 7 +#define NUM_LEDS 60 CRGB leds[NUM_LEDS]; From 9ce524a349a8689f34abedd450dc220699264c75 Mon Sep 17 00:00:00 2001 From: Isaac Date: Mon, 2 Mar 2026 20:16:25 -0300 Subject: [PATCH 07/14] testes turreta --- physics.py | 0 robot.py | 90 +++++++++++++++++++++++++++++------------------------- turret.py | 35 +++++++++++---------- 3 files changed, 66 insertions(+), 59 deletions(-) delete mode 100644 physics.py diff --git a/physics.py b/physics.py deleted file mode 100644 index e69de29..0000000 diff --git a/robot.py b/robot.py index b98ed23..b87b045 100644 --- a/robot.py +++ b/robot.py @@ -11,59 +11,67 @@ from wpilib import SmartDashboard, SendableChooser, DriverStation from commands2.sysid import SysIdRoutine from shooter import Shooter +from turret import Turret from camera import Pixy2, LimelightCamera, PhotonVisionCamera class Robot(TimedCommandRobot): - autonomous: Optional[Command] = None - drivetrain: Drivetrain = Drivetrain() - intake: Intake = Intake() - autoChooser: SendableChooser = AutoBuilder.buildAutoChooser() + # autonomous: Optional[Command] = None + # drivetrain: Drivetrain = Drivetrain() + # intake: Intake = Intake() + # # autoChooser: SendableChooser = AutoBuilder.buildAutoChooser() driverJoystick: GenericJoystick = GenericJoystick(constants.kJoystickDriverPort) - shooter: Shooter = Shooter() + # shooter: Shooter = Shooter() + turret: Turret = Turret() def robotInit(self) -> None: - self.autoChooser.addOption( - "LTV Controller Test Auto", AutoLTVController(self.drivetrain) - ) - self.autoChooser.addOption( - "Drive Straight Path", DriveStraightPath(self.drivetrain, 5) - ) - SmartDashboard.putData("Auto Chooser", self.autoChooser) + pass + # self.autoChooser.addOption( + # "LTV Controller Test Auto", AutoLTVController(self.drivetrain) + # ) + # self.autoChooser.addOption( + # "Drive Straight Path", DriveStraightPath(self.drivetrain, 5) + # ) + # SmartDashboard.putData("Auto Chooser", self.autoChooser) def teleopInit(self) -> None: - JoystickButton(self.driverJoystick, 5).onTrue(self.intake.up()) - - JoystickButton(self.driverJoystick, 6).onTrue(self.intake.down()) + # JoystickButton(self.driverJoystick, 5).onTrue(self.intake.up()) + pass + #JoystickButton(self.driverJoystick, 5).onTrue(self.turret.commandCenterTurret(self.driverJoystick.getLeftXAxis())) + + def testPeriodic(self): + JoystickButton(self.driverJoystick, 5).onTrue(self.turret.commandCenterTurret(1)) + # JoystickButton(self.driverJoystick, 6).onTrue(self.intake.down()) - JoystickButton(self.driverJoystick, 1).onTrue( - self.drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kReverse) - ) + # JoystickButton(self.driverJoystick, 1).onTrue( + # self.drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kReverse) + # ) - JoystickButton(self.driverJoystick, 2).onTrue( - self.drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kForward) - ) + # JoystickButton(self.driverJoystick, 2).onTrue( + # self.drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kForward) + # ) - JoystickButton(self.driverJoystick, 3).onTrue( - self.drivetrain.sysIdDynamic(SysIdRoutine.Direction.kForward) - ) + # JoystickButton(self.driverJoystick, 3).onTrue( + # self.drivetrain.sysIdDynamic(SysIdRoutine.Direction.kForward) + # ) - JoystickButton(self.driverJoystick, 4).onTrue( - self.drivetrain.sysIdDynamic(SysIdRoutine.Direction.kReverse) - ) + # JoystickButton(self.driverJoystick, 4).onTrue( + # self.drivetrain.sysIdDynamic(SysIdRoutine.Direction.kReverse) + # ) - self.drivetrain.setDefaultCommand( - self.drivetrain.arcadeDrive( - lambda: self.driverJoystick.getLeftYAxis(), - lambda: self.driverJoystick.getRightXAxis(), - ) - ) + # self.drivetrain.setDefaultCommand( + # self.drivetrain.arcadeDrive( + # lambda: self.driverJoystick.getLeftYAxis(), + # lambda: self.driverJoystick.getRightXAxis(), + # ) + # ) def autonomousInit(self) -> None: - DriverStation.silenceJoystickConnectionWarning(True) - self.autonomous = self.autoChooser.getSelected() + pass + # DriverStation.silenceJoystickConnectionWarning(True) + # self.autonomous = self.autoChooser.getSelected() - if self.autonomous: - self.autonomous.schedule() + # if self.autonomous: + # self.autonomous.schedule() def autonomousPeriodic(self) -> None: pass @@ -71,7 +79,7 @@ def autonomousPeriodic(self) -> None: def autonomousExit(self) -> None: CommandScheduler.getInstance().cancelAll() - def testInit(self) -> None: - JoystickButton(self.driverJoystick, 1).onTrue( - self.shooter.setFlywheelBySetpointCommand() - ) + # def testInit(self) -> None: + # JoystickButton(self.driverJoystick, 1).onTrue( + # self.shooter.setFlywheelBySetpointCommand() + # ) diff --git a/turret.py b/turret.py index 40a9896..fdab7e9 100644 --- a/turret.py +++ b/turret.py @@ -1,6 +1,6 @@ from rev import SparkMax, SparkLowLevel from wpimath.controller import PIDController, SimpleMotorFeedforwardRadians -from commands2 import Subsystem +from commands2 import Subsystem, command class Turret(Subsystem): @@ -8,25 +8,24 @@ def __init__(self): self.turret_motor = SparkMax(51, SparkLowLevel.MotorType.kBrushless) self.encoder = self.turret_motor.getEncoder() self.pid = PIDController(0.00025, 0, 0) - self.pid.setTolerance(100) - self.target_RPM = 4500 + self.pid.setTolerance(0.1) + # self.target_RPM = 4500 self.feedforward = SimpleMotorFeedforwardRadians(0, 0.002) + + def normalize(pixels, max_pixels): + return (pixels * 2 / max_pixels) -1 - def shoot(self): - self.target_RPM = 4500 + def clamp(self,value, min_value, max_value): + return max(min(value, max_value), min_value) - def stop(self): - self.target_RPM = 0 - self.turret_motor.stopMotor() - self.pid.reset() + def centerTurret(self, alignment): + #normalized_alignment = self.normalize(alignment, 1080)#chutando que a camera tem 1080 - def update(self): - if self.target_RPM == 0: - return - output = self.pid.calculate(self.encoder.getVelocity(), self.target_RPM) - output += self.feedforward.calculate(self.target_RPM) - output = max(min(output, 1.0), -1.0) - self.turret_motor.set(output) + output = self.pid.calculate(alignment, 0) - def isReady(self): - return abs(self.encoder.getVelocity() - self.target_RPM) < 100 + output = self.clamp(output, -1,1) + + self.turret_motor.setVoltage(output * 12) + + def commandCenterTurret(self,alignment) -> command: + return self.run(lambda: self.centerTurret(alignment)) \ No newline at end of file From c2888503c209a9d55d85ea1d810870d0b9b3c6d3 Mon Sep 17 00:00:00 2001 From: Batista Date: Mon, 2 Mar 2026 21:54:24 -0300 Subject: [PATCH 08/14] crest and drivetrain buttons --- crest.py | 17 ++++++++---- robot.py | 85 +++++++++++++++++++++++++++++++++----------------------- 2 files changed, 61 insertions(+), 41 deletions(-) diff --git a/crest.py b/crest.py index ab75ada..2ec1889 100644 --- a/crest.py +++ b/crest.py @@ -1,13 +1,15 @@ import rev import wpimath.controller from wpilib import SmartDashboard +from commands2 import Subsystem, command + gear_ratio = 11.52 def clamp(value, min_value, max_value): return max(min(value, max_value), min_value) -class Crest: +class Crest(Subsystem): def __init__(self): self.motor = rev.SparkMax(53, rev.SparkMax.MotorType.kBrushless) self.encoder = self.motor.getEncoder() @@ -20,30 +22,33 @@ def __init__(self): def getPosition(self) -> float: return self.encoder.getPosition() * gear_ratio * 360 - def update_dashboard(self): + def updateDashboard(self): SmartDashboard.putData("PID", self.pid) SmartDashboard.putNumber("crest encoder", self.encoder.getPosition()) SmartDashboard.putNumber("crest position", float(self.getPosition())) # self.pid.setSetpoint(self.setpoint) print(self.pid.getSetpoint()) - def move_to_setpoint(self): + def moveToSetpoint(self): current_position = self.getPosition() motor_value = self.pid.calculate(current_position) motor_value = clamp(motor_value, -0.4, 0.4) self.motor.set(motor_value) - def move_to(self, setpoint): + def commandMoveToSetpoint(self) -> command: + return self.run(lambda: self.moveToSetpoint) + + def moveTo(self, setpoint): current_position = self.getPosition() motor_value = self.pid.calculate(current_position, setpoint) motor_value = clamp(motor_value, -0.4, 0.4) self.motor.set(motor_value) def up(self): - self.move_to(20) + self.moveTo(20) def down(self): - self.move_to(0) + self.moveTo(0) def subir(self): self.motor.set(0.2) diff --git a/robot.py b/robot.py index b87b045..4392ab4 100644 --- a/robot.py +++ b/robot.py @@ -13,65 +13,80 @@ from shooter import Shooter from turret import Turret from camera import Pixy2, LimelightCamera, PhotonVisionCamera +from crest import Crest class Robot(TimedCommandRobot): # autonomous: Optional[Command] = None - # drivetrain: Drivetrain = Drivetrain() + drivetrain: Drivetrain = Drivetrain() # intake: Intake = Intake() # # autoChooser: SendableChooser = AutoBuilder.buildAutoChooser() driverJoystick: GenericJoystick = GenericJoystick(constants.kJoystickDriverPort) # shooter: Shooter = Shooter() turret: Turret = Turret() + crest: Crest = Crest() def robotInit(self) -> None: - pass - # self.autoChooser.addOption( - # "LTV Controller Test Auto", AutoLTVController(self.drivetrain) - # ) - # self.autoChooser.addOption( - # "Drive Straight Path", DriveStraightPath(self.drivetrain, 5) - # ) - # SmartDashboard.putData("Auto Chooser", self.autoChooser) + # definicao dos botoes + JoystickButton(self.driverJoystick, 1).onTrue(self.crest.commandMoveToSetpoint()) + JoystickButton(self.driverJoystick, 2).onTrue(self.drivetrain.forward()) + JoystickButton(self.driverJoystick, 3).onTrue(self.drivetrain.backward()) + JoystickButton(self.driverJoystick, 4).onTrue(self.drivetrain.aim(20))#Tag aleatoria da apriltag + JoystickButton(self.driverJoystick, 5).onTrue(self.turret.commandCenterTurret(self.driverJoystick.getLeftXAxis())) + JoystickButton(self.driverJoystick, 6).onTrue(self.drivetrain.aimAndRange(20))#Tag aleatora + JoystickButton(self.driverJoystick, 7).onTrue() + JoystickButton(self.driverJoystick, 8).onTrue() + JoystickButton(self.driverJoystick, 9).onTrue() + ''' + self.autoChooser.addOption( + "LTV Controller Test Auto", AutoLTVController(self.drivetrain) + ) + self.autoChooser.addOption( + "Drive Straight Path", DriveStraightPath(self.drivetrain, 5) + ) + SmartDashboard.putData("Auto Chooser", self.autoChooser) + ''' def teleopInit(self) -> None: - # JoystickButton(self.driverJoystick, 5).onTrue(self.intake.up()) pass - #JoystickButton(self.driverJoystick, 5).onTrue(self.turret.commandCenterTurret(self.driverJoystick.getLeftXAxis())) def testPeriodic(self): - JoystickButton(self.driverJoystick, 5).onTrue(self.turret.commandCenterTurret(1)) - # JoystickButton(self.driverJoystick, 6).onTrue(self.intake.down()) + pass + ''' + JoystickButton(self.driverJoystick, 6).onTrue(self.intake.down()) - # JoystickButton(self.driverJoystick, 1).onTrue( - # self.drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kReverse) - # ) + JoystickButton(self.driverJoystick, 1).onTrue( + self.drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kReverse) + ) - # JoystickButton(self.driverJoystick, 2).onTrue( - # self.drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kForward) - # ) + JoystickButton(self.driverJoystick, 2).onTrue( + self.drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kForward) + ) - # JoystickButton(self.driverJoystick, 3).onTrue( - # self.drivetrain.sysIdDynamic(SysIdRoutine.Direction.kForward) - # ) + JoystickButton(self.driverJoystick, 3).onTrue( + self.drivetrain.sysIdDynamic(SysIdRoutine.Direction.kForward) + ) - # JoystickButton(self.driverJoystick, 4).onTrue( - # self.drivetrain.sysIdDynamic(SysIdRoutine.Direction.kReverse) - # ) + JoystickButton(self.driverJoystick, 4).onTrue( + self.drivetrain.sysIdDynamic(SysIdRoutine.Direction.kReverse) + ) - # self.drivetrain.setDefaultCommand( - # self.drivetrain.arcadeDrive( - # lambda: self.driverJoystick.getLeftYAxis(), - # lambda: self.driverJoystick.getRightXAxis(), - # ) - # ) + self.drivetrain.setDefaultCommand( + self.drivetrain.arcadeDrive( + lambda: self.driverJoystick.getLeftYAxis(), + lambda: self.driverJoystick.getRightXAxis(), + ) + ) + ''' def autonomousInit(self) -> None: pass - # DriverStation.silenceJoystickConnectionWarning(True) - # self.autonomous = self.autoChooser.getSelected() + ''' + DriverStation.silenceJoystickConnectionWarning(True) + self.autonomous = self.autoChooser.getSelected() - # if self.autonomous: - # self.autonomous.schedule() + if self.autonomous: + self.autonomous.schedule() + ''' def autonomousPeriodic(self) -> None: pass From 96c2d657432640d005f5c2d2da6db01f577f2b9a Mon Sep 17 00:00:00 2001 From: Isaac Date: Tue, 3 Mar 2026 20:53:22 -0300 Subject: [PATCH 09/14] fix: change onTrue commands to whileTrue --- drivetrain.py | 1 + robot.py | 18 +++++++++--------- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/drivetrain.py b/drivetrain.py index f704692..648ca52 100644 --- a/drivetrain.py +++ b/drivetrain.py @@ -306,3 +306,4 @@ def rotate(self, angle: float) -> Command: lambda output: self.drivetrain.arcadeDrive(0, output), self, ) + \ No newline at end of file diff --git a/robot.py b/robot.py index 4392ab4..dfa9499 100644 --- a/robot.py +++ b/robot.py @@ -27,15 +27,15 @@ class Robot(TimedCommandRobot): def robotInit(self) -> None: # definicao dos botoes - JoystickButton(self.driverJoystick, 1).onTrue(self.crest.commandMoveToSetpoint()) - JoystickButton(self.driverJoystick, 2).onTrue(self.drivetrain.forward()) - JoystickButton(self.driverJoystick, 3).onTrue(self.drivetrain.backward()) - JoystickButton(self.driverJoystick, 4).onTrue(self.drivetrain.aim(20))#Tag aleatoria da apriltag - JoystickButton(self.driverJoystick, 5).onTrue(self.turret.commandCenterTurret(self.driverJoystick.getLeftXAxis())) - JoystickButton(self.driverJoystick, 6).onTrue(self.drivetrain.aimAndRange(20))#Tag aleatora - JoystickButton(self.driverJoystick, 7).onTrue() - JoystickButton(self.driverJoystick, 8).onTrue() - JoystickButton(self.driverJoystick, 9).onTrue() + JoystickButton(self.driverJoystick, 1).whileTrue(self.crest.commandMoveToSetpoint()) + JoystickButton(self.driverJoystick, 2).whileTrue(self.drivetrain.forward()) + JoystickButton(self.driverJoystick, 3).whileTrue(self.drivetrain.backward()) + JoystickButton(self.driverJoystick, 4).whileTrue(self.drivetrain.aim(20))#Tag aleatoria da apriltag + JoystickButton(self.driverJoystick, 5).whileTrue(self.turret.commandCenterTurret(self.driverJoystick.getLeftXAxis())) + JoystickButton(self.driverJoystick, 6).whileTrue(self.drivetrain.aimAndRange(20))#Tag aleatora + JoystickButton(self.driverJoystick, 7).whileTrue() + JoystickButton(self.driverJoystick, 8).whileTrue() + JoystickButton(self.driverJoystick, 9).whileTrue() ''' self.autoChooser.addOption( From 1e9fc2045a9b870e02f099422705deddb70d4f73 Mon Sep 17 00:00:00 2001 From: Danone Date: Tue, 3 Mar 2026 21:44:08 -0300 Subject: [PATCH 10/14] all commands have buttons --- robot.py | 19 ++++++++++++------- shooter.py | 2 +- 2 files changed, 13 insertions(+), 8 deletions(-) diff --git a/robot.py b/robot.py index dfa9499..ef9b00a 100644 --- a/robot.py +++ b/robot.py @@ -18,24 +18,29 @@ class Robot(TimedCommandRobot): # autonomous: Optional[Command] = None drivetrain: Drivetrain = Drivetrain() - # intake: Intake = Intake() + intake: Intake = Intake() # # autoChooser: SendableChooser = AutoBuilder.buildAutoChooser() driverJoystick: GenericJoystick = GenericJoystick(constants.kJoystickDriverPort) - # shooter: Shooter = Shooter() + shooter: Shooter = Shooter() turret: Turret = Turret() crest: Crest = Crest() def robotInit(self) -> None: # definicao dos botoes + # comandos crest JoystickButton(self.driverJoystick, 1).whileTrue(self.crest.commandMoveToSetpoint()) + # comandos drivetrain JoystickButton(self.driverJoystick, 2).whileTrue(self.drivetrain.forward()) JoystickButton(self.driverJoystick, 3).whileTrue(self.drivetrain.backward()) JoystickButton(self.driverJoystick, 4).whileTrue(self.drivetrain.aim(20))#Tag aleatoria da apriltag - JoystickButton(self.driverJoystick, 5).whileTrue(self.turret.commandCenterTurret(self.driverJoystick.getLeftXAxis())) - JoystickButton(self.driverJoystick, 6).whileTrue(self.drivetrain.aimAndRange(20))#Tag aleatora - JoystickButton(self.driverJoystick, 7).whileTrue() - JoystickButton(self.driverJoystick, 8).whileTrue() - JoystickButton(self.driverJoystick, 9).whileTrue() + JoystickButton(self.driverJoystick, 5).whileTrue(self.drivetrain.aimAndRange(20))#Tag aleatora + # comandos turret + JoystickButton(self.driverJoystick, 6).whileTrue(self.turret.commandCenterTurret(self.driverJoystick.getLeftXAxis())) + # comandos intake + JoystickButton(self.driverJoystick, 7).whileTrue(self.intake.up()) + JoystickButton(self.driverJoystick, 8).whileTrue(self.intake.down()) + # comandos shooter + JoystickButton(self.driverJoystick, 9).whileTrue(self.shooter.setFlywheelBySetpointCommand()) ''' self.autoChooser.addOption( diff --git a/shooter.py b/shooter.py index 2e80ce7..e17140f 100644 --- a/shooter.py +++ b/shooter.py @@ -72,7 +72,7 @@ def setFlywheelBySetpoint(self, setpoint: float) -> None: self.flywheel.set(output) def setFlywheelBySetpointCommand(self) -> Command: - return run( + return self.run( lambda: self.setFlywheelBySetpoint( SmartDashboard.getNumber("Shooter Setpoint", 0) ) From 09b8c751b135fd09bdbe511dd7a1ee73baae73e0 Mon Sep 17 00:00:00 2001 From: Guilherme Samuel Date: Wed, 4 Mar 2026 20:15:32 -0300 Subject: [PATCH 11/14] refactor: activate flywheel, indexer and intake at same time --- indexer.py | 19 ++++++++++++------- intake.py | 2 ++ robot.py | 14 ++++++++++---- shooter.py | 4 +++- turret.py | 6 ++++++ 5 files changed, 33 insertions(+), 12 deletions(-) diff --git a/indexer.py b/indexer.py index 98850d8..02017cd 100644 --- a/indexer.py +++ b/indexer.py @@ -1,15 +1,20 @@ -from commands2 import ParallelCommandGroup -from commands2.cmd import run +from commands2 import ParallelCommandGroup, Subsystem from phoenix5 import WPI_VictorSPX import constants -class Indexer: +class Indexer(Subsystem): def __init__(self) -> None: self.front_roller = WPI_VictorSPX(constants.kFrontRoller) self.back_roller = WPI_VictorSPX(constants.kBackRoller) - def feed(self) -> Command: - return ParallelCommandGroup( - run(lambda: self.front_roller.set(-0.5)), - run(lambda: self.back_roller.set(1)) + self.setDefaultCommand( + self.run(lambda: self.deactivate()) ) + + def feed(self) -> None: + self.front_roller.set(-0.5) + self.back_roller.set(-0.7) + + def deactivate(self) -> None: + self.front_roller.set(0) + self.back_roller.set(0) \ No newline at end of file diff --git a/intake.py b/intake.py index 7773f38..1d25728 100644 --- a/intake.py +++ b/intake.py @@ -13,7 +13,9 @@ def __init__(self) -> None: SmartDashboard.putNumber("up", 0.5) SmartDashboard.putNumber("down", 0.5) + self.pivot.setInverted(True) + self.setDefaultCommand(self.run(lambda: self.stopGamePieceCollector())) def isPivotUp(self) -> bool: return self.pivotUp diff --git a/robot.py b/robot.py index 354c51b..7513088 100644 --- a/robot.py +++ b/robot.py @@ -9,6 +9,7 @@ from pathplannerlib.auto import AutoBuilder from wpilib import SmartDashboard, SendableChooser, DriverStation, Joystick from commands2.sysid import SysIdRoutine +from commands2.cmd import run from shooter import Shooter from turret import Turret from camera import Pixy2, LimelightCamera, PhotonVisionCamera @@ -29,13 +30,18 @@ def robotInit(self) -> None: JoystickButton(self.driverJoystick, 1).whileTrue( ParallelCommandGroup( self.shooter.activateFlywheel(), - self.indexer.feed() + run(lambda: self.indexer.feed()) ) ) - JoystickButton(self.driverJoystick, 7).whileTrue(self.intake.up()) - JoystickButton(self.driverJoystick, 8).whileTrue(self.intake.down()) - + JoystickButton(self.driverJoystick, 2).whileTrue(self.intake.up()) + JoystickButton(self.driverJoystick, 3).whileTrue(self.intake.down()) + JoystickButton(self.driverJoystick, 5).whileTrue(run(lambda: self.intake.collectGamePiece())) + JoystickButton(self.driverJoystick, 6).whileTrue(run(lambda: self.intake.releaseGamePiece())) + + JoystickButton(self.driverJoystick, 7).whileTrue(self.turret.activateYawClockwise()) + JoystickButton(self.driverJoystick, 8).whileTrue(self.turret.activateYawCounterClockwise()) + self.autoChooser.addOption( "LTV Controller Test Auto", AutoLTVController(self.drivetrain) ) diff --git a/shooter.py b/shooter.py index eea34bc..caa3484 100644 --- a/shooter.py +++ b/shooter.py @@ -43,6 +43,8 @@ def __init__(self) -> None: config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters ) + self.setDefaultCommand(self.deactivateFlywheel()) + def periodic(self) -> None: SmartDashboard.putData("PID", self.hood_pid) SmartDashboard.putNumber("crest encoder", self.hood_encoder.getPosition()) @@ -70,7 +72,7 @@ def down(self): self.moveTo(0) def activateFlywheel(self) -> Command: - return self.run(lambda: self.flywheel.set(0.7)) + return self.run(lambda: self.flywheel.set(0.2)) def deactivateFlywheel(self) -> Command: return self.run(lambda: self.flywheel.set(0)) diff --git a/turret.py b/turret.py index 64472ae..bc4c6eb 100644 --- a/turret.py +++ b/turret.py @@ -13,6 +13,12 @@ def __init__(self): # self.target_RPM = 4500 self.feedforward = SimpleMotorFeedforwardRadians(0, 0.002) + def activateYawClockwise(self) -> Command: + self.run(lambda: self.yaw.set(0.5)) + + def activateYawCounterClockwise(self) -> Command: + self.run(lambda: self.yaw.set(-0.5)) + def normalize(pixels, max_pixels): return (pixels * 2 / max_pixels) -1 From 91c03db94920b7d0144a81398926c6dae865773b Mon Sep 17 00:00:00 2001 From: Guilherme Samuel Date: Thu, 5 Mar 2026 03:50:01 -0300 Subject: [PATCH 12/14] refactor: add necessary rest commands for each subsystem --- camera.py | 32 +++++++++++++++++++++----------- constants.py | 2 +- indexer.py | 24 ++++++++++++++---------- intake.py | 14 +++++++------- led.py | 17 +++++++++-------- robot.py | 36 ++++++++++++++++++++++++------------ shooter.py | 45 ++++----------------------------------------- turret.py | 27 ++++++++++++++++++--------- utils.py | 5 ++++- 9 files changed, 102 insertions(+), 100 deletions(-) diff --git a/camera.py b/camera.py index 3e14d9d..6b7d061 100644 --- a/camera.py +++ b/camera.py @@ -23,16 +23,21 @@ def getYawFromTag(self, tag: int) -> float: def getYawAndRangeFromTag(self, tag: int) -> Tuple[float, float]: pass + @abstractmethod + def getYawFromBestTarget(self) -> float: + pass + class PhotonVisionCamera(AprilTagCamera): def __init__(self, camera: str) -> None: self.camera = PhotonCamera(camera) - - def getBestTarget(self) -> Optional[PhotonTrackedTarget]: + + def getYawFromBestTarget(self) -> float: result = self.camera.getLatestResult() if result.hasTargets(): target = result.getBestTarget() - return target - return None + if target is not None: + return target.getYaw() + return -1 def getYawFromTag(self, tag: int) -> float: results = self.camera.getAllUnreadResults() @@ -88,6 +93,9 @@ def getYawFromTag(self, tag: int) -> float: def getYawAndRangeFromTag(self, tag: int) -> Tuple[float, float]: return 0, 0 + + def getYawFromBestTarget(self) -> float: + return 0 class PixyFuelDetector(Subsystem): def __init__(self) -> None: @@ -103,15 +111,17 @@ def getBiggestBlock(self) -> Optional[Pixy2CCC.Block]: return None blocks = self.pixy.getCCC().getBlockCache() - largestBlock: Optional[PixyCCC.Block] = None + if blocks: + largestBlock: Optional[Pixy2CCC.Block] = None - for block in blocks: - if largestBlock is None: - largestBlock = block - elif block.getWidth() > largestBlock.getWidth(): - largestBlock = block + for block in blocks: + if largestBlock is None: + largestBlock = block + elif block.getWidth() > largestBlock.getWidth(): + largestBlock = block - return largestBlock + return largestBlock + return None def periodic(self) -> None: self.getBiggestBlock() diff --git a/constants.py b/constants.py index 3a33d6d..e468534 100644 --- a/constants.py +++ b/constants.py @@ -21,7 +21,7 @@ kDrivetrainPID = (0.2, 0, 0, ClosedLoopSlot.kSlot0) # kP, kI, kD # PhotonVision -kCameraName = "Camera7459" +kCameraName = "camera ps3" kCameraHeightMeters = 0.83 kTargetHeightMeters = 1.12 kCameraPitchRadians = 0 diff --git a/indexer.py b/indexer.py index 02017cd..a7eb481 100644 --- a/indexer.py +++ b/indexer.py @@ -1,5 +1,5 @@ -from commands2 import ParallelCommandGroup, Subsystem -from phoenix5 import WPI_VictorSPX +from commands2 import Command, Subsystem +from phoenix5 import WPI_VictorSPX, ControlMode import constants class Indexer(Subsystem): @@ -7,14 +7,18 @@ def __init__(self) -> None: self.front_roller = WPI_VictorSPX(constants.kFrontRoller) self.back_roller = WPI_VictorSPX(constants.kBackRoller) - self.setDefaultCommand( - self.run(lambda: self.deactivate()) - ) + self.setDefaultCommand(self.activateExpulse()) def feed(self) -> None: - self.front_roller.set(-0.5) - self.back_roller.set(-0.7) + self.front_roller.set(ControlMode.PercentOutput, -0.5) + self.back_roller.set(ControlMode.PercentOutput, -0.7) - def deactivate(self) -> None: - self.front_roller.set(0) - self.back_roller.set(0) \ No newline at end of file + def expulse(self) -> None: + self.front_roller.set(ControlMode.PercentOutput, 0) + self.back_roller.set(ControlMode.PercentOutput, 0) + + def activateFeed(self) -> Command: + return self.run(lambda: self.feed()) + + def activateExpulse(self) -> Command: + return self.run(lambda: self.expulse()) \ No newline at end of file diff --git a/intake.py b/intake.py index 1d25728..c39e24f 100644 --- a/intake.py +++ b/intake.py @@ -15,7 +15,7 @@ def __init__(self) -> None: SmartDashboard.putNumber("down", 0.5) self.pivot.setInverted(True) - self.setDefaultCommand(self.run(lambda: self.stopGamePieceCollector())) + self.setDefaultCommand(self.stopGamePieceCollector()) def isPivotUp(self) -> bool: return self.pivotUp @@ -57,11 +57,11 @@ def up(self) -> Command: def down(self) -> Command: return self.run(lambda: self.startPivotDown()) - def collectGamePiece(self) -> None: - self.roller.set(ControlMode.PercentOutput, -1) + def collectGamePiece(self) -> Command: + return self.run(lambda: self.roller.set(ControlMode.PercentOutput, -1)) - def stopGamePieceCollector(self) -> None: - self.roller.set(ControlMode.PercentOutput, 0) + def stopGamePieceCollector(self) -> Command: + return self.run(lambda: self.roller.set(ControlMode.PercentOutput, 0)) - def releaseGamePiece(self) -> None: - self.roller.set(ControlMode.PercentOutput, 1) + def releaseGamePiece(self) -> Command: + return self.run(lambda: self.roller.set(ControlMode.PercentOutput, 1)) \ No newline at end of file diff --git a/led.py b/led.py index 8021ddf..94b6cd0 100644 --- a/led.py +++ b/led.py @@ -1,22 +1,23 @@ from wpilib import SerialPort, Solenoid, PneumaticsModuleType +from commands2 import Command, Subsystem import constants -class LEDController: +class LEDController(Subsystem): def __init__(self) -> None: self.arduino = SerialPort(constants.kBaudRate, constants.kLEDUSBPort) self.extraLED = Solenoid(PneumaticsModuleType.CTREPCM, 4) - def red(self) -> None: - self.changeColor("r") + def red(self) -> Command: + return self.run(lambda: self.changeColor("r")) - def green(self) -> None: - self.changeColor("g") + def green(self) -> Command: + return self.run(lambda: self.changeColor("g")) - def blue(self) -> None: - self.changeColor("b") + def blue(self) -> Command: + return self.run(lambda: self.changeColor("b")) def setExtraLED(self, status: bool) -> None: - self.extraLED.set(status) + return self.extraLED.set(status) def changeColor(self, char: str) -> None: byte_obj = char.encode("ascii") diff --git a/robot.py b/robot.py index 7513088..001810d 100644 --- a/robot.py +++ b/robot.py @@ -1,50 +1,62 @@ import constants from drivetrain import Drivetrain -from autonomous.autoltvcontroller import AutoLTVController from autonomous.drivestraightpath import DriveStraightPath from commands2 import TimedCommandRobot, CommandScheduler, Command, ParallelCommandGroup from typing import Optional +from led import LEDController from commands2.button import JoystickButton from intake import Intake -from pathplannerlib.auto import AutoBuilder from wpilib import SmartDashboard, SendableChooser, DriverStation, Joystick -from commands2.sysid import SysIdRoutine -from commands2.cmd import run from shooter import Shooter from turret import Turret -from camera import Pixy2, LimelightCamera, PhotonVisionCamera +from camera import PhotonVisionCamera from indexer import Indexer class Robot(TimedCommandRobot): autonomous: Optional[Command] = None + autoChooser: SendableChooser = SendableChooser() drivetrain: Drivetrain = Drivetrain() intake: Intake = Intake() - autoChooser: SendableChooser = AutoBuilder.buildAutoChooser() driverJoystick: Joystick = Joystick(constants.kJoystickDriverPort) shooter: Shooter = Shooter() turret: Turret = Turret() indexer: Indexer = Indexer() + led: LEDController = LEDController() + turretCamera: PhotonVisionCamera = PhotonVisionCamera(constants.kCameraName) + + def combineAxis(self) -> float: + leftTrigger = -self.driverJoystick.getRawAxis(3) + rightTrigger = self.driverJoystick.getRawAxis(4) + + return rightTrigger + leftTrigger def robotInit(self) -> None: JoystickButton(self.driverJoystick, 1).whileTrue( ParallelCommandGroup( self.shooter.activateFlywheel(), - run(lambda: self.indexer.feed()) + self.indexer.activateFeed(), + self.intake.collectGamePiece() ) ) + + JoystickButton(self.driverJoystick, 1).onTrue(self.led.red()) JoystickButton(self.driverJoystick, 2).whileTrue(self.intake.up()) JoystickButton(self.driverJoystick, 3).whileTrue(self.intake.down()) - JoystickButton(self.driverJoystick, 5).whileTrue(run(lambda: self.intake.collectGamePiece())) - JoystickButton(self.driverJoystick, 6).whileTrue(run(lambda: self.intake.releaseGamePiece())) - + JoystickButton(self.driverJoystick, 4).whileTrue(self.turret.followYawTag(self.turretCamera)) JoystickButton(self.driverJoystick, 7).whileTrue(self.turret.activateYawClockwise()) JoystickButton(self.driverJoystick, 8).whileTrue(self.turret.activateYawCounterClockwise()) + + JoystickButton(self.driverJoystick, 6).whileTrue(self.intake.releaseGamePiece()) - self.autoChooser.addOption( - "LTV Controller Test Auto", AutoLTVController(self.drivetrain) + self.drivetrain.setDefaultCommand( + self.drivetrain.arcadeDrive( + lambda: self.combineAxis(), + lambda: self.driverJoystick.getRawAxis(0) + ) ) + self.autoChooser.addOption( "Drive Straight Path", DriveStraightPath(self.drivetrain, 5) ) diff --git a/shooter.py b/shooter.py index caa3484..915c760 100644 --- a/shooter.py +++ b/shooter.py @@ -7,17 +7,9 @@ SparkBaseConfig, ) from commands2 import Subsystem, Command -from wpilib import SmartDashboard -from wpimath.controller import ( - BangBangController, - SimpleMotorFeedforwardMeters, - PIDController, -) +from wpimath.controller import PIDController from wpimath.units import rotationsPerMinuteToRadiansPerSecond import constants -from utils import Utils - -gear_ratio = 11.52 class Shooter(Subsystem): def __init__(self) -> None: @@ -33,7 +25,7 @@ def __init__(self) -> None: config = SparkMaxConfig() - config.smartCurrentLimit(40) + config.smartCurrentLimit(30) config.setIdleMode(SparkBaseConfig.IdleMode.kCoast) self.flywheel.configure( @@ -45,37 +37,8 @@ def __init__(self) -> None: self.setDefaultCommand(self.deactivateFlywheel()) - def periodic(self) -> None: - SmartDashboard.putData("PID", self.hood_pid) - SmartDashboard.putNumber("crest encoder", self.hood_encoder.getPosition()) - SmartDashboard.putNumber("crest position", float(self.getPosition())) - - def getPosition(self) -> float: - return self.hood_encoder.getPosition() * gear_ratio * 360 - - def moveToSetpoint(self): - current_position = self.getPosition() - hood_value = self.hood_pid.calculate(current_position) - hood_value = Utils.clamp(hood_value, -0.4, 0.4) - self.hood.set(hood_value) - - def moveTo(self, setpoint): - current_position = self.getPosition() - hood_value = self.hood_pid.calculate(current_position, setpoint) - hood_value = Utils.clamp(hood_value, -0.4, 0.4) - self.hood.set(hood_value) - - def up(self): - self.moveTo(20) - - def down(self): - self.moveTo(0) - def activateFlywheel(self) -> Command: - return self.run(lambda: self.flywheel.set(0.2)) + return self.run(lambda: self.flywheel.set(0.4)) def deactivateFlywheel(self) -> Command: - return self.run(lambda: self.flywheel.set(0)) - - def commandMoveToSetpoint(self) -> Command: - return self.run(lambda: self.moveToSetpoint) \ No newline at end of file + return self.run(lambda: self.flywheel.set(0)) \ No newline at end of file diff --git a/turret.py b/turret.py index bc4c6eb..76be18e 100644 --- a/turret.py +++ b/turret.py @@ -1,6 +1,8 @@ from rev import SparkMax, SparkLowLevel from wpimath.controller import PIDController, SimpleMotorFeedforwardRadians -from commands2 import Subsystem, command +from commands2 import Subsystem, Command +from utils import Utils +from camera import AprilTagCamera import constants @@ -8,25 +10,32 @@ class Turret(Subsystem): def __init__(self): self.yaw = SparkMax(constants.kTurretId, SparkLowLevel.MotorType.kBrushless) self.encoder = self.yaw.getEncoder() - self.pid = PIDController(0.00025, 0, 0) + self.pid = PIDController(0.05, 0, 0) self.pid.setTolerance(0.1) # self.target_RPM = 4500 self.feedforward = SimpleMotorFeedforwardRadians(0, 0.002) + + self.setDefaultCommand(self.stopYaw()) def activateYawClockwise(self) -> Command: - self.run(lambda: self.yaw.set(0.5)) + return self.run(lambda: self.yaw.set(0.5)) - def activateYawCounterClockwise(self) -> Command: - self.run(lambda: self.yaw.set(-0.5)) + def followYawTag(self, camera: AprilTagCamera) -> Command: + yaw = camera.getYawFromBestTarget() + rotation = self.pid.calculate(yaw, 0) if yaw != -1 else 0 + return self.run(lambda: self.yaw.set(rotation)) - def normalize(pixels, max_pixels): - return (pixels * 2 / max_pixels) -1 + def stopYaw(self) -> Command: + return self.run(lambda: self.yaw.set(0)) + + def activateYawCounterClockwise(self) -> Command: + return self.run(lambda: self.yaw.set(-0.5)) def clamp(self,value, min_value, max_value): return max(min(value, max_value), min_value) def centerTurret(self, alignment): - #normalized_alignment = self.normalize(alignment, 1080)#chutando que a camera tem 1080 + normalized_alignment = Utils.normalize(alignment, 1080) #chutando que a camera tem 1080 output = self.pid.calculate(alignment, 0) @@ -34,5 +43,5 @@ def centerTurret(self, alignment): self.yaw.setVoltage(output * 12) - def commandCenterTurret(self,alignment) -> command: + def commandCenterTurret(self,alignment) -> Command: return self.run(lambda: self.centerTurret(alignment)) \ No newline at end of file diff --git a/utils.py b/utils.py index de7ad62..510b9e6 100644 --- a/utils.py +++ b/utils.py @@ -18,10 +18,13 @@ def calculateDistanceToTargetMeters( def readString(port: SerialPort) -> str: port_bytes = port.getBytesReceived() buffer = bytearray(port_bytes) - converted = port.read() + converted = port.read(buffer) return buffer[:converted].decode("ascii") @staticmethod def clamp(value, min_value, max_value): return max(min(value, max_value), min_value) + @staticmethod + def normalize(pixels, max_pixels): + return (pixels * 2 / max_pixels) -1 \ No newline at end of file From 33531af7cda77816d6eca091ec348f709721585a Mon Sep 17 00:00:00 2001 From: Guilherme Samuel Date: Thu, 5 Mar 2026 04:50:09 -0300 Subject: [PATCH 13/14] feat: add blink green and rainbow led combination colors options --- led.py | 10 ++++++++++ ws2812b/ws2812b.ino | 21 +++++++++++++++++++++ 2 files changed, 31 insertions(+) diff --git a/led.py b/led.py index 94b6cd0..6f5f590 100644 --- a/led.py +++ b/led.py @@ -6,6 +6,7 @@ class LEDController(Subsystem): def __init__(self) -> None: self.arduino = SerialPort(constants.kBaudRate, constants.kLEDUSBPort) self.extraLED = Solenoid(PneumaticsModuleType.CTREPCM, 4) + self.status = "off" def red(self) -> Command: return self.run(lambda: self.changeColor("r")) @@ -16,9 +17,18 @@ def green(self) -> Command: def blue(self) -> Command: return self.run(lambda: self.changeColor("b")) + def blinkGreen(self) -> Command: + return self.run(lambda: self.changeColor("w")) + + def rainbow(self) -> Command: + return self.run(lambda: self.changeColor("a")) + def setExtraLED(self, status: bool) -> None: return self.extraLED.set(status) def changeColor(self, char: str) -> None: + if self.status == char: + return + self.status = char byte_obj = char.encode("ascii") self.arduino.write(byte_obj) diff --git a/ws2812b/ws2812b.ino b/ws2812b/ws2812b.ino index 31dfb4f..ab5f4a5 100644 --- a/ws2812b/ws2812b.ino +++ b/ws2812b/ws2812b.ino @@ -4,6 +4,7 @@ #define NUM_LEDS 60 CRGB leds[NUM_LEDS]; +uint8_t waveOffset = 0; void changeColor(int red, int green, int blue) { for (int i = 0; i < NUM_LEDS; i++) @@ -11,6 +12,22 @@ void changeColor(int red, int green, int blue) { FastLED.show(); } +void rainbow() { + for(int i = 0; i < NUM_LEDS; i++) { + uint8_t wave = sin8(i * 12 + waveOffset); + leds[i] = CHSV(i * 5 + waveOffset, 255, wave); + } + FastLED.show(); + waveOffset += 4; + delay(20); +} + +void blinkGreen() { + changeColor(0, 255, 0); + delay(500); + changeColor(0, 0, 0); +} + void updateSelectedColor() { if (Serial.available() > 0) { byte received = Serial.read(); @@ -20,6 +37,10 @@ void updateSelectedColor() { changeColor(0, 255, 0); else if (received == 'b') changeColor(0, 0, 255); + else if (received == 'w') + blinkGreen(); + else if (received == 'a') + rainbow(); } } From cc02d15c94b25456925ce6e783042d964b9122e3 Mon Sep 17 00:00:00 2001 From: Guilherme Samuel Date: Thu, 5 Mar 2026 04:50:35 -0300 Subject: [PATCH 14/14] combine more commands --- camera.py | 23 +++++++++++++++++++++-- drivetrain.py | 44 +++++++++++++++++++++++++------------------- robot.py | 49 ++++++++++++++++++++++++++++++++++++++----------- shooter.py | 15 +++++++++++++-- turret.py | 27 ++++++++++++++++++++++----- 5 files changed, 119 insertions(+), 39 deletions(-) diff --git a/camera.py b/camera.py index 6b7d061..8cca87f 100644 --- a/camera.py +++ b/camera.py @@ -26,6 +26,10 @@ def getYawAndRangeFromTag(self, tag: int) -> Tuple[float, float]: @abstractmethod def getYawFromBestTarget(self) -> float: pass + + @abstractmethod + def getRangeFromBestTarget(self) -> float: + pass class PhotonVisionCamera(AprilTagCamera): def __init__(self, camera: str) -> None: @@ -39,6 +43,19 @@ def getYawFromBestTarget(self) -> float: return target.getYaw() return -1 + def getRangeFromBestTarget(self) -> float: + result = self.camera.getLatestResult() + if result.hasTargets(): + target = result.getBestTarget() + if target is not None: + return Utils.calculateDistanceToTargetMeters( + constants.kCameraHeightMeters, + constants.kTargetHeightMeters, + constants.kCameraPitchRadians, + degreesToRadians(target.getPitch()), + ) + return 0 + def getYawFromTag(self, tag: int) -> float: results = self.camera.getAllUnreadResults() if len(results) > 0: @@ -63,8 +80,7 @@ def getYawAndRangeFromTag(self, tag: int) -> Tuple[float, float]: ) return target.getYaw(), target_range return -1, -1 - - + class LimelightCamera(AprilTagCamera): def __init__(self, camera: str) -> None: self.limelight = None @@ -96,6 +112,9 @@ def getYawAndRangeFromTag(self, tag: int) -> Tuple[float, float]: def getYawFromBestTarget(self) -> float: return 0 + + def getRangeFromBestTarget(self) -> float: + return 0 class PixyFuelDetector(Subsystem): def __init__(self) -> None: diff --git a/drivetrain.py b/drivetrain.py index 648ca52..05c3faa 100644 --- a/drivetrain.py +++ b/drivetrain.py @@ -53,6 +53,7 @@ def __init__(self) -> None: self.drivetrain.setMaxOutput(1.0) self.field = Field2d() + self.slowMode = False config = SparkMaxConfig() @@ -208,8 +209,15 @@ def backward(self) -> Command: def arcadeDrive( self, speed: Callable[[], float], rotate: Callable[[], float] ) -> Command: + if self.slowMode: + return self.run( + lambda: self.drivetrain.arcadeDrive(speed() * 0.5, rotate() * 0.5) + ) return self.run(lambda: self.drivetrain.arcadeDrive(speed(), rotate())) + def setSlowMode(self) -> Command: + return self.run(lambda: setattr(self, "slowMode", not self.slowMode)) + def cheesyDrive( self, speed: Callable[[], float], @@ -265,7 +273,7 @@ def aim(self, camera: AprilTagCamera, tag: int) -> Command: if yaw != -1: return PIDCommand( PIDController(*constants.kDrivetrainPID[:3]), - yaw, + lambda: yaw, 0, lambda output: self.drivetrain.arcadeDrive(0, output), self, @@ -278,30 +286,28 @@ def aimAndRange(self, camera: AprilTagCamera, tag: int) -> Command: if yaw != -1: return SequentialCommandGroup( - [ - PIDCommand( - PIDController(*constants.kDrivetrainPID), - yaw, - 0, - lambda output: self.drivetrain.arcadeDrive(0, output), - self, - ), - PIDCommand( - PIDController(*constants.kDrivetrainPID), - range, - constants.kGoalRangeMeters, - lambda output: self.drivetrain.arcadeDrive(output, 0), - self, - ), - ] + PIDCommand( + PIDController(*constants.kDrivetrainPID[:3]), + lambda: yaw, + 0, + lambda output: self.drivetrain.arcadeDrive(0, output), + self, + ), + PIDCommand( + PIDController(*constants.kDrivetrainPID[:3]), + lambda:range, + constants.kGoalRangeMeters, + lambda output: self.drivetrain.arcadeDrive(output, 0), + self, + ), ) return self.run(lambda: self.drivetrain.arcadeDrive(0, 0)) def rotate(self, angle: float) -> Command: return PIDCommand( - PIDController(*constants.kDrivetrainPID), - self.navx.getAngle(), + PIDController(*constants.kDrivetrainPID[:3]), + lambda: self.navx.getAngle(), angle, lambda output: self.drivetrain.arcadeDrive(0, output), self, diff --git a/robot.py b/robot.py index 001810d..ebb4469 100644 --- a/robot.py +++ b/robot.py @@ -1,7 +1,7 @@ import constants from drivetrain import Drivetrain from autonomous.drivestraightpath import DriveStraightPath -from commands2 import TimedCommandRobot, CommandScheduler, Command, ParallelCommandGroup +from commands2 import TimedCommandRobot, CommandScheduler, Command, ParallelCommandGroup, SequentialCommandGroup from typing import Optional from led import LEDController from commands2.button import JoystickButton @@ -26,13 +26,13 @@ class Robot(TimedCommandRobot): turretCamera: PhotonVisionCamera = PhotonVisionCamera(constants.kCameraName) def combineAxis(self) -> float: - leftTrigger = -self.driverJoystick.getRawAxis(3) - rightTrigger = self.driverJoystick.getRawAxis(4) + leftTrigger = -self.driverJoystick.getRawAxis(2) + rightTrigger = self.driverJoystick.getRawAxis(3) return rightTrigger + leftTrigger def robotInit(self) -> None: - JoystickButton(self.driverJoystick, 1).whileTrue( + JoystickButton(self.driverJoystick, 5).whileTrue( ParallelCommandGroup( self.shooter.activateFlywheel(), self.indexer.activateFeed(), @@ -40,15 +40,39 @@ def robotInit(self) -> None: ) ) - JoystickButton(self.driverJoystick, 1).onTrue(self.led.red()) + JoystickButton(self.driverJoystick, 5).onTrue(self.led.red()) - JoystickButton(self.driverJoystick, 2).whileTrue(self.intake.up()) - JoystickButton(self.driverJoystick, 3).whileTrue(self.intake.down()) - JoystickButton(self.driverJoystick, 4).whileTrue(self.turret.followYawTag(self.turretCamera)) - JoystickButton(self.driverJoystick, 7).whileTrue(self.turret.activateYawClockwise()) - JoystickButton(self.driverJoystick, 8).whileTrue(self.turret.activateYawCounterClockwise()) + JoystickButton(self.driverJoystick, 1).onTrue(self.drivetrain.setSlowMode()) + JoystickButton(self.driverJoystick, 3).onTrue( + SequentialCommandGroup( + self.intake.up(), + self.intake.down() + ) + ) + + JoystickButton(self.driverJoystick, 2).whileTrue( + SequentialCommandGroup( + self.intake.down(), + self.intake.releaseGamePiece() + ) + ) - JoystickButton(self.driverJoystick, 6).whileTrue(self.intake.releaseGamePiece()) + JoystickButton(self.driverJoystick, 6).whileTrue( + self.turret.followYawTag(self.turretCamera, self.led) + ) + + ''' + JoystickButton(self.driverJoystick, 6).whileTrue( + ParallelCommandGroup( + self.turret.followYawTag(self.turretCamera, self.led) + self.shooter.openHoodByDistanceOfTag(self.turretCamera) + ) + ) + ''' + + self.turret.setDefaultCommand( + self.turret.activateYaw(lambda: self.driverJoystick.getRawAxis(4)) + ) self.drivetrain.setDefaultCommand( self.drivetrain.arcadeDrive( @@ -61,6 +85,9 @@ def robotInit(self) -> None: "Drive Straight Path", DriveStraightPath(self.drivetrain, 5) ) SmartDashboard.putData("Auto Chooser", self.autoChooser) + + def teleopExit(self) -> None: + self.led.rainbow() def autonomousInit(self) -> None: DriverStation.silenceJoystickConnectionWarning(True) diff --git a/shooter.py b/shooter.py index 915c760..6b7f7ae 100644 --- a/shooter.py +++ b/shooter.py @@ -7,8 +7,8 @@ SparkBaseConfig, ) from commands2 import Subsystem, Command +from camera import AprilTagCamera from wpimath.controller import PIDController -from wpimath.units import rotationsPerMinuteToRadiansPerSecond import constants class Shooter(Subsystem): @@ -41,4 +41,15 @@ def activateFlywheel(self) -> Command: return self.run(lambda: self.flywheel.set(0.4)) def deactivateFlywheel(self) -> Command: - return self.run(lambda: self.flywheel.set(0)) \ No newline at end of file + return self.run(lambda: self.flywheel.set(0)) + + def hoodUp(self) -> Command: + return self.run(lambda: self.hood.set(0.5)) + + def hoodDown(self) -> Command: + return self.run(lambda: self.hood.set(-0.5)) + + def openHoodByDistanceOfTag(self, camera: AprilTagCamera) -> Command: + range = camera.getRangeFromBestTarget() + rotation = self.hood_pid.calculate(range, 0) + return self.run(lambda: self.hood.set(rotation)) \ No newline at end of file diff --git a/turret.py b/turret.py index 76be18e..b4100e8 100644 --- a/turret.py +++ b/turret.py @@ -1,8 +1,10 @@ from rev import SparkMax, SparkLowLevel from wpimath.controller import PIDController, SimpleMotorFeedforwardRadians -from commands2 import Subsystem, Command +from commands2 import Subsystem, Command, ParallelCommandGroup from utils import Utils from camera import AprilTagCamera +from typing import Callable, Optional +from led import LEDController import constants @@ -14,15 +16,30 @@ def __init__(self): self.pid.setTolerance(0.1) # self.target_RPM = 4500 self.feedforward = SimpleMotorFeedforwardRadians(0, 0.002) - - self.setDefaultCommand(self.stopYaw()) def activateYawClockwise(self) -> Command: return self.run(lambda: self.yaw.set(0.5)) - def followYawTag(self, camera: AprilTagCamera) -> Command: + def activateYaw(self, rotate: Callable[[], float]) -> Command: + return self.run(lambda: self.yaw.set(rotate())) + + def followYawTag(self, camera: AprilTagCamera, led: Optional[LEDController]=None) -> Command: yaw = camera.getYawFromBestTarget() - rotation = self.pid.calculate(yaw, 0) if yaw != -1 else 0 + rotation = 0 + if yaw != -1: + rotation = self.pid.calculate(yaw, 0) + if led is not None: + return ParallelCommandGroup( + led.blinkGreen(), + self.run(lambda: self.yaw.set(rotation)) + ) + + if led is not None: + return ParallelCommandGroup( + led.red(), + self.run(lambda: self.yaw.set(rotation)) + ) + return self.run(lambda: self.yaw.set(rotation)) def stopYaw(self) -> Command: