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
+
diff --git a/camera.py b/camera.py
index b5afcbf..8cca87f 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):
@@ -22,16 +23,38 @@ def getYawFromTag(self, tag: int) -> float:
     def getYawAndRangeFromTag(self, tag: int) -> Tuple[float, float]:
         pass
 
+    @abstractmethod
+    def getYawFromBestTarget(self) -> float:
+        pass
+    
+    @abstractmethod
+    def getRangeFromBestTarget(self) -> float:
+        pass
+
 class PhotonVisionCamera(AprilTagCamera):
     def __init__(self, camera: str) -> None:
         self.camera = PhotonCamera(camera)
+    
+    def getYawFromBestTarget(self) -> float:
+        result = self.camera.getLatestResult()
+        if result.hasTargets():
+            target = result.getBestTarget()
+            if target is not None:
+                return target.getYaw()
+        return -1
 
-    def getBestTarget(self) -> Optional[PhotonTrackedTarget]:
+    def getRangeFromBestTarget(self) -> float:
         result = self.camera.getLatestResult()
         if result.hasTargets():
             target = result.getBestTarget()
-            return target
-        return None
+            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()
@@ -57,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
@@ -87,27 +109,38 @@ def getYawFromTag(self, tag: int) -> float:
 
     def getYawAndRangeFromTag(self, tag: int) -> Tuple[float, float]:
         return 0, 0
-
-class PixyFuelDetector:
+    
+    def getYawFromBestTarget(self) -> float:
+        return 0
+    
+    def getRangeFromBestTarget(self) -> float:
+        return 0
+
+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
 
         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 None
 
-        return largestBlock
+    def periodic(self) -> None:
+        self.getBiggestBlock()
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/constants.py b/constants.py
index 1954fe6..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
@@ -63,12 +63,20 @@
 kLEDUSBPort = SerialPort.Port.kUSB1
 
 # Intake
-kIntakeAngleId = 1
-kIntakeTrackId = 12
+kIntakeAngleId = 4
+kIntakeTrackId = 1
 kPivotTimeDown = 0.35
 kPivotTimeUp = 0.5
 
 # Shooter
-kFlywheelId = 11
+kFlywheelId = 51
 kFlywheelFeedForward = (0, 0, 0, ClosedLoopSlot.kSlot0)  # kS, kV, kA
 kFlywheelPID = (0.2, 0.1, 0, ClosedLoopSlot.kSlot0)  # kP, kI, kD
+kHoodId = 53
+
+# Indexer
+kFrontRoller = 5 # 775 RedLine
+kBackRoller = 2
+
+# Turret
+kTurretId = 56
\ No newline at end of file
diff --git a/crest.py b/crest.py
deleted file mode 100644
index ab75ada..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()
\ No newline at end of file
diff --git a/drivetrain.py b/drivetrain.py
index f704692..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,31 +286,30 @@ 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,
         )
+    
\ No newline at end of file
diff --git a/indexer.py b/indexer.py
new file mode 100644
index 0000000..a7eb481
--- /dev/null
+++ b/indexer.py
@@ -0,0 +1,24 @@
+from commands2 import Command, Subsystem
+from phoenix5 import WPI_VictorSPX, ControlMode
+import constants
+
+class Indexer(Subsystem):
+    def __init__(self) -> None:
+        self.front_roller = WPI_VictorSPX(constants.kFrontRoller)
+        self.back_roller = WPI_VictorSPX(constants.kBackRoller)
+
+        self.setDefaultCommand(self.activateExpulse())
+
+    def feed(self) -> None:
+        self.front_roller.set(ControlMode.PercentOutput, -0.5)
+        self.back_roller.set(ControlMode.PercentOutput, -0.7)
+    
+    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 f7c93c8..c39e24f 100644
--- a/intake.py
+++ b/intake.py
@@ -5,15 +5,17 @@
 
 
 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)
+        self.setDefaultCommand(self.stopGamePieceCollector())
 
     def isPivotUp(self) -> bool:
         return self.pivotUp
