diff --git a/src/main/cpp/Autonomous.cpp b/src/main/cpp/Autonomous.cpp
index d1006c4..7a44697 100644
--- a/src/main/cpp/Autonomous.cpp
+++ b/src/main/cpp/Autonomous.cpp
@@ -105,9 +105,10 @@ void Autonomous::PeriodicDoNothing() {
     a_AutoState0 = nextState;
 }
 
-
+//BLUE LEFT RED LEFT
 void Autonomous::NoteOne() {
-    a_SwerveDrive->zeroPose(frc::Pose2d(units::meter_t(.47), units::meter_t(.79), units::degree_t(60.0)));
+    a_SwerveDrive->zeroPose(frc::Pose2d(units::meter_t(.47), units::meter_t(-.79), units::degree_t(60.0)));
+    a_Gyro->Zero(60.0);
     a_AutoState1 = kShoot1stNote1;
     state_time = gettime_d();
 }
@@ -121,38 +122,46 @@ void Autonomous::PeriodicNoteOne() {
             StopSwerves();
             break;
         case kShoot1stNote1:
-            a_NoteHandler->indexToShoot();
-            nextState = kAutoIdle1;
-            break;
-        case kGoTo2ndNote1:
-            a_NoteHandler->collectNote(-0.4, false);
-            if(a_SwerveDrive -> odometryGoToPose(8.277, -1.912, 0.0)){
-                nextState = kShoot2ndNote1;
-            }
-            break;
-        case kShoot2ndNote1:
-            if(a_SwerveDrive -> odometryGoToPose(3.5, -0.5, 0.0)){
+            if(gettime_d() > state_time + WAIT_TO_SHOOT) {
+                state_time = gettime_d();
                 a_NoteHandler->indexToShoot();
-                nextState = kGoTo3rdNote1;
+                nextState = kAutoIdle1;
             }
             break;
-        case kGoTo3rdNote1:
-            a_NoteHandler->collectNote(-0.4, false);
-            if(a_SwerveDrive -> odometryGoToPose(8.277, -.236, 0.0)){
-                nextState = kShoot3rdNote1;
+        case kGoTo2ndNote1:
+            if(gettime_d() > state_time + WAIT_TO_SHOOT) {
+                state_time = gettime_d();
+                a_NoteHandler->collectNote(-0.4, false);
+                if(a_SwerveDrive -> odometryGoToPose(2.902, -1.455, 0.0)){
+                    nextState = kAutoIdle1;
+                }
             }
             break;
-        case kShoot3rdNote1:
-             if(a_SwerveDrive -> odometryGoToPose(3.5, -.5, 0.0)){
-                a_NoteHandler->indexToShoot();
-                nextState = kAutoIdle1;
-            }
+        // case kShoot2ndNote1:
+        //     if(a_SwerveDrive -> odometryGoToPose(3.5, -0.5, 0.0)){
+        //         a_NoteHandler->indexToShoot();
+        //         nextState = kGoTo3rdNote1;
+        //     }
+        //     break;
+        // case kGoTo3rdNote1:
+        //     a_NoteHandler->collectNote(-0.4, false);
+        //     if(a_SwerveDrive -> odometryGoToPose(8.277, -.236, 0.0)){
+        //         nextState = kShoot3rdNote1;
+        //     }
+        //     break;
+        // case kShoot3rdNote1:
+        //      if(a_SwerveDrive -> odometryGoToPose(3.5, -.5, 0.0)){
+        //         a_NoteHandler->indexToShoot();
+        //         nextState = kAutoIdle1;
+        //     }
        }
     a_AutoState1 = nextState;
 }
 
-
+//BLUE CENTER RED CENTER
 void Autonomous::NoteTwo() {
+    a_SwerveDrive->zeroPose(frc::Pose2d(units::meter_t(1.3), units::meter_t(0.0), units::degree_t(0.0)));
+    a_Gyro->Zero(0.0);
     a_AutoState2 = kGoToNote2;   
     state_time = gettime_d();
 }
@@ -162,25 +171,36 @@ void Autonomous::PeriodicNoteTwo() {
     AutoState2 nextState = a_AutoState2;
 
     switch (a_AutoState2) {
-       case kAutoIdle2:
+        case kAutoIdle2:
             StopSwerves();
             break;
+        case kShootNote2:
+            if(gettime_d() > state_time + WAIT_TO_SHOOT) {
+                state_time = gettime_d();
+                a_NoteHandler->indexToShoot();
+                nextState = kGoToNote2;
+            }
+            break;
          case kGoToNote2:
-            a_SwerveDrive -> odometryGoToPose(2.9, -1.45, 0.0);
-            nextState = kShootNote2;
+            if(gettime_d() > state_time + WAIT_TO_SHOOT) {
+                state_time = gettime_d();
+                a_NoteHandler->collectNote(-0.4, false);
+                if(a_SwerveDrive -> odometryGoToPose(2.9, 0.0, 0.0)){
+                    nextState = kAutoIdle2;
+                }
+            }
             break;
-        case kShootNote2:
-            a_NoteHandler->indexToShoot();
-            nextState = kAutoIdle2;
-            break;      
+              
     }
     a_AutoState2 = nextState;
 }
 
-
+//BLUE RIGHT RED RIGHT
 void Autonomous::NoteThree() 
 {
-    a_AutoState3 =   kGoToNote3;
+    a_SwerveDrive->zeroPose(frc::Pose2d(units::meter_t(.47), units::meter_t(0.79), units::degree_t(0.0)));
+    a_Gyro->Zero(300.0);
+    a_AutoState3 =  kGoToNote3;
     state_time = gettime_d();
 }
 
@@ -191,19 +211,30 @@ void Autonomous::PeriodicNoteThree() {
          case kAutoIdle3:
             StopSwerves();
             break;
-        case kGoToNote3:
-            a_SwerveDrive -> odometryGoToPose(2.9, 1.45, 0.0);
-            nextState = kShootNote3;
-            break;
         case kShootNote3:
-            a_NoteHandler->indexToShoot();
-            nextState = kAutoIdle3;
+            if(gettime_d() > state_time + WAIT_TO_SHOOT) {
+                state_time = gettime_d();
+                a_NoteHandler->indexToShoot();
+                nextState = kAutoIdle3;
+            }
             break;
+        case kGoToNote3:
+            if(gettime_d() > state_time + WAIT_TO_SHOOT) {
+                state_time = gettime_d();
+                a_NoteHandler->collectNote(-0.4, false);
+                if(a_SwerveDrive -> odometryGoToPose(2.9, 1.45, 0.0)){
+                    nextState = kShootNote3;
+                }
+            }
+            break;
+        
     }
     a_AutoState3 = nextState;
 }
