diff --git a/robot/src/main/cpp/ColorWheel.cpp b/robot/src/main/cpp/ColorWheel.cpp
index 81eabb3..6f56f02 100644
--- a/robot/src/main/cpp/ColorWheel.cpp
+++ b/robot/src/main/cpp/ColorWheel.cpp
@@ -1,6 +1,10 @@
+#include <frc/smartdashboard/SmartDashboard.h>
 #include <frc/Joystick.h>
 #include <ColorWheel.h>
-
+#include <iostream>
+#include <string>
+#include <RobotUtilities.h>
+using namespace std;
 
 enum WheelState{
     NotSpinning,
@@ -9,57 +13,240 @@ enum WheelState{
 };
 WheelState spinState = WheelState::NotSpinning;
 
-frc::Color ColorState;
+frc::Color CurrentColor;
+
+frc::Color PreviousColor;
 
 int NumSpins = 0;
 
-ColorWheel::ColorWheel(){
-  //Add color match code here
+bool OnRed = false;
+
+double ColorConfidenceTarget = 0.9;
+
+int NumColorSamples = 0;
+
+int CurrentButton = 0;
+
+// Joystick2Layout deployColorWheelButton = Joystick2Layout::kDeployColorWheelButton;
+
+//Joystick2Layout RotateToNumberButton = Joystick2Layout::kColorWheelRotationControl;
+
+//Joystick2Layout RotateToColorButton = Joystick2Layout::kColorWheelColorControl;
+
+
+static constexpr auto i2cPort = frc::I2C::Port::kOnboard;
+rev::ColorSensorV3 m_colorSensor(i2cPort);
+rev::ColorMatch m_colorMatcher;
+WPI_TalonSRX *colormotor;
+frc::Joystick *colorjoystick;
+frc::Solenoid *colorsolenoid;
+
+ColorWheel::ColorWheel(WPI_TalonSRX *motor, frc::Joystick *joystick, frc::Solenoid *solenoid){
+    m_colorMatcher.AddColorMatch(kBlueTarget);
+    m_colorMatcher.AddColorMatch(kGreenTarget);
+    m_colorMatcher.AddColorMatch(kRedTarget);
+    m_colorMatcher.AddColorMatch(kYellowTarget);
+    colormotor = motor;
+    colorjoystick = joystick;
+    colorsolenoid = solenoid;
+ } 
+
+frc::Color ColorWheel::getPreviousColor(frc::Color color){
+    //Color order: Red, Yellow, Blue, Green
+    if (color == kGreenTarget)
+    {
+        return kBlueTarget;
+    }
+    else if (color == kBlueTarget)
+    {
+        return kYellowTarget;
+    }
+    else if (color == kYellowTarget)
+    {
+        return kRedTarget;
+    }
+    else if (color == kRedTarget)
+    {
+        return kGreenTarget;
+    }
+}
+frc::Color ColorWheel::getNextColor(frc::Color color){
+    if (color == kGreenTarget)
+    {
+        return kRedTarget;
+    }
+    else if (color == kBlueTarget)
+    {
+        return kGreenTarget;
+    }
+    else if (color == kYellowTarget)
+    {
+        return kBlueTarget;
+    }
+    else if (color == kRedTarget)
+    {
+        return kYellowTarget;
+    }
+}
+frc::Color ColorWheel::getSpinTargetColor(frc::Color color){
+    if (color == kGreenTarget)
+    {
+        return kYellowTarget;
+    }
+    else if (color == kBlueTarget)
+    {
+        return kRedTarget;
+    }
+    else if (color == kYellowTarget)
+    {
+        return kGreenTarget;
+    }
+    else if (color == kRedTarget)
+    {
+        return kBlueTarget;
+    }
+
+}
+void ColorWheel::ControlSolenoid(){
+    if (colorjoystick->GetRawButtonPressed(1)){
+        cout <<"button 1 pressed"<< endl;
+        bool isup = colorsolenoid->Get();
+
+        if(isup == true){
+            colorsolenoid->Set(false);
+        }
+        else{
+            colorsolenoid->Set(true);
+        }
+    }
+
 }
 
-void ColorWheel::RotateToNumber(WPI_TalonSRX *motor, frc::Joystick *joystick, rev::ColorSensorV3 *sensor){
-    if (spinState == WheelState::NotSpinning && joystick->GetRawButton(1))
+
+void ColorWheel::RotateToNumber(){
+    if (spinState == WheelState::NotSpinning && colorjoystick->GetRawButtonPressed (2))
     {
+        cout <<"button 2 pressed"<< endl;
         spinState = WheelState::InitSpinning;
+        CurrentButton = 1;
 
     }
-    if (spinState == WheelState::InitSpinning) 
+    if (spinState == WheelState::InitSpinning && CurrentButton == 1) 
     {
         NumSpins = 0;
-        motor->Set(ControlMode::PercentOutput,0.2);
+        colormotor->Set(ControlMode::PercentOutput,0.7);
         spinState = WheelState::Spinning;
     }
-    if (spinState == WheelState::Spinning)
-    {
-        if (joystick->GetRawButton(1) || NumSpins>6)
+    if (spinState == WheelState::Spinning && CurrentButton == 1)
+    { 
+        if (colorjoystick->GetRawButtonPressed(2) || NumSpins>7)
         {
-            motor->Set(ControlMode::PercentOutput,0.0);
+            colormotor->Set(ControlMode::PercentOutput,0.0);
             spinState = WheelState::NotSpinning;
+            CurrentButton = 0;
             return;
+        
         }
-        if (sensor->GetColor()== kRedTarget){
-            NumSpins = NumSpins+1;
 
+        //A value betwen 0 and 1, 1 being absolute perfect color match
+        double colorConfidence = 0.0;
+        frc::Color detectedColor = m_colorSensor.GetColor();
+        frc::Color matchedColor = m_colorMatcher.MatchClosestColor(detectedColor, colorConfidence); 
+        PrintColor(matchedColor, colorConfidence);
+        //do we need to check confidence number in this condition to see how confident the color matcher 
+        //thinks the color is red? A value close to one means more confident. 
+
+        if (matchedColor == kRedTarget && OnRed == false && colorConfidence >= ColorConfidenceTarget){
+            NumSpins = NumSpins+1;
+            OnRed = true;
+        }
+        else if (!(matchedColor == kRedTarget)) {
+            OnRed = false;
         }
     }
 }
 
-/*ColorWheel::ColorWheel(WPI_TalonSRX *colorMotor)
-{
-    m_colorMotor = colorMotor;
-    m_colorSensor = new rev::ColorSensorV3(frc::I2C::Port::kOnboard);
 
-    m_colorMatch->AddColorMatch(kBlueTarget);
-    m_colorMatch->AddColorMatch(kRedTarget);
-    m_colorMatch->AddColorMatch(kYellowTarget);
-    m_colorMatch->AddColorMatch(kGreenTarget);
-}
-frc::Color ColorWheel::GetCurrentColor(){
-    return m_colorMatch->MatchClosestColor(m_colorSensor->GetColor(), confidence);
+void ColorWheel::RotateToColor(frc::Color *targetcolor){
+    double colorConfidence = 0.0;
+    frc::Color spinTargetColor = getSpinTargetColor(*targetcolor);
+    frc::Color detectedColor = m_colorSensor.GetColor();
+    frc::Color matchedColor = m_colorMatcher.MatchClosestColor(detectedColor, colorConfidence); 
+    frc::SmartDashboard::PutNumber("SpinState", spinState);
+    if (spinState == WheelState::NotSpinning && colorjoystick->GetRawButton(3))
+    {
+        spinState = WheelState::InitSpinning;
+        CurrentButton = 2;
+        cout <<"button 3 pressed"<< endl;
+    }
+    if (spinState == WheelState::InitSpinning && CurrentButton == 2)
+    {
+        spinState = WheelState::Spinning;
+        colormotor->Set(ControlMode::PercentOutput, 0.2);
+        //*********
+        // check to make sure we're getting a unique copy of matched color 
+        //*********
+        PreviousColor = getPreviousColor(matchedColor);
+        CurrentColor = matchedColor;
+    }
+    if (spinState == WheelState::Spinning && CurrentButton == 2)
+    {
+        PrintColor(matchedColor, colorConfidence);
+        if (!(matchedColor == CurrentColor))
+        {
+            if (matchedColor == getNextColor(PreviousColor))
+            {
+                if (matchedColor == spinTargetColor && colorConfidence >= ColorConfidenceTarget)
+                {
+                    if (NumColorSamples > 1)
+                    {
+                        spinState = WheelState::NotSpinning;
+                        colormotor->Set(ControlMode::PercentOutput, 0.0);
+                        NumColorSamples = 0;
+                        CurrentButton = 0;
+                    }
+                    else 
+                    {
+                        NumColorSamples += 1;
+                    }
+                }
+                else 
+                {
+                    PreviousColor = getPreviousColor(matchedColor);
+                    CurrentColor = matchedColor;
+                }
+            }
+        }
+        
+    }
 }
-void ColorWheel::TurnToColor(frc::Color targetColor){
-  if(targetColor == GetCurrentColor()){}
-  else{
-      m_colorMotor->Set(ControlMode::PercentOutput, .1);
-  }   
-}*/
+
+void ColorWheel::PrintColor(frc::Color color, double colorConfidence){
+        if(color == kBlueTarget){
+            frc::SmartDashboard::PutString("color","Blue"); 
+            cout << "Blue\t" << colorConfidence << endl;
+        }
+        else if(color == kRedTarget){
+            frc::SmartDashboard::PutString("color","Red");
+            cout << "Red\t" << colorConfidence << endl;
+        }
+        else if(color == kYellowTarget){
+            frc::SmartDashboard::PutString("color","Yellow");
+            cout << "Yellow\t" << colorConfidence << endl;
+        }
+        else if(color == kGreenTarget){
+            frc::SmartDashboard::PutString("color","Green");
+            cout << "Green\t" << colorConfidence << endl;
+        }
+        else{
+            frc::SmartDashboard::PutString("color","No color detected");
+            cout << "no color detected" << endl;
+        }
+        frc::SmartDashboard::PutNumber("NumSpins", NumSpins);
+        cout << "Green\t" << colorConfidence << endl;
+        cout << "Yellow\t" << colorConfidence << endl;
+        frc::SmartDashboard::PutNumber("Confidence", colorConfidence);
+        frc::SmartDashboard::PutNumber("SpinState", spinState);
+
+    }
+
diff --git a/robot/src/main/cpp/Robot.cpp b/robot/src/main/cpp/Robot.cpp
index 202adc9..08ef2b3 100644
--- a/robot/src/main/cpp/Robot.cpp
+++ b/robot/src/main/cpp/Robot.cpp
@@ -39,31 +39,14 @@
 #include <frc/smartdashboard/SmartDashboard.h>
 
 void Robot::RobotInit() {
-  m_chooser.SetDefaultOption(kAutoNameDefault, kAutoNameDefault);
-  m_chooser.AddOption(kAutoNameCustom, kAutoNameCustom);
-  frc::SmartDashboard::PutData("Auto Modes", &m_chooser);
-  frc::SmartDashboard::PutNumber("Aimpid",0.1);
+  //frc::SmartDashboard::PutData("Auto Modes", &m_chooser);
+  //frc::SmartDashboard::PutNumber("Aimpid",0.1);
 
   joystick_1 = new frc::Joystick(kPrimaryDriverJoystickID);
-  shooter = new Shooter(3,3);//Should have different numbers if your board supports it during testing
-  //printf("robotcpp joystick_addr = %d \n",joystick_1);
-  teleopFunctions = new TeleopFunctions(joystick_1, shooter);
-  //Button assignments
-
-  // Unused Variable
-  // int shooterButton = 1;
-  intake = new Intake(); //Uses SparkMax motor 3 
-
-  // Trajectory Test (prints to RioLog)
-  trajectory_generation_utility = new TrajectoryGenerationUtility();
-  trajectory_generation_utility->GenerateTestTrajectory();
-
-  // Initialize SparkDrive Object using the UltraLord Drivetrain Configuration.
-  spark_drive = new SparkDrive(new rev::CANSparkMax(kUltraLeftFrontMotorID, rev::CANSparkMax::MotorType::kBrushless),
-    new rev::CANSparkMax(kUltraRightFrontMotorID, rev::CANSparkMax::MotorType::kBrushless), 
-    new rev::CANSparkMax(kUltraLeftBackMotorID, rev::CANSparkMax::MotorType::kBrushless), 
-    new rev::CANSparkMax(kUltraRightBackMotorID, rev::CANSparkMax::MotorType::kBrushless)
-  );
+  colorWheelmotor = new WPI_TalonSRX(4);
+  solenoid = new frc::Solenoid(4);
+  colorWheel = new ColorWheel(colorWheelmotor, joystick_1, solenoid);
+  targetcolor = new frc::Color(kGreenTarget);
 }
 
 /**
@@ -88,69 +71,26 @@ void Robot::RobotPeriodic() {}
  * make sure to add them to the chooser code above as well.
  */
 void Robot::AutonomousInit() {
-  m_autoSelected = m_chooser.GetSelected();
-  // m_autoSelected = SmartDashboard::GetString("Auto Selector",
-  //     kAutoNameDefault);
-  std::cout << "Auto selected: " << m_autoSelected << std::endl;
-
-  if (m_autoSelected == kAutoNameCustom) {
-    // Custom Auto goes here
-  } else {
-    // Default Auto goes here
-  }
-}
+ 
+ }
 
 void Robot::AutonomousPeriodic() {
-  if (m_autoSelected == kAutoNameCustom) {
-    // Custom Auto goes here
-  } else {
-    // Default Auto goes here
-  }
-}
+ 
+ }
 
 void Robot::TeleopInit() {}
 
 void Robot::TeleopPeriodic() {
- //teleopFunctions->ShooterFunction();
-
-  // need to create sparkdrive above for this code 
- // spark_drive = new SparkDrive(new rev::CANSparkMax(3, rev::CANSparkMax::MotorType::kBrushless)
-   
-  std::cout << "Teleop Tick" << std::endl;
-  
-  double Pid = frc::SmartDashboard::GetNumber("Aimpid",0.1);
-  //shooter->SetAimHeightPid(Pid);
-  //std::cout << "pidvalue = " << Pid <<std::endl;
-    if (joystick_1->GetRawButtonPressed(3)) {
-    //intake->Start();
-    shooter->AimHeight(10);
-  }
-    if (joystick_1->GetRawButtonPressed(4)) {
-    shooter->AimHeight(0);
-    }
-
-    
-  //test of intake motor code
   
-  if (joystick_1->GetRawButtonPressed(1)) {
-    //intake->Start();
-    intake->SetSpeed(100);
-  }
-    if (joystick_1->GetRawButtonPressed(2)) {
-    intake->Stop();
-  }
-  
-  // Call SparkDrive::TankDrive() using the drivetrain motors
-  spark_drive->TankDrive(
-    joystick_1->GetRawAxis(y_axis), 
-    joystick_1->GetRawAxis(z_axis), 
-    joystick_1->GetRawButton(right_bumper), 
-    joystick_1->GetRawButton(left_bumper)
-  );
+  //Call our color wheel class to execute code determining when to count rotations.
+  colorWheel->RotateToNumber();
+  colorWheel->RotateToColor(targetcolor);
+  colorWheel->ControlSolenoid();
 }
+  
 
 void Robot::TestPeriodic() {}
-
+//std::cout << "In test periodic" << endl;
 #ifndef RUNNING_FRC_TESTS
 int main() { return frc::StartRobot<Robot>(); }
 #endif
diff --git a/robot/src/main/include/ColorWheel.h b/robot/src/main/include/ColorWheel.h
index 8ddc8d2..bb30b90 100644
--- a/robot/src/main/include/ColorWheel.h
+++ b/robot/src/main/include/ColorWheel.h
@@ -1,17 +1,25 @@
-    #pragma once
+#pragma once
 
-#include "rev/ColorSensorV3.h"
-#include "rev/ColorMatch.h"
-#include "frc/I2C.h"
-#include "frc/util/color.h"
-#include "ctre/Phoenix.h"
-#include "RobotUtilities.h"
+#include <rev/ColorSensorV3.h>
+#include <rev/ColorMatch.h>
+#include <frc/I2C.h>
+#include <frc/util/color.h>
+#include <ctre/Phoenix.h>
+#include <RobotUtilities.h>
+#include <frc/Solenoid.h>
 
 class ColorWheel{
     public:
-     ColorWheel();
-     void RotateToNumber(WPI_TalonSRX *motor, frc::Joystick *joystick, rev::ColorSensorV3 *sensor);
+     ColorWheel(WPI_TalonSRX *motor, frc::Joystick *joystick, frc::Solenoid *solenoid);
+     void RotateToNumber();
+     void RotateToColor(frc::Color *targetcolor);
+     void ControlSolenoid();
     private:
+     void PrintColor(frc::Color color, double colorConfidence);
+     frc::Solenoid *Solenoid;
+     frc::Color getNextColor(frc::Color color);
+     frc::Color getPreviousColor(frc::Color color);
+     frc::Color getSpinTargetColor(frc::Color color);
 
 };
 
diff --git a/robot/src/main/include/Robot.h b/robot/src/main/include/Robot.h
index b055738..975d111 100644
--- a/robot/src/main/include/Robot.h
+++ b/robot/src/main/include/Robot.h
@@ -16,11 +16,7 @@
 
 #include "RobotUtilities.h"
 
-#include "SparkDrive.h"
-#include "TeleopFunctions.h"
-#include "TrajectoryGenerationUtility.h"
-
-#include "Intake.h"
+#include "ColorWheel.h"
 
 class Robot : public frc::TimedRobot {
  public:
@@ -33,27 +29,16 @@ class Robot : public frc::TimedRobot {
   void TestPeriodic() override;
 
  private:
-  frc::SendableChooser<std::string> m_chooser;
-  const std::string kAutoNameDefault = "Default";
-  const std::string kAutoNameCustom = "My Auto";
-  std::string m_autoSelected;
-
-  // Autonomous Functions
-  TrajectoryGenerationUtility* trajectory_generation_utility;
-
   // INPUTS
   frc::Joystick *joystick_1;
-
-  // DRIVE
-  SparkDrive* spark_drive;
-
-  TeleopFunctions *teleopFunctions;
-  Shooter *shooter;
+  frc::Color *targetcolor;
 
    //BUTTONS
-  int const shooterButton = 1;
+  int const colorWheelButton = 1;
 
-  // Intake mechanism
-  Intake* intake;
+  //Color wheel class
+  WPI_TalonSRX *colorWheelmotor;
+  ColorWheel* colorWheel;
+  frc::Solenoid *solenoid;
 
 };
diff --git a/robot/src/main/include/RobotUtilities.h b/robot/src/main/include/RobotUtilities.h
index 559f998..d7c0c42 100644
--- a/robot/src/main/include/RobotUtilities.h
+++ b/robot/src/main/include/RobotUtilities.h
@@ -16,6 +16,17 @@ enum JoystickInputs
   left_bumper = 5, right_bumper = 6, left_trigger = 7, right_trigger = 8,
   back_button = 9, start_button = 10, l_trigger = 11, r_trigger = 12
 };
+enum Joystick1Layout{
+    kForwardBackwardAxis = y_axis, kRotAxis = z_axis, kExtendClimbButton = start_button, 
+    kRetractClimbButton = back_button, kWinchButton = x_button, kTurtleButton = right_bumper, 
+    kTurboButton = right_trigger
+}; 
+enum Joystick2Layout{
+    kAimShootButton = b_button, kIntakeButton = x_button, kOuttakeButton = a_button, kShootButton = y_button,
+    kRegressGenevaButton = left_bumper, kAdvanceGenevaButton = right_bumper, kRetractColorWheelButton = left_trigger,
+    kDeployColorWheelButton = right_trigger, kColorWheelColorControl = start_button, kColorWheelRotationControl = back_button, 
+    kIncreaseAimOffsetPOV = 90, kDecreaseAimOffsetPOV = 270,
+};
 
 const frc::Color kBlueTarget = frc::Color(0.143, 0.427, 0.429);
 const frc::Color kGreenTarget = frc::Color(0.197, 0.561, 0.240);
@@ -23,8 +34,26 @@ const frc::Color kRedTarget = frc::Color(0.561, 0.232, 0.114);
 const frc::Color kYellowTarget = frc::Color(0.361, 0.524, 0.113);
 
 const int kPrimaryDriverJoystickID = 0;
+const int kSecondaryDriverJoystickID = 1;
+
+const int kUltraLeftFrontMotorID = 1;
+const int kUltraRightFrontMotorID = 2;
+const int kUltraLeftBackMotorID = 3;
+const int kUltraRightBackMotorID = 4;
+
+const int kBigSlinkLeftFrontMotorID = 1;
+const int kBigSlinkRightFrontMotorID = 2;
+const int kBigSlinkLeftBackMotorID = 3;
+const int kBigSlinkRightBackMotorID = 4;
+
+const int kIntakeMotorID = 5;
+const int kGenevaMotorID = 6;
+const int kFlyWheelMotorID = 7;
+const int kAimMotorID = 8;
+const int kColorWheelMotorID = 9;
+const int kClimbWinchMotorID = 10; //10 on comp bot 11 on practice
 
-const int kUltraLeftFrontMotorID = 10;
-const int kUltraRightFrontMotorID = 1;
-const int kUltraLeftBackMotorID = 2;
-const int kUltraRightBackMotorID = 3;
\ No newline at end of file
+const int kIntakePinID = 0;
+const int kColorWheelSolenoidID = 1;
+const int kPunchSolenoidID = 2;
+const int kClimbTelescopeSolenoidID = 3; //4 on practice, 3 on comp