@@ -55,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 897be66..6f5f590 100644
--- a/led.py
+++ b/led.py
@@ -1,20 +1,34 @@
-from wpilib import SerialPort
-from dataclasses import dataclass
+from wpilib import SerialPort, Solenoid, PneumaticsModuleType
+from commands2 import Command, Subsystem
 import constants
 
-@dataclass
-class LEDController:
-    arduino: SerialPort = SerialPort(constants.kBaudRate, constants.kLEDUSBPort)
+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) -> 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 changeColor(char: str) -> None:
+    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/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..ebb4469 100644
--- a/robot.py
+++ b/robot.py
@@ -1,63 +1,94 @@
 import constants
 from drivetrain import Drivetrain
-from genericjoystick import GenericJoystick
-from autonomous.autoltvcontroller import AutoLTVController
 from autonomous.drivestraightpath import DriveStraightPath
-from commands2 import TimedCommandRobot, CommandScheduler, Command
+from commands2 import TimedCommandRobot, CommandScheduler, Command, ParallelCommandGroup, SequentialCommandGroup
 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
-from commands2.sysid import SysIdRoutine
+from wpilib import SmartDashboard, SendableChooser, DriverStation, Joystick
 from shooter import Shooter
-from camera import Pixy2, LimelightCamera, PhotonVisionCamera
+from turret import Turret
+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: GenericJoystick = GenericJoystick(constants.kJoystickDriverPort)
+    driverJoystick: Joystick = Joystick(constants.kJoystickDriverPort)
     shooter: Shooter = Shooter()
+    turret: Turret = Turret()
+    indexer: Indexer = Indexer()
+    led: LEDController = LEDController()
+    turretCamera: PhotonVisionCamera = PhotonVisionCamera(constants.kCameraName)
 
-    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)
-
-    def teleopInit(self) -> None:
-        JoystickButton(self.driverJoystick, 5).onTrue(self.intake.up())
+    def combineAxis(self) -> float:
+        leftTrigger = -self.driverJoystick.getRawAxis(2)
+        rightTrigger = self.driverJoystick.getRawAxis(3)
 
-        JoystickButton(self.driverJoystick, 6).onTrue(self.intake.down())
+        return rightTrigger + leftTrigger
 
-        JoystickButton(self.driverJoystick, 1).onTrue(
-            self.drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)
+    def robotInit(self) -> None:
+        JoystickButton(self.driverJoystick, 5).whileTrue(
+            ParallelCommandGroup(
+                self.shooter.activateFlywheel(),
+                self.indexer.activateFeed(),
+                self.intake.collectGamePiece()
+            )
         )
+        
+        JoystickButton(self.driverJoystick, 5).onTrue(self.led.red())
 
-        JoystickButton(self.driverJoystick, 2).onTrue(
-            self.drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kForward)
+        JoystickButton(self.driverJoystick, 1).onTrue(self.drivetrain.setSlowMode())
+        JoystickButton(self.driverJoystick, 3).onTrue(
+            SequentialCommandGroup(
+                self.intake.up(),
+                self.intake.down()
+            )
         )
 