-
+//RED LEFT
 void Autonomous::NoteFour(){
+    a_SwerveDrive->zeroPose(frc::Pose2d(units::meter_t(.47), units::meter_t(0.79), units::degree_t(0.0)));
+    a_Gyro->Zero(300.0);
     a_AutoState4 = kGoToNote4;
     state_time = gettime_d();
 }
diff --git a/src/main/cpp/Climber.cpp b/src/main/cpp/Climber.cpp
index da73c75..1d13810 100644
--- a/src/main/cpp/Climber.cpp
+++ b/src/main/cpp/Climber.cpp
@@ -1,10 +1,10 @@
 #include "Climber.h"
 #include "Prefs.h"
 
-Climber::Climber(int climberMotorID)://, int topLimitSwitchPort)://, int topPort, int bottomPort):
+Climber::Climber(int climberMotorID, int topLimitSwitchPort)://, int topLimitSwitchPort)://, int topPort, int bottomPort):
 climberMotor(climberMotorID),
-//topLimitSwitch(topLimitSwitchPort),
-climberPID(0.0, 0.0, 0.0)
+topLimitSwitch(topLimitSwitchPort),
+climberPID(0.04, 0.0, 0.0)
 //m_climberMotorSignal(climberMotor.GetPosition())
 {
     //m_climberMotorSignal.SetUpdateFrequency(units::frequency::hertz_t(10.0));
@@ -15,15 +15,26 @@ void Climber::stopClimber(){
 }
 void Climber::extendClimnber(){
     
-    climberPID.SetSetpoint(0.0);//neeed to change from 0.0
+    climberPID.SetSetpoint(10.0);//neeed to change from 0.0
     double dist = GetClimberPosition();
-    double speed = climberPID.Calculate(dist, 0.0);
+    double speed = climberPID.Calculate(dist, 10.0);
     speed = std::clamp(speed, -.2, .2);
-    climberMotor.Set(speed);
+    climberMotor.Set(-speed);
     if(climberPID.AtSetpoint() ){
         climberMotor.StopMotor();
     }
 }
+void Climber::runClimberUp(){
+    if(topLimitSwitch.limitSwitchPressed()){
+        climberMotor.StopMotor();
+    }
+    else{
+    climberMotor.Set(.05);
+    }
+}
+void Climber::runClimberDown(){
+    climberMotor.Set(-.05);
+}
 
 void Climber::retractClimber(){
 //    if(climberPID.AtSetpoint() || topLimitSwitch.limitSwitchPressed()){
@@ -39,9 +50,13 @@ void Climber::retractClimber(){
     
 }
 double Climber::GetClimberPosition(){
-    return climberMotor.GetPosition().GetValue().value();
+    // HOOK moved 6 in and the change in turns was below
+    double conversion = 6.0/(-69.629395- -31.272461);
+    return (climberMotor.GetPosition().GetValue().value())*conversion;
 }
 void Climber::setPosition(){
-    climberMotor.SetPosition(units::angle::turn_t{0.0});
+    if(topLimitSwitch.limitSwitchPressed()){
+        climberMotor.SetPosition(units::angle::turn_t{0.0});
+    }
 }
 
diff --git a/src/main/cpp/Collector.cpp b/src/main/cpp/Collector.cpp
index 6fa0bf6..e01122b 100644
--- a/src/main/cpp/Collector.cpp
+++ b/src/main/cpp/Collector.cpp
@@ -27,6 +27,7 @@ void Collector::indexToShoot() {
     indexerMotor.Set(-0.40);
 }
 void Collector::indexToAmp() {
+    std::cout << "Shooter Angle True?" << std::endl;
     indexerMotor.Set(-.2);
 }
 void Collector::stopIndexer() {
diff --git a/src/main/cpp/LED.cpp b/src/main/cpp/LED.cpp
index 73c6b95..8954727 100644
--- a/src/main/cpp/LED.cpp
+++ b/src/main/cpp/LED.cpp
@@ -28,7 +28,7 @@ void LED::Init()
 	for(i = 0; i < BUFF_SIZE; i++) {
 		rx_buff[i] = 0;
 	}
-	SetTargetType(LED_STAGE_enum::WHITE);
+	//SetTargetType(LED_STAGE_enum::WHITE);
 }
 
 void LED::Update()
@@ -83,22 +83,75 @@ void LED::ProcessReport()
 {
 	// parse report
 	// no action needed, no report expected
+	
+
 }
 
-void LED::SetTargetType(LED_STAGE_enum target_type_param)
-{
-#ifdef COMP_BOT  // Not available on the practice bot
+// void LED::SetTargetType(LED_STAGE_enum target_type_param)
+// {
+// #ifdef COMP_BOT  // Not available on the practice bot
+// 	char cmd[10];
+// 	strncpy(cmd, "1,1,1\r\n", 8);
+// 	target_type = target_type_param;
+// 	// lazy way to build a message
+// 	cmd[4] = target_type ? '1' : '0';
+// 	m_serial.Write(cmd, strlen(cmd));
+// 	m_serial.Flush();
+// #endif
+// }
+
+// LED_STAGE_enum LED::GetTargetType()
+// {
+// 	return target_type;
+// }
+
+void LED::SetWhite() {
 	char cmd[10];
-	strncpy(cmd, "1,1,1\r\n", 8);
-	target_type = target_type_param;
-	// lazy way to build a message
-	cmd[4] = target_type ? '1' : '0';
+	strncpy(cmd, "0,0\r\n", 8);
 	m_serial.Write(cmd, strlen(cmd));
 	m_serial.Flush();
-#endif
+	
 }
 
-LED_STAGE_enum LED::GetTargetType()
-{
-	return target_type;
+void LED::SetMSGIdle() {
+	char cmd[10];
+	strncpy(cmd, "1,0\r\n", 8);
+	m_serial.Write(cmd, strlen(cmd));
+	m_serial.Flush();
+
 }
+
+void LED::SetNoComms() {
+	char cmd[10];
+	strncpy(cmd, "2,0\r\n", 8);
+	m_serial.Write(cmd, strlen(cmd));
+	m_serial.Flush();
+
+}
+
+void LED::SetNoteOnBoard() {
+	char cmd[10];
+	strncpy(cmd, "3,0\r\n", 8);
+	m_serial.Write(cmd, strlen(cmd));
+	m_serial.Flush();
+
+
+}
+
+void LED::SetAngleToNote(float angle) {
+	char cmd[10];
+	sprintf(cmd, "4,1,%1.2f\r\n", angle);
+	printf("%s\n", cmd);
+	m_serial.Write(cmd, strlen(cmd));
+	m_serial.Flush();
+
+}
+
+void LED::SetShooterReady() {
+	char cmd[10];
+	strncpy(cmd, "5,0\r\n", 8);
+	m_serial.Write(cmd, strlen(cmd));
+	m_serial.Flush();
+
+}
+
diff --git a/src/main/cpp/LED_DIO.cpp b/src/main/cpp/LED_DIO.cpp
deleted file mode 100644
index 51c7605..0000000
--- a/src/main/cpp/LED_DIO.cpp
+++ /dev/null
@@ -1,43 +0,0 @@
-/* LED_DIO.cpp - Control LED lights representing desired game piece
-
- */
-
-#include <stdio.h> // printf
-#include <stdlib.h> // atoi
-#include <Prefs.h>
-#include "LED_DIO.h"
-
-LED_DIO::LED_DIO(int DIO_pin):
-	a_Output(DIO_pin)
-{
-	Init();
-}
-
-void LED_DIO::Init()
-{
-	SetTargetType(target_type_enum::CONE);
-}
-
-void LED_DIO::Update()
-{
-	// refresh the output pin
-	// this should not be needed
-	SetTargetType(target_type);
-}
-
-void LED_DIO::SetTargetType(target_type_enum target_type_param)
-{
-	target_type = target_type_param;
-	if(target_type == target_type_enum::CONE) {
-		// set the output HIGH for a CONE
-		a_Output.Set(true);
-	} else {
-		// set the output LOW for a CUBE
-		a_Output.Set(false);
-	}
-}
-
-target_type_enum LED_DIO::GetTargetType()
-{
-	return target_type;
-}
diff --git a/src/main/cpp/NoteHandler.cpp b/src/main/cpp/NoteHandler.cpp
index 5c3d34e..9717aee 100644
--- a/src/main/cpp/NoteHandler.cpp
+++ b/src/main/cpp/NoteHandler.cpp
@@ -5,8 +5,8 @@
 // Maybe no : after ()
 NoteHandler::NoteHandler(): 
 a_Collector(COLLECTOR_MOTOR_ID, INDEXER_MOTOR_ID),
-a_Shooter(SHOOTER_LEFT_MOTOR_ID, SHOOTER_RIGHT_MOTOR_ID, PIVOT_MOTOR_ID),
-a_Climber(CLIMBER_MOTOR_ID),
+a_Shooter(SHOOTER_LEFT_MOTOR_ID, SHOOTER_RIGHT_MOTOR_ID, PIVOT_MOTOR_ID, SHOOTER_LIMIT_SWITCH_PORT),
+a_Climber(CLIMBER_MOTOR_ID, TOP_LIMIT_SWITCH_PORT),
 a_AmpTrap(ROLLER_ID, ARM_PIVOT_MOTOR_ID, EXTENSION_ID),
 currentAmpLoadState(IDLE)
 {
@@ -114,24 +114,34 @@ bool NoteHandler::armToPose(double angle){
 void NoteHandler::setRotPID(double p, double i, double d){
     a_AmpTrap.setPID(p, i, d);
 }
-void NoteHandler::shootToAmp(bool buttonState){
+void NoteHandler::shootToAmp(bool transferButtonState, bool intoAmpButtonState, bool toDefaultPositionButtonState) {
     switch(currentAmpLoadState){
         case IDLE:
             released = false;
-            if(buttonState == true){
+            // if (a_Gamepad.GetRawButton(3)) {
+            //     double rpm = 3500;
+            //     double angle = 29.0;
+            //     a_NoteHandler.startShooter(rpm, angle);
+            // } 
+            // else {
+            //     a_NoteHandler.stopShooter();
+            //     a_NoteHandler.moveShooterToAngle(0.0);
+            // }
+            a_AmpTrap.moveToPosition(15.0);
+            if(transferButtonState == true){
                 currentAmpLoadState = LOADING;
             }
             break;
 
         case LOADING:
-            if(!buttonState){
+            if(!transferButtonState){
                 currentAmpLoadState = IDLE;
             }
             a_AmpTrap.runRoller();
             a_Shooter.setSpeed(600);
-            if(a_Shooter.moveToAngle(50.0) && a_AmpTrap.moveToPosition(243.0)){
-
+            if(a_Shooter.moveToAngle(50.0) && a_AmpTrap.moveToPosition(237.0)){
                 feedToAmp(-.2);
+
                 if(a_AmpTrap.beamBroken()){
                     shootToAmpMode = true;
                 }
@@ -140,14 +150,34 @@ void NoteHandler::shootToAmp(bool buttonState){
                         a_AmpTrap.stopRoller();
                         stopAll();
                         shootToAmpMode = false;
-                        currentAmpLoadState = DONE;
+                        currentAmpLoadState = HOLDING;
                     }
                 }
             }
-            break;
-
+                break;
+            case HOLDING:
+                if(intoAmpButtonState){
+                    if(armToPose(154.0)){
+                        runArmRoller();
+                        currentAmpLoadState = DONE;
+                    }
+                }
+                if(toDefaultPositionButtonState){
+                    if(armToPose(10.0)){
+                        currentAmpLoadState = TOAMP;
+                    }
+                }
+                break;
+            case TOAMP:
+                if(intoAmpButtonState){
+                    if(armToPose(154.0)){
+                        runArmRoller();
+                        currentAmpLoadState = DONE;
+                    }
+                }
+                break;
             case DONE:
-                if(!buttonState){
+                if(!transferButtonState){
                     currentAmpLoadState = IDLE;
                 }
                 break;
@@ -159,3 +189,21 @@ void NoteHandler::runArmRoller(){
 double NoteHandler::getClimberPosition(){
     return a_Climber.GetClimberPosition();
 }
+void NoteHandler::manualClimberUp(){
+    a_Climber.runClimberUp();
+}
+void NoteHandler::manualClimberDown(){
+    a_Climber.runClimberDown();
+}
+void NoteHandler::setClimberPosition(){
+    a_Climber.setPosition();
+}
+void NoteHandler::stopClimber(){
+    a_Climber.stopClimber();
+}
+void NoteHandler::pidClimb(){
+    a_Climber.extendClimnber();
+}
+void NoteHandler::moveShooterToAngle(double angle){
+    a_Shooter.moveToAngle(0.0);
+}
\ No newline at end of file
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index b1d2e42..48e7ff9 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -57,6 +57,8 @@ a_Autonomous(&a_Gyro, &a_SwerveDrive, &a_NoteHandler)
     a_BRModule.setDrivePID(pvaluedrive, 0, 0);
     a_BRModule.setSteerPID(pvaluesteer, ivaluesteer, dvaluesteer);
 
+    a_NoteHandler.setRotPID(rotP, rotI, rotD);
+
     a_SwerveDrive.brakeOnStop();
 }
 
@@ -72,8 +74,7 @@ void Robot::RobotInit() {
     a_Gyro.Init();
     a_Gyro.Zero();
 
-    a_NoteHandler.setShooterAngleToDefault();
-
+    
     m_AutoModeSelector.SetDefaultOption(RobotDoNothing, RobotDoNothing);
     m_AutoModeSelector.AddOption(RobotDoNothing, RobotDoNothing);
     m_AutoModeSelector.AddOption(firstNote, firstNote);
@@ -87,8 +88,9 @@ void Robot::RobotInit() {
     frc::SmartDashboard::PutData("Auto Modes", &m_AutoModeSelector);
 
     a_LED.Init();
+    a_LED.SetAngleToNote(0.3);
 
-    a_LED.SetTargetType(LED_STAGE_enum::WHITE);
+    //a_LED.SetTargetType(LED_STAGE_enum::WHITE);
     //InterpolationValues value = {22.5, 3500};
     a_NoteHandler.insertToInterpolatingMap(2.546859, {22.5, 4000});
     a_NoteHandler.insertToInterpolatingMap(4.212965, {9.5, 4000});
@@ -96,6 +98,15 @@ void Robot::RobotInit() {
 }
 
 void Robot::RobotPeriodic() {
+    a_NoteHandler.setShooterAngleToDefault();
+    a_NoteHandler.setClimberPosition();
+    
+    if(a_NoteHandler.beamBroken()){
+        a_LED.SetNoteOnBoard();
+    } else {
+        a_LED.SetMSGIdle();
+    }
+
     a_NoteHandler.updateDashboard();
 
 
@@ -104,7 +115,7 @@ void Robot::RobotPeriodic() {
 
     // if (result.HasTargets()) {
     //     frc::SmartDashboard::PutString("Has_AprilTags", "YES");
-    // } else {
+    // } else { 
     //     frc::SmartDashboard::PutString("Has_AprilTags", "NO");
     // }
 
@@ -186,7 +197,7 @@ void Robot::AutonomousInit() {
     }
 
     a_SwerveDrive.unsetHoldAngle();
-    a_Gyro.Zero(0.0);
+    
     std::string SelectedRoute = m_AutoModeSelector.GetSelected(); //assigns value frm smart dashboard to a string variable
 
     a_Autonomous.StartAuto(SelectedRoute); //starts auto from selected route
@@ -219,12 +230,7 @@ void Robot::TeleopInit() {
 
 // main loop
 void Robot::TeleopPeriodic() {
-    if(a_NoteHandler.beamBroken()){
-        a_LED.SetTargetType(LED_STAGE_enum::NOTE_COLLECTED);
-    }
-    else{
-        a_LED.SetTargetType(LED_STAGE_enum::LED_IDLE);
-    }
+    
     //a_Shooter.moveToAngle(20.0);
     // frc::SmartDashboard::PutNumber("desired angle", pivotAngle);
     // a_Shooter.moveToAngle(pivotAngle);
@@ -244,7 +250,7 @@ void Robot::TeleopPeriodic() {
 
     /* =-=-=-=-=-=-=-=-=-=-= Shooter Controls =-=-=-=-=-=-=-=-=-=-= */
     // getting shooter up to speeed
-    if (a_Gamepad.GetRawButton(SHOOTER_BUTTON)) {
+    if (a_Gamepad.GetRawButton(3)) {
         // if (result.HasTargets()) {
         //     std::span<const photon::PhotonTrackedTarget> targets = result.GetTargets();
         //     for (photon::PhotonTrackedTarget target : targets) {
@@ -262,13 +268,20 @@ void Robot::TeleopPeriodic() {
         double rpm = 3500;
         double angle = 35.0;
         a_NoteHandler.startShooter(rpm, angle);
-    } else {
+    } 
+    else {
         a_NoteHandler.stopShooter();
+        a_NoteHandler.moveShooterToAngle(0.0);
     }
     /* =-=-=-=-=-=-=-=-=-=-= Collector/Indexer Controls =-=-=-=-=-=-=-=-=-=-= */
 
     // start collector
-    if (a_Gamepad.GetRawButton(COLLECTOR_BUTTON)) {
+    a_NoteHandler.shootToAmp(a_DriverXboxController.GetRightTriggerAxis() > .75, a_DriverXboxController.GetAButton(), a_DriverXboxController.GetLeftBumper());
+    if(a_DriverXboxController.GetAButton()) {
+        // if(a_NoteHandler.armToPose(154.0)){
+        //     a_NoteHandler.runArmRoller();
+        // }
+    } else if (a_Gamepad.GetRawButton(1)) {
         a_NoteHandler.collectNote(-0.4, true);
     } else if (a_DriverXboxController.GetRightBumper()) {
         // give note to shooter
@@ -280,13 +293,20 @@ void Robot::TeleopPeriodic() {
         a_NoteHandler.stopCollection();
     }
 
-    /* Amp Control*/
-    a_NoteHandler.shootToAmp(a_DriverXboxController.GetRightTriggerAxis() > .75);
-    if(a_DriverXboxController.GetAButton()){
-        if(a_NoteHandler.armToPose(154.0)){
-            a_NoteHandler.runArmRoller();
-        }
+    if(a_DriverXboxController.GetBButton()){
+        a_NoteHandler.manualClimberDown();
+    }
+    else if(a_DriverXboxController.GetXButton()){
+        a_NoteHandler.manualClimberUp();
+    }
+    else if(a_DriverXboxController.GetYButton()){
+        a_NoteHandler.pidClimb();
     }
+    else{
+        a_NoteHandler.stopClimber();
+    }
+    /* Amp Control*/
+    
     /* =-=-=-=-=-=-=-=-=-=-= Swerve Controls =-=-=-=-=-=-=-=-=-=-= */
 
     if (a_DriverXboxController.GetLeftTriggerAxis() > .5) {
@@ -427,7 +447,7 @@ void Robot::TestPeriodic() {
     // else if(a_DriverXboxController.GetYButtonPressed()){
     //     rotD-=.0001;
     // }
-    a_NoteHandler.setRotPID(rotP, rotI, rotD);
+    
     
     // a_FLModule.setSteerPID(pvaluesteer, ivaluesteer, dvaluesteer);
 
@@ -456,17 +476,17 @@ void Robot::TestPeriodic() {
     //     a_BLModule.steerToAng(45);
     // }
     
-    a_NoteHandler.shootToAmp(a_DriverXboxController.GetRightTriggerAxis() > .75);
+    // a_NoteHandler.shootToAmp(a_DriverXboxController.GetRightTriggerAxis() > .75);
      
-    if(a_DriverXboxController.GetAButton()){
-        if(a_NoteHandler.armToPose(154.0)){
-            //frc::SmartDashboard::PutString("through if?", "YES");
-            a_NoteHandler.runArmRoller();
-        }
-        else{
-           // frc::SmartDashboard::PutString("through if?", "NO");
-        }
-    }
+    // if(a_DriverXboxController.GetAButton()){
+    //     if(a_NoteHandler.armToPose(154.0)){
+    //         //frc::SmartDashboard::PutString("through if?", "YES");
+    //         a_NoteHandler.runArmRoller();
+    //     }
+    //     else{
+    //        // frc::SmartDashboard::PutString("through if?", "NO");
+    //     }
+    // }
     
 }
 
diff --git a/src/main/cpp/Shooter.cpp b/src/main/cpp/Shooter.cpp
index 301f99d..085c5a7 100644
--- a/src/main/cpp/Shooter.cpp
+++ b/src/main/cpp/Shooter.cpp
@@ -5,15 +5,16 @@
 #include <ctre/phoenix6/configs/Configs.hpp>
 #include <ctre/phoenix6/controls/VelocityVoltage.hpp>
 
-Shooter::Shooter(int rightShooterMotorID, int leftShooterMotorID, int pivotMotorID):
+Shooter::Shooter(int rightShooterMotorID, int leftShooterMotorID, int pivotMotorID, int limitSwitchID):
 
 rightShooterMotor(rightShooterMotorID),
 leftShooterMotor(leftShooterMotorID),
 pivotMotor(pivotMotorID),
 // pivotEncoder(pivotMotor),
-//shooterLimitSwitch(limitSwitchID),
-// .0067, .006, 0.0
-pivotPID(0.005, 0.0045, 0.0005),
+shooterLimitSwitch(limitSwitchID),
+// .005, .0045, 0.00025
+//0.003, 0.0015, 0.00012
+pivotPID(0.005, 0.000, 0.000),
 leftShooterPID(0.0, 0.0, 0.0),
 rightShooterPID(0.0, 0.0, 0.0)
 {
@@ -47,8 +48,8 @@ rightShooterPID(0.0, 0.0, 0.0)
 void Shooter::setSpeed(double rpm){
   
    
-    rightShooterMotor.SetControl(m_request.WithVelocity(units::angular_velocity::turns_per_second_t{3600/60.0} ));
-    leftShooterMotor.SetControl(m_request.WithVelocity(units::angular_velocity::turns_per_second_t{3600/60.0} ));
+    rightShooterMotor.SetControl(m_request.WithVelocity(units::angular_velocity::turns_per_second_t{-rpm/60.0} ));
+    leftShooterMotor.SetControl(m_request.WithVelocity(units::angular_velocity::turns_per_second_t{-rpm/60.0} ));
     
     // double velo = rpm/6000;
     // rightShooterMotor.Set(-velo);
@@ -59,8 +60,10 @@ double Shooter::getSpeed(){
     //return; //(units * 600.0) / FALCON_UNITS_PER_REV;; 
     return units;
 }
-void Shooter::setShooterAngle(){
-    pivotMotor.SetPosition(units::angle::turn_t{units::degree_t{20.0}});
+void Shooter::setShooterAngle(){ 
+    if(shooterLimitSwitch.limitSwitchPressed()){
+        pivotMotor.SetPosition(units::angle::turn_t{units::degree_t{0.0}});
+    }
 }
 void Shooter::stopShooter(){
    rightShooterMotor.StopMotor();
@@ -79,6 +82,7 @@ bool Shooter::moveToAngle(double angle){
     else{
        return false;
     }
+
 }
 double Shooter::GetShooterAngle(){
     return (((-8.876)*pivotMotor.GetPosition().GetValue().value()));
diff --git a/src/main/include/Climber.h b/src/main/include/Climber.h
index c98a45f..c84cb33 100644
--- a/src/main/include/Climber.h
+++ b/src/main/include/Climber.h
@@ -5,12 +5,14 @@
 
 class Climber {
     public:
-        Climber(int climberMotorID);//, int topLimitSwitchPort);//,int topPort, int bottomPort);          
+        Climber(int climberMotorID, int topLimitSwitchPort);//, int topLimitSwitchPort);//,int topPort, int bottomPort);          
         void stopClimber();
         void extendClimnber();
         void retractClimber();
         double GetClimberPosition();
         void setPosition();
+        void runClimberUp();
+        void runClimberDown();
 
     private:
         
@@ -19,6 +21,6 @@ class Climber {
         //ctre::phoenix6::StatusSignal<units::turn_t> m_climberMotorSignal;
         
         frc::PIDController climberPID;
-        //LimitSwitch topLimitSwitch; 
+        LimitSwitch topLimitSwitch; 
         
 };
\ No newline at end of file
diff --git a/src/main/include/LED.h b/src/main/include/LED.h
index bc2aa17..25ae85d 100644
--- a/src/main/include/LED.h
+++ b/src/main/include/LED.h
@@ -5,11 +5,18 @@
 #ifndef H_LED
 #define H_LED
 
-#include "TOF_protocol.h" //yay i can use CONE and CUBE
+#include "Protocol.h" //yay i can use CONE and CUBE
 #include <frc/SerialPort.h>
 
 #define BUFF_SIZE 256
 
+// enum LED_STAGE_enum {
+//   WHITE = 0,
+//   LED_IDLE,
+//   NO_COMMS,
+//   NOTE_COLLECTED  
+// };
+
 class LED
 {
 public:
@@ -20,18 +27,24 @@ class LED
 	void Init();
 	void Update();
 
+	void SetWhite();
+	void SetMSGIdle();
+	void SetNoComms();
+	void SetNoteOnBoard();
+	void SetAngleToNote(float angle);
+	void SetShooterReady();
+
 	void ProcessReport();
-	enum LED_STAGE_enum GetTargetRangeIndicator();
-	void SetTargetType(LED_STAGE_enum target_type_param);
-	LED_STAGE_enum GetTargetType();
+	//enum LED_STAGE_enum GetTargetRangeIndicator();
+	//void SetTargetType(LED_STAGE_enum target_type_param);
+	//LED_STAGE_enum GetTargetType();
 
 private:
-#ifdef COMP_BOT  // Not available on the practice bot
+	
 	frc::SerialPort m_serial;
-#endif
 	char rx_buff[BUFF_SIZE];
 	int rx_index = 0;
-	LED_STAGE_enum target_type = LED_STAGE_enum::WHITE;
+	//LED_STAGE_enum target_type = LED_STAGE_enum::WHITE;
 } ;
 
 #endif
diff --git a/src/main/include/LED_DIO.h b/src/main/include/LED_DIO.h
deleted file mode 100644
index d37db69..0000000
--- a/src/main/include/LED_DIO.h
+++ /dev/null
@@ -1,31 +0,0 @@
-/* LED_DIO.cpp - Control LED lights representing desired game piece
-
- */
-
-#ifndef H_LED_DIO
-#define H_LED_DIO
-
-#include <frc/DigitalOutput.h>
-#include "TOF_protocol.h" //yay i can use CONE and CUBE
-
-#define BUFF_SIZE 256
-
-class LED_DIO
-{
-public:
-
-	LED_DIO(int DIO_pin);
-	virtual ~LED_DIO() = default;
-
-	void Init();
-	void Update();
-
-	void SetTargetType(target_type_enum target_type_param);
-	target_type_enum GetTargetType();
-
-private:
-	frc::DigitalOutput a_Output;
-	target_type_enum target_type = target_type_enum::CONE;
-} ;
-
-#endif
diff --git a/src/main/include/NoteHandler.h b/src/main/include/NoteHandler.h
index f57ebfe..4a2d87d 100644
--- a/src/main/include/NoteHandler.h
+++ b/src/main/include/NoteHandler.h
@@ -10,6 +10,8 @@
 enum AmpLoadState { // Encoders
             IDLE = 0,
             LOADING,
+            HOLDING,
+            TOAMP,
             DONE,
         };
 
@@ -55,13 +57,20 @@ class NoteHandler {
 
         void setRotPID(double p, double i, double d);
 
-        void shootToAmp(bool buttonState);
+        void shootToAmp(bool transferButtonState, bool intoAmpButtonState, bool toDefaultPositionButtonState);
 
         void feedToAmp(double speed);
 
         void runArmRoller();
 
+        void moveShooterToAngle(double angle);
+
         double getClimberPosition();
+        void manualClimberUp();
+        void manualClimberDown();
+        void setClimberPosition();
+        void stopClimber();
+        void pidClimb();
 
         bool noteShot = false;
     private:
diff --git a/src/main/include/Prefs.h b/src/main/include/Prefs.h
index 7e3c469..9326779 100644
--- a/src/main/include/Prefs.h
+++ b/src/main/include/Prefs.h
@@ -19,7 +19,7 @@
 #define INVERSE_COLLECTOR_BUTTON 4
 #define SHOOTER_BUTTON 6
 //#define DEFAULT_SHOOTER_ANGLE 36.0
-
+#define WAIT_TO_SHOOT 3.0
 // For the competition bot, this line *MUST* be enabled. For the practice bot, comment out this line.
 #define COMP_BOT
 
@@ -33,6 +33,8 @@
 
 #define AMP_BEAM_BREAK_PORT 5
 #define COLLECTOR_BEAMBREAK_PORT 2
+#define TOP_LIMIT_SWITCH_PORT 0
+#define SHOOTER_LIMIT_SWITCH_PORT 1
 
 #define LIMIT_SWITCH 0 // change later
 
diff --git a/src/main/include/Protocol.h b/src/main/include/Protocol.h
new file mode 100644
index 0000000..dca47b3
--- /dev/null
+++ b/src/main/include/Protocol.h
@@ -0,0 +1,12 @@
+// TOF protocol version 1.0
+#pragma once
+
+enum RIO_msgs_enum {
+  WHITE = 0,
+  MSG_IDLE = 1,
+  NO_COMMS = 2,
+  NOTE_ON_BOARD = 3,
+  ANGLE_TO_NOTE = 4,
+  SHOOTER_READY = 5,
+};
+
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index 90f82ec..eb9914a 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -14,7 +14,6 @@
 #include "photon/PhotonCamera.h"
 #include "photon/PhotonUtils.h"
 #include <frc/smartdashboard/SendableChooser.h>
-#include "LED_DIO.h"
 #include <frc/controller/PIDController.h>
 #include <frc/GenericHID.h>
 #include "NoteHandler.h"
@@ -136,13 +135,11 @@ class Robot : public frc::TimedRobot {
         // Shooter camera
         photon::PhotonCamera a_camera{SHOOTER_CAMERA_NAME};
 
-        enum target_type_enum target_type = target_type_enum::CONE;
-
         frc::AprilTagFieldLayout aprilTagFieldLayout = frc::LoadAprilTagLayoutField(frc::AprilTagField::k2024Crescendo);
         
         // .0025, .001, .0001
         // .0002, .002, .0025
-         double rotP = 0.00275;
+        double rotP = 0.00275;
         double rotI = 0.001;
         double rotD = 0.0001;
 };
diff --git a/src/main/include/Shooter.h b/src/main/include/Shooter.h
index 9547807..e7e2edf 100644
--- a/src/main/include/Shooter.h
+++ b/src/main/include/Shooter.h
@@ -7,7 +7,7 @@
 
 class Shooter{
     public:
-        Shooter(int rightShooterMotorID, int leftShooterMotorID, int pivotMotorID);
+        Shooter(int rightShooterMotorID, int leftShooterMotorID, int pivotMotorID, int limitSwitchID);
         void setSpeed(double percent);
         void stopShooter();
         bool moveToAngle(double angle);
@@ -27,5 +27,5 @@ class Shooter{
         ctre::phoenix6::controls::VelocityVoltage m_request = ctre::phoenix6::controls::VelocityVoltage{0_tps}.WithSlot(0);
     
       
-        //LimitSwitch shooterLimitSwitch;
+        LimitSwitch shooterLimitSwitch;
 };
\ No newline at end of file
diff --git a/src/main/include/SwerveDrive.h b/src/main/include/SwerveDrive.h
index 6bb8918..d8ee51a 100644
--- a/src/main/include/SwerveDrive.h
+++ b/src/main/include/SwerveDrive.h
@@ -143,7 +143,7 @@ class SwerveDrive // Class to handle the kinematics of Swerve Drive
         frc::ProfiledPIDController<units::meters> xProfiledPid;
         frc::ProfiledPIDController<units::meters> yProfiledPid;
 
-        frc::TrapezoidProfile<units::radian>::Constraints rotationalConstraints{units::radians_per_second_t(4.0), units::radians_per_second_squared_t(4.0)};
+        frc::TrapezoidProfile<units::radian>::Constraints rotationalConstraints{units::radians_per_second_t(2.0), units::radians_per_second_squared_t(2.0)};
         frc::ProfiledPIDController<units::radian> rotProfiledPid;
 
 };
diff --git a/src/main/include/TOF_protocol.h b/src/main/include/TOF_protocol.h
deleted file mode 100644
index 9d8c898..0000000
--- a/src/main/include/TOF_protocol.h
+++ /dev/null
@@ -1,36 +0,0 @@
-// TOF protocol version 1.0
-#pragma once
-
-enum TOF_RIO_msgs_enum {
-  RESERVED = 0,
-  RANGE = 1,
-  HISTOGRAM = 2,
-  ARM_ANGLE = 3,
-  RAW_PIXEL_DATA = 4,
-};
-
-enum target_range_enum {
-  TARGET_NOT_PRESENT = 0,
-  TARGET_IN_RANGE = 1,
-  TARGET_TOO_FAR = 2,
-  TARGET_TOO_CLOSE = 3,
-};
-
-enum RIO_TOF_msgs_enum {
-  RESERVED_RIO = 0,
-  TARGET_TYPE = 1,
-  HISTOGRAM_ENABLE = 2,
-  ARM_ANGLE_OFFSET = 3,
-  RAW_PIXEL_DATA_ENABLE = 4,
-};
-
-enum target_type_enum {
-  CONE = 0,
-  CUBE = 1,
-} ;
-enum LED_STAGE_enum {
-  WHITE = 0,
-  LED_IDLE,
-  NO_COMMS,
-  NOTE_COLLECTED  
-};