-        JoystickButton(self.driverJoystick, 3).onTrue(
-            self.drivetrain.sysIdDynamic(SysIdRoutine.Direction.kForward)
+        JoystickButton(self.driverJoystick, 2).whileTrue(
+            SequentialCommandGroup(
+                self.intake.down(),
+                self.intake.releaseGamePiece()
+            )
+        )
+        
+        JoystickButton(self.driverJoystick, 6).whileTrue(
+            self.turret.followYawTag(self.turretCamera, self.led)
         )
 
-        JoystickButton(self.driverJoystick, 4).onTrue(
-            self.drivetrain.sysIdDynamic(SysIdRoutine.Direction.kReverse)
+        '''
+        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(
-                lambda: self.driverJoystick.getLeftYAxis(),
-                lambda: self.driverJoystick.getRightXAxis(),
+                lambda: self.combineAxis(),
+                lambda: self.driverJoystick.getRawAxis(0)
             )
         )
 
+        self.autoChooser.addOption(
+            "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)
         self.autonomous = self.autoChooser.getSelected()
@@ -69,9 +100,4 @@ def autonomousPeriodic(self) -> None:
         pass
 
     def autonomousExit(self) -> None:
-        CommandScheduler.getInstance().cancelAll()
-
-    def testInit(self) -> None:
-        JoystickButton(self.driverJoystick, 1).onTrue(
-            self.shooter.setFlywheelBySetpointCommand()
-        )
+        CommandScheduler.getInstance().cancelAll()
\ No newline at end of file
diff --git a/shooter.py b/shooter.py
index 2e80ce7..6b7f7ae 100644
--- a/shooter.py
+++ b/shooter.py
@@ -7,87 +7,49 @@
     SparkBaseConfig,
 )
 from commands2 import Subsystem, Command
-from commands2.cmd import run
-from wpilib import SmartDashboard
-from wpimath.controller import (
-    BangBangController,
-    SimpleMotorFeedforwardMeters,
-    PIDController,
-)
-from wpimath.units import rotationsPerMinuteToRadiansPerSecond
+from camera import AprilTagCamera
+from wpimath.controller import PIDController
 import constants
 
-
 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.hood = SparkMax(constants.kHoodId, SparkMax.MotorType.kBrushless)
+        self.hood_encoder = self.hood.getEncoder()
+
+        self.hood_pid = PIDController(0.006, 0.0, 0.0)
+        self.hood_pid.setTolerance(0.1)
+        self.hood_encoder.setPosition(0)
+
         config = SparkMaxConfig()
 
-        config.smartCurrentLimit(40)
+        config.smartCurrentLimit(30)
         config.setIdleMode(SparkBaseConfig.IdleMode.kCoast)
-        config.closedLoop.feedForward.sva(*constants.kFlywheelFeedForward)
-        config.closedLoop.pid(*constants.kFlywheelPID)
-
-        self.feedforward = SimpleMotorFeedforwardMeters(
-            *constants.kFlywheelFeedForward[:3]
-        )
-        self.pid = PIDController(*constants.kFlywheelPID[:3])
 
         self.flywheel.configure(
             config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters
         )
-
-        self.closedLoopController = self.flywheel.getClosedLoopController()
-
-        self.encoder = self.flywheel.getEncoder()
-        SmartDashboard.putData(self.bangBangController)
-
-        SmartDashboard.putData("Shooter BangBang Controller", self.bangBangController)
-        SmartDashboard.putNumberArray(
-            "Shooter PID Controller", [*constants.kFlywheelPID[:3]]
-        )
-        SmartDashboard.putNumber("Shooter Setpoint", 0)
-        SmartDashboard.putNumberArray(
-            "Shooter FeedForward", [*constants.kFlywheelFeedForward[:3]]
+        self.hood.configure(
+            config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters
         )
 
-    def activate(self) -> None:
-        self.flywheel.set(0.7)
+        self.setDefaultCommand(self.deactivateFlywheel())
 
-    def periodic(self) -> None:
-        feedforward = SmartDashboard.getNumberArray("Shooter FeedForward", [0, 0, 0])
-        self.pid.setPID(
-            *SmartDashboard.getNumberArray("Shooter PID Controller", [0, 0, 0])
-        )
-        self.feedforward.setKs(feedforward[0])
-        self.feedforward.setKv(feedforward[1])
-        self.feedforward.setKa(feedforward[2])
+    def activateFlywheel(self) -> Command:
+        return self.run(lambda: self.flywheel.set(0.4))
 
-    def setFlywheelBySetpoint(self, setpoint: float) -> None:
-        output = self.pid.calculate(self.encoder.getPosition(), setpoint)
-        self.flywheel.set(output)
+    def deactivateFlywheel(self) -> Command:
+        return self.run(lambda: self.flywheel.set(0))
 
-    def setFlywheelBySetpointCommand(self) -> Command:
-        return run(
-            lambda: self.setFlywheelBySetpoint(
-                SmartDashboard.getNumber("Shooter Setpoint", 0)
-            )
-        )
-
-    def deactivate(self) -> None:
-        self.flywheel.set(0)
+    def hoodUp(self) -> Command:
+        return self.run(lambda: self.hood.set(0.5))
 
-    def bangBangActivate(self, maxSetpoint: float) -> None:
-        setpoint = max(0, rotationsPerMinuteToRadiansPerSecond(maxSetpoint))
-        output = (
-            self.bangBangController.calculate(self.encoder.getPosition(), setpoint)
-            * 12.0
-        )
-        self.closedLoopController.setReference(
-            output + 0.9 * self.feedforward.calculate(setpoint),
-            SparkMax.ControlType.kVoltage,
-        )
+    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 40a9896..b4100e8 100644
--- a/turret.py
+++ b/turret.py
@@ -1,32 +1,64 @@
 from rev import SparkMax, SparkLowLevel
 from wpimath.controller import PIDController, SimpleMotorFeedforwardRadians
-from commands2 import Subsystem
+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
 
 
 class Turret(Subsystem):
     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.yaw = SparkMax(constants.kTurretId, SparkLowLevel.MotorType.kBrushless)
+        self.encoder = self.yaw.getEncoder()
+        self.pid = PIDController(0.05, 0, 0)
+        self.pid.setTolerance(0.1)
+        # self.target_RPM = 4500
         self.feedforward = SimpleMotorFeedforwardRadians(0, 0.002)
+    
+    def activateYawClockwise(self) -> Command:
+        return self.run(lambda: self.yaw.set(0.5))
 
-    def shoot(self):
-        self.target_RPM = 4500
+    def activateYaw(self, rotate: Callable[[], float]) -> Command:
+        return self.run(lambda: self.yaw.set(rotate()))
 
-    def stop(self):
-        self.target_RPM = 0
-        self.turret_motor.stopMotor()
-        self.pid.reset()
+    def followYawTag(self, camera: AprilTagCamera, led: Optional[LEDController]=None) -> Command:
+        yaw = camera.getYawFromBestTarget()
+        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))
+                    )
 
-    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)
+        if led is not None:
+            return ParallelCommandGroup(
+                    led.red(),
+                    self.run(lambda: self.yaw.set(rotation))
+                )
 
-    def isReady(self):
-        return abs(self.encoder.getVelocity() - self.target_RPM) < 100
+        return self.run(lambda: self.yaw.set(rotation))
+
+    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 = Utils.normalize(alignment, 1080) #chutando que a camera tem 1080
+
+        output = self.pid.calculate(alignment, 0)
+
+        output = self.clamp(output, -1,1)
+
+        self.yaw.setVoltage(output * 12)
+
+    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 3929442..510b9e6 100644
--- a/utils.py
+++ b/utils.py
@@ -18,5 +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
diff --git a/led-arduino/ws2812b_pixy2.ino b/ws2812b/ws2812b.ino
similarity index 58%
rename from led-arduino/ws2812b_pixy2.ino
rename to ws2812b/ws2812b.ino
index 666730a..ab5f4a5 100644
--- a/led-arduino/ws2812b_pixy2.ino
+++ b/ws2812b/ws2812b.ino
@@ -1,11 +1,10 @@
 #include <FastLED.h>
-#include <Pixy2.h>
 
 #define LED_PIN 7
-#define NUM_LEDS 30
+#define NUM_LEDS 60
 
 CRGB leds[NUM_LEDS];
-Pixy2 pixy;
+uint8_t waveOffset = 0;
 
 void changeColor(int red, int green, int blue) {
   for (int i = 0; i < NUM_LEDS; i++)
@@ -13,32 +12,35 @@ 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 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();
-    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);
-    }
+    else if (received == 'w')
+      blinkGreen();
+    else if (received == 'a')
+      rainbow();
   }
 }
 
@@ -46,7 +48,6 @@ void setup() {
   Serial.begin(9600);
   FastLED.addLeds<WS2812, LED_PIN, GRB>(leds, NUM_LEDS);
   changeColor(255, 0, 0);
-  pixy.init();
 }
 
 void loop() {