From 3913355bc047d92a7e62104ad738f7df5685afe8 Mon Sep 17 00:00:00 2001
From: erinleonello <erin.leonello@gmail.com>
Date: Thu, 6 Feb 2020 17:20:01 -0500
Subject: [PATCH 01/23] initial commit.

corrected author.
---
 robot/src/main/cpp/ColorWheel.cpp | 13 ++++++++++---
 1 file changed, 10 insertions(+), 3 deletions(-)

diff --git a/robot/src/main/cpp/ColorWheel.cpp b/robot/src/main/cpp/ColorWheel.cpp
index 81eabb3..bb45b17 100644
--- a/robot/src/main/cpp/ColorWheel.cpp
+++ b/robot/src/main/cpp/ColorWheel.cpp
@@ -11,8 +11,11 @@ WheelState spinState = WheelState::NotSpinning;
 
 frc::Color ColorState;
 
+
 int NumSpins = 0;
 
+bool OnRed = false;
+
 ColorWheel::ColorWheel(){
   //Add color match code here
 }
@@ -30,16 +33,20 @@ void ColorWheel::RotateToNumber(WPI_TalonSRX *motor, frc::Joystick *joystick, re
         spinState = WheelState::Spinning;
     }
     if (spinState == WheelState::Spinning)
-    {
+    {   
+        ColorState = sensor->GetColor();
         if (joystick->GetRawButton(1) || NumSpins>6)
         {
             motor->Set(ControlMode::PercentOutput,0.0);
             spinState = WheelState::NotSpinning;
             return;
         }
-        if (sensor->GetColor()== kRedTarget){
+        if (sensor->GetColor()== kRedTarget && OnRed == false){
             NumSpins = NumSpins+1;
-
+            OnRed = true;
+        }
+        else if (!(sensor->GetColor()== kRedTarget)) {
+            OnRed = false;
         }
     }
 }

From 03e6a3d80659089def6616299780410e47ff5018 Mon Sep 17 00:00:00 2001
From: Grant <grant@abgames.com>
Date: Thu, 6 Feb 2020 18:23:32 -0500
Subject: [PATCH 02/23] adding rotate to color method

---
 robot/src/main/cpp/ColorWheel.cpp   | 27 +++++++++++++++++++++++++--
 robot/src/main/include/ColorWheel.h |  1 +
 2 files changed, 26 insertions(+), 2 deletions(-)

diff --git a/robot/src/main/cpp/ColorWheel.cpp b/robot/src/main/cpp/ColorWheel.cpp
index bb45b17..ce1aeee 100644
--- a/robot/src/main/cpp/ColorWheel.cpp
+++ b/robot/src/main/cpp/ColorWheel.cpp
@@ -9,13 +9,15 @@ enum WheelState{
 };
 WheelState spinState = WheelState::NotSpinning;
 
-frc::Color ColorState;
+frc::Color CurrentColor;
 
 
 int NumSpins = 0;
 
 bool OnRed = false;
 
+bool OnTargetColor = false;
+
 ColorWheel::ColorWheel(){
   //Add color match code here
 }
@@ -34,7 +36,7 @@ void ColorWheel::RotateToNumber(WPI_TalonSRX *motor, frc::Joystick *joystick, re
     }
     if (spinState == WheelState::Spinning)
     {   
-        ColorState = sensor->GetColor();
+
         if (joystick->GetRawButton(1) || NumSpins>6)
         {
             motor->Set(ControlMode::PercentOutput,0.0);
@@ -51,6 +53,27 @@ void ColorWheel::RotateToNumber(WPI_TalonSRX *motor, frc::Joystick *joystick, re
     }
 }
 
+
+void ColorWheel::RotateToColor(WPI_TalonSRX *motor, frc::Joystick *joystick, rev::ColorSensorV3 *sensor, frc::Color *targetcolor){
+    CurrentColor = sensor->GetColor();
+    if (spinState == WheelState::NotSpinning && joystick->GetRawButton(1))
+    {
+        spinState = WheelState::InitSpinning;
+    }
+    if (spinState == WheelState::InitSpinning)
+    {
+        spinState = WheelState::Spinning;
+        motor->Set(ControlMode::PercentOutput, 0.2);
+    }
+    if (spinState == WheelState::Spinning)
+    {
+        if (!(sensor->GetColor()== targetcolor)){
+
+        }
+    }
+   
+}
+
 /*ColorWheel::ColorWheel(WPI_TalonSRX *colorMotor)
 {
     m_colorMotor = colorMotor;
diff --git a/robot/src/main/include/ColorWheel.h b/robot/src/main/include/ColorWheel.h
index 8ddc8d2..1b8210a 100644
--- a/robot/src/main/include/ColorWheel.h
+++ b/robot/src/main/include/ColorWheel.h
@@ -11,6 +11,7 @@ class ColorWheel{
     public:
      ColorWheel();
      void RotateToNumber(WPI_TalonSRX *motor, frc::Joystick *joystick, rev::ColorSensorV3 *sensor);
+     void RotateToColor(WPI_TalonSRX *motor, frc::Joystick *joystick, rev::ColorSensorV3 *sensor, frc::Color *targetcolor);
     private:
 
 };

From 0eafde2d07958603f908f0804e1706f39d90220f Mon Sep 17 00:00:00 2001
From: Grant <grant@abgames.com>
Date: Sat, 8 Feb 2020 13:15:28 -0500
Subject: [PATCH 03/23] vaughan, comment out code that won't compile

---
 robot/src/main/cpp/ColorWheel.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/robot/src/main/cpp/ColorWheel.cpp b/robot/src/main/cpp/ColorWheel.cpp
index ce1aeee..6dd021d 100644
--- a/robot/src/main/cpp/ColorWheel.cpp
+++ b/robot/src/main/cpp/ColorWheel.cpp
@@ -67,9 +67,9 @@ void ColorWheel::RotateToColor(WPI_TalonSRX *motor, frc::Joystick *joystick, rev
     }
     if (spinState == WheelState::Spinning)
     {
-        if (!(sensor->GetColor()== targetcolor)){
+      //  if (!(sensor->GetColor() == targetcolor)){
 
-        }
+        //}
     }
    
 }

From 74374bdc8a40c5214d063a8d932b572d4e83ea42 Mon Sep 17 00:00:00 2001
From: Grant <grant@abgames.com>
Date: Sat, 8 Feb 2020 14:43:27 -0500
Subject: [PATCH 04/23] Added comments to use ColorMatch class

---
 robot/src/main/cpp/ColorWheel.cpp | 10 +++++++++-
 1 file changed, 9 insertions(+), 1 deletion(-)

diff --git a/robot/src/main/cpp/ColorWheel.cpp b/robot/src/main/cpp/ColorWheel.cpp
index 6dd021d..655b70e 100644
--- a/robot/src/main/cpp/ColorWheel.cpp
+++ b/robot/src/main/cpp/ColorWheel.cpp
@@ -18,10 +18,18 @@ bool OnRed = false;
 
 bool OnTargetColor = false;
 
+//Notes after some research Saturday: We probably want to use the ColorMatch class to detect color rather than the 
+//ColorSensor. See example code https://github.com/REVrobotics/Color-Sensor-v3-Examples/blob/master/C%2B%2B/Color%20Match/src/main/cpp/Robot.cpp
+static constexpr auto i2cPort = frc::I2C::Port::kOnboard;
+rev::ColorSensorV3 m_sensor(i2cPort);
+rev::ColorMatch m_colorMatch;
+
 ColorWheel::ColorWheel(){
-  //Add color match code here
+    
+  
 }
 
+//Update RotateToNumber to not take in the sensor and get current color from m_colorMatch
 void ColorWheel::RotateToNumber(WPI_TalonSRX *motor, frc::Joystick *joystick, rev::ColorSensorV3 *sensor){
     if (spinState == WheelState::NotSpinning && joystick->GetRawButton(1))
     {

From a361fb921f9da33caaaa57122a87772f14a25037 Mon Sep 17 00:00:00 2001
From: Grant <grant@abgames.com>
Date: Mon, 10 Feb 2020 18:20:50 -0500
Subject: [PATCH 05/23] vaughan - added color matcher code to determine if we
 are on Red.

---
 robot/src/main/cpp/ColorWheel.cpp   | 39 ++++++++++++++++++-----------
 robot/src/main/cpp/Robot.cpp        |  3 +++
 robot/src/main/include/ColorWheel.h |  4 +--
 robot/src/main/include/Robot.h      |  5 ++++
 4 files changed, 35 insertions(+), 16 deletions(-)

diff --git a/robot/src/main/cpp/ColorWheel.cpp b/robot/src/main/cpp/ColorWheel.cpp
index 655b70e..ee2788e 100644
--- a/robot/src/main/cpp/ColorWheel.cpp
+++ b/robot/src/main/cpp/ColorWheel.cpp
@@ -10,8 +10,6 @@ enum WheelState{
 WheelState spinState = WheelState::NotSpinning;
 
 frc::Color CurrentColor;
-
-
 int NumSpins = 0;
 
 bool OnRed = false;
@@ -21,17 +19,22 @@ bool OnTargetColor = false;
 //Notes after some research Saturday: We probably want to use the ColorMatch class to detect color rather than the 
 //ColorSensor. See example code https://github.com/REVrobotics/Color-Sensor-v3-Examples/blob/master/C%2B%2B/Color%20Match/src/main/cpp/Robot.cpp
 static constexpr auto i2cPort = frc::I2C::Port::kOnboard;
-rev::ColorSensorV3 m_sensor(i2cPort);
-rev::ColorMatch m_colorMatch;
+rev::ColorSensorV3 m_colorSensor(i2cPort);
+rev::ColorMatch m_colorMatcher;
 
 ColorWheel::ColorWheel(){
-    
-  
-}
+    m_colorMatcher.AddColorMatch(kBlueTarget);
+    m_colorMatcher.AddColorMatch(kGreenTarget);
+    m_colorMatcher.AddColorMatch(kRedTarget);
+    m_colorMatcher.AddColorMatch(kYellowTarget);
+ }
 
 //Update RotateToNumber to not take in the sensor and get current color from m_colorMatch
-void ColorWheel::RotateToNumber(WPI_TalonSRX *motor, frc::Joystick *joystick, rev::ColorSensorV3 *sensor){
-    if (spinState == WheelState::NotSpinning && joystick->GetRawButton(1))
+void ColorWheel::RotateToNumber(WPI_TalonSRX *motor, frc::Joystick *joystick){
+
+    //Put in some smart dashboard output to be helpful for debugging
+    
+    if (spinState == WheelState::NotSpinning && joystick->GetRawButtonPressed(1))
     {
         spinState = WheelState::InitSpinning;
 
@@ -45,25 +48,33 @@ void ColorWheel::RotateToNumber(WPI_TalonSRX *motor, frc::Joystick *joystick, re
     if (spinState == WheelState::Spinning)
     {   
 
-        if (joystick->GetRawButton(1) || NumSpins>6)
+        if (joystick->GetRawButtonPressed(1) || NumSpins>6)
         {
             motor->Set(ControlMode::PercentOutput,0.0);
             spinState = WheelState::NotSpinning;
             return;
         }
-        if (sensor->GetColor()== kRedTarget && OnRed == false){
+
+        //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); 
+
+        //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){
             NumSpins = NumSpins+1;
             OnRed = true;
         }
-        else if (!(sensor->GetColor()== kRedTarget)) {
+        else if (!(matchedColor == kRedTarget)) {
             OnRed = false;
         }
     }
 }
 
 
-void ColorWheel::RotateToColor(WPI_TalonSRX *motor, frc::Joystick *joystick, rev::ColorSensorV3 *sensor, frc::Color *targetcolor){
-    CurrentColor = sensor->GetColor();
+void ColorWheel::RotateToColor(WPI_TalonSRX *motor, frc::Joystick *joystick, frc::Color *targetcolor){
+    CurrentColor = m_colorSensor.GetColor();
     if (spinState == WheelState::NotSpinning && joystick->GetRawButton(1))
     {
         spinState = WheelState::InitSpinning;
diff --git a/robot/src/main/cpp/Robot.cpp b/robot/src/main/cpp/Robot.cpp
index 202adc9..2e1e90b 100644
--- a/robot/src/main/cpp/Robot.cpp
+++ b/robot/src/main/cpp/Robot.cpp
@@ -64,6 +64,8 @@ void Robot::RobotInit() {
     new rev::CANSparkMax(kUltraLeftBackMotorID, rev::CANSparkMax::MotorType::kBrushless), 
     new rev::CANSparkMax(kUltraRightBackMotorID, rev::CANSparkMax::MotorType::kBrushless)
   );
+
+  colorWheel = new ColorWheel();
 }
 
 /**
@@ -147,6 +149,7 @@ void Robot::TeleopPeriodic() {
     joystick_1->GetRawButton(right_bumper), 
     joystick_1->GetRawButton(left_bumper)
   );
+
 }
 
 void Robot::TestPeriodic() {}
diff --git a/robot/src/main/include/ColorWheel.h b/robot/src/main/include/ColorWheel.h
index 1b8210a..b69caa6 100644
--- a/robot/src/main/include/ColorWheel.h
+++ b/robot/src/main/include/ColorWheel.h
@@ -10,8 +10,8 @@
 class ColorWheel{
     public:
      ColorWheel();
-     void RotateToNumber(WPI_TalonSRX *motor, frc::Joystick *joystick, rev::ColorSensorV3 *sensor);
-     void RotateToColor(WPI_TalonSRX *motor, frc::Joystick *joystick, rev::ColorSensorV3 *sensor, frc::Color *targetcolor);
+     void RotateToNumber(WPI_TalonSRX *motor, frc::Joystick *joystick);
+     void RotateToColor(WPI_TalonSRX *motor, frc::Joystick *joystick, frc::Color *targetcolor);
     private:
 
 };
diff --git a/robot/src/main/include/Robot.h b/robot/src/main/include/Robot.h
index b055738..8c7eb54 100644
--- a/robot/src/main/include/Robot.h
+++ b/robot/src/main/include/Robot.h
@@ -21,6 +21,7 @@
 #include "TrajectoryGenerationUtility.h"
 
 #include "Intake.h"
+#include "ColorWheel.h"
 
 class Robot : public frc::TimedRobot {
  public:
@@ -56,4 +57,8 @@ class Robot : public frc::TimedRobot {
   // Intake mechanism
   Intake* intake;
 
+  //Color wheel class
+  ColorWheel* colorWheel;
+
+
 };

From b4d59ac2409deca821b169a156d7272ba7dd7b25 Mon Sep 17 00:00:00 2001
From: Sherry Vaughan <slvaughan@gmail.com>
Date: Tue, 11 Feb 2020 20:43:23 -0500
Subject: [PATCH 06/23] Vaughan - removed all non color wheel code

---
 robot/src/main/cpp/Robot.cpp   | 87 ++++------------------------------
 robot/src/main/include/Robot.h | 26 +---------
 2 files changed, 11 insertions(+), 102 deletions(-)

diff --git a/robot/src/main/cpp/Robot.cpp b/robot/src/main/cpp/Robot.cpp
index 2e1e90b..a75aa50 100644
--- a/robot/src/main/cpp/Robot.cpp
+++ b/robot/src/main/cpp/Robot.cpp
@@ -39,32 +39,11 @@
 #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(1);
   colorWheel = new ColorWheel();
 }
 
@@ -90,66 +69,18 @@ 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(colorWheelmotor, joystick_1);
 }
 
 void Robot::TestPeriodic() {}
diff --git a/robot/src/main/include/Robot.h b/robot/src/main/include/Robot.h
index 8c7eb54..ea9ad85 100644
--- a/robot/src/main/include/Robot.h
+++ b/robot/src/main/include/Robot.h
@@ -16,11 +16,6 @@
 
 #include "RobotUtilities.h"
 
-#include "SparkDrive.h"
-#include "TeleopFunctions.h"
-#include "TrajectoryGenerationUtility.h"
-
-#include "Intake.h"
 #include "ColorWheel.h"
 
 class Robot : public frc::TimedRobot {
@@ -34,31 +29,14 @@ 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;
-
    //BUTTONS
-  int const shooterButton = 1;
-
-  // Intake mechanism
-  Intake* intake;
+  int const colorWheelButton = 1;
 
   //Color wheel class
+  WPI_TalonSRX *colorWheelmotor;
   ColorWheel* colorWheel;
 
-
 };

From 12398033192da67edc582a7e574df9925e3a6d16 Mon Sep 17 00:00:00 2001
From: ColeScheller <schelcc@pobox.com>
Date: Thu, 13 Feb 2020 17:42:59 -0500
Subject: [PATCH 07/23] Finishing rotoate to color.

---
 robot/src/main/cpp/ColorWheel.cpp | 15 ++++++++++-----
 1 file changed, 10 insertions(+), 5 deletions(-)

diff --git a/robot/src/main/cpp/ColorWheel.cpp b/robot/src/main/cpp/ColorWheel.cpp
index ee2788e..da2462a 100644
--- a/robot/src/main/cpp/ColorWheel.cpp
+++ b/robot/src/main/cpp/ColorWheel.cpp
@@ -16,6 +16,8 @@ bool OnRed = false;
 
 bool OnTargetColor = false;
 
+double ColorConfidenceTarget = 0.9;
+
 //Notes after some research Saturday: We probably want to use the ColorMatch class to detect color rather than the 
 //ColorSensor. See example code https://github.com/REVrobotics/Color-Sensor-v3-Examples/blob/master/C%2B%2B/Color%20Match/src/main/cpp/Robot.cpp
 static constexpr auto i2cPort = frc::I2C::Port::kOnboard;
@@ -62,7 +64,7 @@ void ColorWheel::RotateToNumber(WPI_TalonSRX *motor, frc::Joystick *joystick){
 
         //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){
+        if (matchedColor == kRedTarget && OnRed == false && colorConfidence >= ColorConfidenceTarget){
             NumSpins = NumSpins+1;
             OnRed = true;
         }
@@ -74,7 +76,9 @@ void ColorWheel::RotateToNumber(WPI_TalonSRX *motor, frc::Joystick *joystick){
 
 
 void ColorWheel::RotateToColor(WPI_TalonSRX *motor, frc::Joystick *joystick, frc::Color *targetcolor){
-    CurrentColor = m_colorSensor.GetColor();
+    double colorConfidence = 0.0;
+    frc::Color detectedColor = m_colorSensor.GetColor();
+    frc::Color matchedColor = m_colorMatcher.MatchClosestColor(detectedColor, colorConfidence); 
     if (spinState == WheelState::NotSpinning && joystick->GetRawButton(1))
     {
         spinState = WheelState::InitSpinning;
@@ -86,9 +90,10 @@ void ColorWheel::RotateToColor(WPI_TalonSRX *motor, frc::Joystick *joystick, frc
     }
     if (spinState == WheelState::Spinning)
     {
-      //  if (!(sensor->GetColor() == targetcolor)){
-
-        //}
+        if (matchedColor == targetcolor && colorConfidence >= ColorConfidenceTarget)){
+            spinState = WheelState::NotSpinning;
+            motor->Set(ControlMode::PercentOutput, 0.2);
+        }
     }
    
 }

From cfa311d861204628b31ffbd844cb7ffb9d05b313 Mon Sep 17 00:00:00 2001
From: erinleonello <erin.leonello@gmail.com>
Date: Thu, 13 Feb 2020 17:42:59 -0500
Subject: [PATCH 08/23] Finishing rotoate to color.

correcting author
---
 robot/src/main/cpp/ColorWheel.cpp | 15 ++++++++++-----
 1 file changed, 10 insertions(+), 5 deletions(-)

diff --git a/robot/src/main/cpp/ColorWheel.cpp b/robot/src/main/cpp/ColorWheel.cpp
index ee2788e..da2462a 100644
--- a/robot/src/main/cpp/ColorWheel.cpp
+++ b/robot/src/main/cpp/ColorWheel.cpp
@@ -16,6 +16,8 @@ bool OnRed = false;
 
 bool OnTargetColor = false;
 
+double ColorConfidenceTarget = 0.9;
+
 //Notes after some research Saturday: We probably want to use the ColorMatch class to detect color rather than the 
 //ColorSensor. See example code https://github.com/REVrobotics/Color-Sensor-v3-Examples/blob/master/C%2B%2B/Color%20Match/src/main/cpp/Robot.cpp
 static constexpr auto i2cPort = frc::I2C::Port::kOnboard;
@@ -62,7 +64,7 @@ void ColorWheel::RotateToNumber(WPI_TalonSRX *motor, frc::Joystick *joystick){
 
         //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){
+        if (matchedColor == kRedTarget && OnRed == false && colorConfidence >= ColorConfidenceTarget){
             NumSpins = NumSpins+1;
             OnRed = true;
         }
@@ -74,7 +76,9 @@ void ColorWheel::RotateToNumber(WPI_TalonSRX *motor, frc::Joystick *joystick){
 
 
 void ColorWheel::RotateToColor(WPI_TalonSRX *motor, frc::Joystick *joystick, frc::Color *targetcolor){
-    CurrentColor = m_colorSensor.GetColor();
+    double colorConfidence = 0.0;
+    frc::Color detectedColor = m_colorSensor.GetColor();
+    frc::Color matchedColor = m_colorMatcher.MatchClosestColor(detectedColor, colorConfidence); 
     if (spinState == WheelState::NotSpinning && joystick->GetRawButton(1))
     {
         spinState = WheelState::InitSpinning;
@@ -86,9 +90,10 @@ void ColorWheel::RotateToColor(WPI_TalonSRX *motor, frc::Joystick *joystick, frc
     }
     if (spinState == WheelState::Spinning)
     {
-      //  if (!(sensor->GetColor() == targetcolor)){
-
-        //}
+        if (matchedColor == targetcolor && colorConfidence >= ColorConfidenceTarget)){
+            spinState = WheelState::NotSpinning;
+            motor->Set(ControlMode::PercentOutput, 0.2);
+        }
     }
    
 }

From 52d23456584fd85ad99f42a80fb1695967bbd7ec Mon Sep 17 00:00:00 2001
From: erinleonello <erin.leonello@gmail.com>
Date: Thu, 13 Feb 2020 18:28:59 -0500
Subject: [PATCH 09/23] does not compile, trying to fix errors

---
 robot/src/main/cpp/ColorWheel.cpp   | 21 +--------------------
 robot/src/main/include/ColorWheel.h | 14 +++++++-------
 2 files changed, 8 insertions(+), 27 deletions(-)

diff --git a/robot/src/main/cpp/ColorWheel.cpp b/robot/src/main/cpp/ColorWheel.cpp
index da2462a..a9b4209 100644
--- a/robot/src/main/cpp/ColorWheel.cpp
+++ b/robot/src/main/cpp/ColorWheel.cpp
@@ -1,7 +1,6 @@
 #include <frc/Joystick.h>
 #include <ColorWheel.h>
 
-
 enum WheelState{
     NotSpinning,
     InitSpinning,
@@ -92,28 +91,10 @@ void ColorWheel::RotateToColor(WPI_TalonSRX *motor, frc::Joystick *joystick, frc
     {
         if (matchedColor == targetcolor && colorConfidence >= ColorConfidenceTarget)){
             spinState = WheelState::NotSpinning;
-            motor->Set(ControlMode::PercentOutput, 0.2);
+            motor->Set(ControlMode::PercentOutput, 0.0);
         }
     }
    
 }
 
-/*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::TurnToColor(frc::Color targetColor){
-  if(targetColor == GetCurrentColor()){}
-  else{
-      m_colorMotor->Set(ControlMode::PercentOutput, .1);
-  }   
-}*/
diff --git a/robot/src/main/include/ColorWheel.h b/robot/src/main/include/ColorWheel.h
index b69caa6..2fc8f4a 100644
--- a/robot/src/main/include/ColorWheel.h
+++ b/robot/src/main/include/ColorWheel.h
@@ -1,11 +1,11 @@
-    #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>
 
 class ColorWheel{
     public:

From e43de3798639cf5262d9ecd98b5f37fd16677ec6 Mon Sep 17 00:00:00 2001
From: ColeScheller <schelcc@pobox.com>
Date: Tue, 18 Feb 2020 16:44:25 -0500
Subject: [PATCH 10/23]

---
 robot/src/main/cpp/ColorWheel.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/robot/src/main/cpp/ColorWheel.cpp b/robot/src/main/cpp/ColorWheel.cpp
index a9b4209..dc4f211 100644
--- a/robot/src/main/cpp/ColorWheel.cpp
+++ b/robot/src/main/cpp/ColorWheel.cpp
@@ -89,7 +89,7 @@ void ColorWheel::RotateToColor(WPI_TalonSRX *motor, frc::Joystick *joystick, frc
     }
     if (spinState == WheelState::Spinning)
     {
-        if (matchedColor == targetcolor && colorConfidence >= ColorConfidenceTarget)){
+        if (matchedColor == *targetcolor && colorConfidence >= ColorConfidenceTarget){
             spinState = WheelState::NotSpinning;
             motor->Set(ControlMode::PercentOutput, 0.0);
         }

From 6f87b4353d77b7b1185873f39da5dba87666aa43 Mon Sep 17 00:00:00 2001
From: ColeScheller <schelcc@pobox.com>
Date: Tue, 18 Feb 2020 16:44:25 -0500
Subject: [PATCH 11/23] color wheel code by Maura and Erin

---
 robot/src/main/cpp/ColorWheel.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/robot/src/main/cpp/ColorWheel.cpp b/robot/src/main/cpp/ColorWheel.cpp
index a9b4209..dc4f211 100644
--- a/robot/src/main/cpp/ColorWheel.cpp
+++ b/robot/src/main/cpp/ColorWheel.cpp
@@ -89,7 +89,7 @@ void ColorWheel::RotateToColor(WPI_TalonSRX *motor, frc::Joystick *joystick, frc
     }
     if (spinState == WheelState::Spinning)
     {
-        if (matchedColor == targetcolor && colorConfidence >= ColorConfidenceTarget)){
+        if (matchedColor == *targetcolor && colorConfidence >= ColorConfidenceTarget){
             spinState = WheelState::NotSpinning;
             motor->Set(ControlMode::PercentOutput, 0.0);
         }

From d0274ef319c3d5e692a1298431068b4e930bdc7b Mon Sep 17 00:00:00 2001
From: ColeScheller <schelcc@pobox.com>
Date: Tue, 18 Feb 2020 18:32:03 -0500
Subject: [PATCH 12/23] added print color by Maura and Erin

---
 robot/src/main/cpp/ColorWheel.cpp   | 22 ++++++++++++++++++++--
 robot/src/main/cpp/Robot.cpp        |  5 +++--
 robot/src/main/include/ColorWheel.h |  1 +
 3 files changed, 24 insertions(+), 4 deletions(-)

diff --git a/robot/src/main/cpp/ColorWheel.cpp b/robot/src/main/cpp/ColorWheel.cpp
index dc4f211..c03e82f 100644
--- a/robot/src/main/cpp/ColorWheel.cpp
+++ b/robot/src/main/cpp/ColorWheel.cpp
@@ -1,3 +1,4 @@
+#include <frc/smartdashboard/smartdashboard.h>
 #include <frc/Joystick.h>
 #include <ColorWheel.h>
 
@@ -60,7 +61,7 @@ void ColorWheel::RotateToNumber(WPI_TalonSRX *motor, frc::Joystick *joystick){
         double colorConfidence = 0.0;
         frc::Color detectedColor = m_colorSensor.GetColor();
         frc::Color matchedColor = m_colorMatcher.MatchClosestColor(detectedColor, colorConfidence); 
-
+        PrintColors(matchedColor);
         //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){
@@ -71,6 +72,7 @@ void ColorWheel::RotateToNumber(WPI_TalonSRX *motor, frc::Joystick *joystick){
             OnRed = false;
         }
     }
+    
 }
 
 
@@ -97,4 +99,20 @@ void ColorWheel::RotateToColor(WPI_TalonSRX *motor, frc::Joystick *joystick, frc
    
 }
 
-
+void ColorWheel::PrintColors(frc::Color color){
+        if(color == kBlueTarget){
+            frc::SmartDashboard::PutString("color","Blue");
+        }
+        else if(color == kRedTarget){
+            frc::SmartDashboard::PutString("color","Red");
+        }
+        else if(color == kYellowTarget){
+            frc::SmartDashboard::PutString("color","Yellow");
+        }
+        else if(color == kGreenTarget){
+            frc::SmartDashboard::PutString("color","Green");
+        }
+        else{
+            frc::SmartDashboard::PutString("color","no detected color");
+        }
+    }
diff --git a/robot/src/main/cpp/Robot.cpp b/robot/src/main/cpp/Robot.cpp
index a75aa50..0636e55 100644
--- a/robot/src/main/cpp/Robot.cpp
+++ b/robot/src/main/cpp/Robot.cpp
@@ -43,7 +43,7 @@ void Robot::RobotInit() {
   //frc::SmartDashboard::PutNumber("Aimpid",0.1);
 
   joystick_1 = new frc::Joystick(kPrimaryDriverJoystickID);
-  colorWheelmotor = new WPI_TalonSRX(1);
+  colorWheelmotor = new WPI_TalonSRX(4);
   colorWheel = new ColorWheel();
 }
 
@@ -79,12 +79,13 @@ void Robot::AutonomousPeriodic() {
 void Robot::TeleopInit() {}
 
 void Robot::TeleopPeriodic() {
+  
   //Call our color wheel class to execute code determining when to count rotations.
   colorWheel->RotateToNumber(colorWheelmotor, joystick_1);
 }
 
 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 2fc8f4a..7fa0b87 100644
--- a/robot/src/main/include/ColorWheel.h
+++ b/robot/src/main/include/ColorWheel.h
@@ -13,6 +13,7 @@ class ColorWheel{
      void RotateToNumber(WPI_TalonSRX *motor, frc::Joystick *joystick);
      void RotateToColor(WPI_TalonSRX *motor, frc::Joystick *joystick, frc::Color *targetcolor);
     private:
+     void PrintColors(frc::Color color);
 
 };
 

From cb104a851d31f6b552a09e72e2ccd6bb7fb96bc9 Mon Sep 17 00:00:00 2001
From: Grant <grant@abgames.com>
Date: Thu, 20 Feb 2020 16:44:43 -0500
Subject: [PATCH 13/23] erin - Finished rotate to number

---
 robot/src/main/cpp/ColorWheel.cpp | 24 ++++++++++++++++++++++--
 1 file changed, 22 insertions(+), 2 deletions(-)

diff --git a/robot/src/main/cpp/ColorWheel.cpp b/robot/src/main/cpp/ColorWheel.cpp
index c03e82f..be5c93d 100644
--- a/robot/src/main/cpp/ColorWheel.cpp
+++ b/robot/src/main/cpp/ColorWheel.cpp
@@ -1,6 +1,9 @@
 #include <frc/smartdashboard/smartdashboard.h>
 #include <frc/Joystick.h>
 #include <ColorWheel.h>
+#include <iostream>
+#include <string>
+using namespace std;
 
 enum WheelState{
     NotSpinning,
@@ -50,7 +53,7 @@ void ColorWheel::RotateToNumber(WPI_TalonSRX *motor, frc::Joystick *joystick){
     if (spinState == WheelState::Spinning)
     {   
 
-        if (joystick->GetRawButtonPressed(1) || NumSpins>6)
+        if (joystick->GetRawButtonPressed(1) || NumSpins>7)
         {
             motor->Set(ControlMode::PercentOutput,0.0);
             spinState = WheelState::NotSpinning;
@@ -100,7 +103,7 @@ void ColorWheel::RotateToColor(WPI_TalonSRX *motor, frc::Joystick *joystick, frc
 }
 
 void ColorWheel::PrintColors(frc::Color color){
-        if(color == kBlueTarget){
+        /*if(color == kBlueTarget){
             frc::SmartDashboard::PutString("color","Blue");
         }
         else if(color == kRedTarget){
@@ -114,5 +117,22 @@ void ColorWheel::PrintColors(frc::Color color){
         }
         else{
             frc::SmartDashboard::PutString("color","no detected color");
+        }*/
+
+        if(color == kBlueTarget){
+            cout << "Blue" << endl;
+        }
+        else if(color == kRedTarget){
+             cout << "Red" << endl;
+        }
+        else if(color == kYellowTarget){
+             cout << "Yellow" << endl;
+        }
+        else if(color == kGreenTarget){
+             cout << "Green" << endl;
         }
+        else{
+             cout << "no color detected" << endl;
+        }
+
     }

From 7a8b50b80f0253723e3c54d6f30b5407f6ba5905 Mon Sep 17 00:00:00 2001
From: Grant <grant@abgames.com>
Date: Thu, 20 Feb 2020 18:01:23 -0500
Subject: [PATCH 14/23] erin - added rotate to color code

---
 robot/src/main/cpp/ColorWheel.cpp   | 24 +++++++++++++++++-------
 robot/src/main/cpp/Robot.cpp        |  4 ++++
 robot/src/main/include/ColorWheel.h |  2 +-
 robot/src/main/include/Robot.h      |  1 +
 4 files changed, 23 insertions(+), 8 deletions(-)

diff --git a/robot/src/main/cpp/ColorWheel.cpp b/robot/src/main/cpp/ColorWheel.cpp
index be5c93d..74cd1ad 100644
--- a/robot/src/main/cpp/ColorWheel.cpp
+++ b/robot/src/main/cpp/ColorWheel.cpp
@@ -21,6 +21,8 @@ bool OnTargetColor = false;
 
 double ColorConfidenceTarget = 0.9;
 
+int NumColorSamples = 0;
+
 //Notes after some research Saturday: We probably want to use the ColorMatch class to detect color rather than the 
 //ColorSensor. See example code https://github.com/REVrobotics/Color-Sensor-v3-Examples/blob/master/C%2B%2B/Color%20Match/src/main/cpp/Robot.cpp
 static constexpr auto i2cPort = frc::I2C::Port::kOnboard;
@@ -64,7 +66,7 @@ void ColorWheel::RotateToNumber(WPI_TalonSRX *motor, frc::Joystick *joystick){
         double colorConfidence = 0.0;
         frc::Color detectedColor = m_colorSensor.GetColor();
         frc::Color matchedColor = m_colorMatcher.MatchClosestColor(detectedColor, colorConfidence); 
-        PrintColors(matchedColor);
+        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){
@@ -83,7 +85,7 @@ void ColorWheel::RotateToColor(WPI_TalonSRX *motor, frc::Joystick *joystick, frc
     double colorConfidence = 0.0;
     frc::Color detectedColor = m_colorSensor.GetColor();
     frc::Color matchedColor = m_colorMatcher.MatchClosestColor(detectedColor, colorConfidence); 
-    if (spinState == WheelState::NotSpinning && joystick->GetRawButton(1))
+    if (spinState == WheelState::NotSpinning && joystick->GetRawButton(2))
     {
         spinState = WheelState::InitSpinning;
     }
@@ -94,15 +96,23 @@ void ColorWheel::RotateToColor(WPI_TalonSRX *motor, frc::Joystick *joystick, frc
     }
     if (spinState == WheelState::Spinning)
     {
+        PrintColor(matchedColor, colorConfidence);
         if (matchedColor == *targetcolor && colorConfidence >= ColorConfidenceTarget){
+            if (NumColorSamples > 25){
             spinState = WheelState::NotSpinning;
             motor->Set(ControlMode::PercentOutput, 0.0);
+            NumColorSamples = 0;
+            }
+            else {
+                NumColorSamples += 1;
+            }
+
         }
     }
    
 }
 
-void ColorWheel::PrintColors(frc::Color color){
+void ColorWheel::PrintColor(frc::Color color, double colorConfidence){
         /*if(color == kBlueTarget){
             frc::SmartDashboard::PutString("color","Blue");
         }
@@ -120,16 +130,16 @@ void ColorWheel::PrintColors(frc::Color color){
         }*/
 
         if(color == kBlueTarget){
-            cout << "Blue" << endl;
+            cout << "Blue\t" << colorConfidence << endl;
         }
         else if(color == kRedTarget){
-             cout << "Red" << endl;
+             cout << "Red\t" << colorConfidence << endl;
         }
         else if(color == kYellowTarget){
-             cout << "Yellow" << endl;
+             cout << "Yellow\t" << colorConfidence << endl;
         }
         else if(color == kGreenTarget){
-             cout << "Green" << endl;
+             cout << "Green\t" << colorConfidence << endl;
         }
         else{
              cout << "no color detected" << endl;
diff --git a/robot/src/main/cpp/Robot.cpp b/robot/src/main/cpp/Robot.cpp
index 0636e55..5d05151 100644
--- a/robot/src/main/cpp/Robot.cpp
+++ b/robot/src/main/cpp/Robot.cpp
@@ -45,6 +45,8 @@ void Robot::RobotInit() {
   joystick_1 = new frc::Joystick(kPrimaryDriverJoystickID);
   colorWheelmotor = new WPI_TalonSRX(4);
   colorWheel = new ColorWheel();
+  targetcolor = new frc::Color(kGreenTarget);
+  
 }
 
 /**
@@ -82,7 +84,9 @@ void Robot::TeleopPeriodic() {
   
   //Call our color wheel class to execute code determining when to count rotations.
   colorWheel->RotateToNumber(colorWheelmotor, joystick_1);
+  colorWheel->RotateToColor(colorWheelmotor, joystick_1, targetcolor);
 }
+  
 
 void Robot::TestPeriodic() {}
 //std::cout << "In test periodic" << endl;
diff --git a/robot/src/main/include/ColorWheel.h b/robot/src/main/include/ColorWheel.h
index 7fa0b87..1ccd4d4 100644
--- a/robot/src/main/include/ColorWheel.h
+++ b/robot/src/main/include/ColorWheel.h
@@ -13,7 +13,7 @@ class ColorWheel{
      void RotateToNumber(WPI_TalonSRX *motor, frc::Joystick *joystick);
      void RotateToColor(WPI_TalonSRX *motor, frc::Joystick *joystick, frc::Color *targetcolor);
     private:
-     void PrintColors(frc::Color color);
+     void PrintColor(frc::Color color, double colorConfidence);
 
 };
 
diff --git a/robot/src/main/include/Robot.h b/robot/src/main/include/Robot.h
index ea9ad85..ecd27a4 100644
--- a/robot/src/main/include/Robot.h
+++ b/robot/src/main/include/Robot.h
@@ -31,6 +31,7 @@ class Robot : public frc::TimedRobot {
  private:
   // INPUTS
   frc::Joystick *joystick_1;
+  frc::Color *targetcolor;
 
    //BUTTONS
   int const colorWheelButton = 1;

From b90eb9b70ca9ce83d136f1daa1d990a4827e2f35 Mon Sep 17 00:00:00 2001
From: Sherry Vaughan <slvaughan@gmail.com>
Date: Sat, 22 Feb 2020 14:03:21 -0500
Subject: [PATCH 15/23] Vaughan, added smart dashboard calls

---
 robot/src/main/cpp/ColorWheel.cpp | 32 ++++++++++++-------------------
 1 file changed, 12 insertions(+), 20 deletions(-)

diff --git a/robot/src/main/cpp/ColorWheel.cpp b/robot/src/main/cpp/ColorWheel.cpp
index 74cd1ad..89df10f 100644
--- a/robot/src/main/cpp/ColorWheel.cpp
+++ b/robot/src/main/cpp/ColorWheel.cpp
@@ -1,4 +1,4 @@
-#include <frc/smartdashboard/smartdashboard.h>
+#include <frc/smartdashboard/SmartDashboard.h>
 #include <frc/Joystick.h>
 #include <ColorWheel.h>
 #include <iostream>
@@ -40,6 +40,7 @@ ColorWheel::ColorWheel(){
 void ColorWheel::RotateToNumber(WPI_TalonSRX *motor, frc::Joystick *joystick){
 
     //Put in some smart dashboard output to be helpful for debugging
+    frc::SmartDashboard::PutNumber("SpinState", spinState);
     
     if (spinState == WheelState::NotSpinning && joystick->GetRawButtonPressed(1))
     {
@@ -85,6 +86,7 @@ void ColorWheel::RotateToColor(WPI_TalonSRX *motor, frc::Joystick *joystick, frc
     double colorConfidence = 0.0;
     frc::Color detectedColor = m_colorSensor.GetColor();
     frc::Color matchedColor = m_colorMatcher.MatchClosestColor(detectedColor, colorConfidence); 
+    frc::SmartDashboard::PutNumber("SpinState", spinState);
     if (spinState == WheelState::NotSpinning && joystick->GetRawButton(2))
     {
         spinState = WheelState::InitSpinning;
@@ -113,36 +115,26 @@ void ColorWheel::RotateToColor(WPI_TalonSRX *motor, frc::Joystick *joystick, frc
 }
 
 void ColorWheel::PrintColor(frc::Color color, double colorConfidence){
-        /*if(color == kBlueTarget){
-            frc::SmartDashboard::PutString("color","Blue");
+  
+        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 detected color");
-        }*/
-
-        if(color == kBlueTarget){
-            cout << "Blue\t" << colorConfidence << endl;
-        }
-        else if(color == kRedTarget){
-             cout << "Red\t" << colorConfidence << endl;
-        }
-        else if(color == kYellowTarget){
-             cout << "Yellow\t" << colorConfidence << endl;
-        }
-        else if(color == kGreenTarget){
-             cout << "Green\t" << colorConfidence << endl;
-        }
-        else{
-             cout << "no color detected" << endl;
+            frc::SmartDashboard::PutString("color","No color detected");
+            cout << "no color detected" << endl;
         }
 
     }

From eb9e4fc8a92f1cc12669eb134ffc782d20a05d3a Mon Sep 17 00:00:00 2001
From: Grant <grant@abgames.com>
Date: Mon, 24 Feb 2020 17:23:43 -0500
Subject: [PATCH 16/23] erin and maura Check for correct button and activate
 actuator

---
 robot/src/main/cpp/ColorWheel.cpp   | 24 ++++++++++++++++++------
 robot/src/main/include/ColorWheel.h |  2 ++
 2 files changed, 20 insertions(+), 6 deletions(-)

diff --git a/robot/src/main/cpp/ColorWheel.cpp b/robot/src/main/cpp/ColorWheel.cpp
index 89df10f..5c1a36e 100644
--- a/robot/src/main/cpp/ColorWheel.cpp
+++ b/robot/src/main/cpp/ColorWheel.cpp
@@ -23,6 +23,8 @@ double ColorConfidenceTarget = 0.9;
 
 int NumColorSamples = 0;
 
+int CurrentButton = 0;
+
 //Notes after some research Saturday: We probably want to use the ColorMatch class to detect color rather than the 
 //ColorSensor. See example code https://github.com/REVrobotics/Color-Sensor-v3-Examples/blob/master/C%2B%2B/Color%20Match/src/main/cpp/Robot.cpp
 static constexpr auto i2cPort = frc::I2C::Port::kOnboard;
@@ -34,6 +36,7 @@ ColorWheel::ColorWheel(){
     m_colorMatcher.AddColorMatch(kGreenTarget);
     m_colorMatcher.AddColorMatch(kRedTarget);
     m_colorMatcher.AddColorMatch(kYellowTarget);
+    Solenoid = new frc::Solenoid(4);
  }
 
 //Update RotateToNumber to not take in the sensor and get current color from m_colorMatch
@@ -45,22 +48,27 @@ void ColorWheel::RotateToNumber(WPI_TalonSRX *motor, frc::Joystick *joystick){
     if (spinState == WheelState::NotSpinning && joystick->GetRawButtonPressed(1))
     {
         spinState = WheelState::InitSpinning;
+        CurrentButton = 1;
+        Solenoid->Set(true);
 
     }
-    if (spinState == WheelState::InitSpinning) 
+    if (spinState == WheelState::InitSpinning && CurrentButton == 1) 
     {
         NumSpins = 0;
-        motor->Set(ControlMode::PercentOutput,0.2);
+        motor->Set(ControlMode::PercentOutput,0.3);
         spinState = WheelState::Spinning;
     }
-    if (spinState == WheelState::Spinning)
+    if (spinState == WheelState::Spinning && CurrentButton == 1)
     {   
 
         if (joystick->GetRawButtonPressed(1) || NumSpins>7)
         {
             motor->Set(ControlMode::PercentOutput,0.0);
             spinState = WheelState::NotSpinning;
+            CurrentButton = 0;
+            Solenoid->Set(false);
             return;
+        
         }
 
         //A value betwen 0 and 1, 1 being absolute perfect color match
@@ -90,13 +98,15 @@ void ColorWheel::RotateToColor(WPI_TalonSRX *motor, frc::Joystick *joystick, frc
     if (spinState == WheelState::NotSpinning && joystick->GetRawButton(2))
     {
         spinState = WheelState::InitSpinning;
+        CurrentButton = 2;
+        Solenoid->Set(true);
     }
-    if (spinState == WheelState::InitSpinning)
+    if (spinState == WheelState::InitSpinning && CurrentButton == 2)
     {
         spinState = WheelState::Spinning;
-        motor->Set(ControlMode::PercentOutput, 0.2);
+        motor->Set(ControlMode::PercentOutput, 0.3);
     }
-    if (spinState == WheelState::Spinning)
+    if (spinState == WheelState::Spinning && CurrentButton == 2)
     {
         PrintColor(matchedColor, colorConfidence);
         if (matchedColor == *targetcolor && colorConfidence >= ColorConfidenceTarget){
@@ -104,6 +114,8 @@ void ColorWheel::RotateToColor(WPI_TalonSRX *motor, frc::Joystick *joystick, frc
             spinState = WheelState::NotSpinning;
             motor->Set(ControlMode::PercentOutput, 0.0);
             NumColorSamples = 0;
+            CurrentButton = 0;
+            Solenoid->Set(false);
             }
             else {
                 NumColorSamples += 1;
diff --git a/robot/src/main/include/ColorWheel.h b/robot/src/main/include/ColorWheel.h
index 1ccd4d4..8e68fba 100644
--- a/robot/src/main/include/ColorWheel.h
+++ b/robot/src/main/include/ColorWheel.h
@@ -6,6 +6,7 @@
 #include <frc/util/color.h>
 #include <ctre/Phoenix.h>
 #include <RobotUtilities.h>
+#include <frc/Solenoid.h>
 
 class ColorWheel{
     public:
@@ -14,6 +15,7 @@ class ColorWheel{
      void RotateToColor(WPI_TalonSRX *motor, frc::Joystick *joystick, frc::Color *targetcolor);
     private:
      void PrintColor(frc::Color color, double colorConfidence);
+     frc::Solenoid *Solenoid;
 
 };
 

From 4296c1396bd03c8def60d1388c2591be8fd58e08 Mon Sep 17 00:00:00 2001
From: Grant <grant@abgames.com>
Date: Mon, 24 Feb 2020 18:10:57 -0500
Subject: [PATCH 17/23] erin and maura speed and sensor changes

---
 robot/src/main/cpp/ColorWheel.cpp | 10 ++++++----
 1 file changed, 6 insertions(+), 4 deletions(-)

diff --git a/robot/src/main/cpp/ColorWheel.cpp b/robot/src/main/cpp/ColorWheel.cpp
index 5c1a36e..63c2090 100644
--- a/robot/src/main/cpp/ColorWheel.cpp
+++ b/robot/src/main/cpp/ColorWheel.cpp
@@ -55,7 +55,7 @@ void ColorWheel::RotateToNumber(WPI_TalonSRX *motor, frc::Joystick *joystick){
     if (spinState == WheelState::InitSpinning && CurrentButton == 1) 
     {
         NumSpins = 0;
-        motor->Set(ControlMode::PercentOutput,0.3);
+        motor->Set(ControlMode::PercentOutput,0.7);
         spinState = WheelState::Spinning;
     }
     if (spinState == WheelState::Spinning && CurrentButton == 1)
@@ -104,13 +104,13 @@ void ColorWheel::RotateToColor(WPI_TalonSRX *motor, frc::Joystick *joystick, frc
     if (spinState == WheelState::InitSpinning && CurrentButton == 2)
     {
         spinState = WheelState::Spinning;
-        motor->Set(ControlMode::PercentOutput, 0.3);
+        motor->Set(ControlMode::PercentOutput, 0.7);
     }
     if (spinState == WheelState::Spinning && CurrentButton == 2)
     {
         PrintColor(matchedColor, colorConfidence);
         if (matchedColor == *targetcolor && colorConfidence >= ColorConfidenceTarget){
-            if (NumColorSamples > 25){
+            if (NumColorSamples > 5){
             spinState = WheelState::NotSpinning;
             motor->Set(ControlMode::PercentOutput, 0.0);
             NumColorSamples = 0;
@@ -120,7 +120,7 @@ void ColorWheel::RotateToColor(WPI_TalonSRX *motor, frc::Joystick *joystick, frc
             else {
                 NumColorSamples += 1;
             }
-
+ 
         }
     }
    
@@ -148,5 +148,7 @@ void ColorWheel::PrintColor(frc::Color color, double colorConfidence){
             frc::SmartDashboard::PutString("color","No color detected");
             cout << "no color detected" << endl;
         }
+        frc::SmartDashboard::PutNumber("NumSpins", NumSpins);
 
     }
+

From 91eac592bad4863443ea6fc6f489075666b0dabd Mon Sep 17 00:00:00 2001
From: Grant <grant@abgames.com>
Date: Fri, 28 Feb 2020 18:09:13 -0500
Subject: [PATCH 18/23] maura and erin changed initialization of variables

---
 robot/src/main/cpp/ColorWheel.cpp   | 35 ++++++++++++++++-------------
 robot/src/main/cpp/Robot.cpp        |  8 +++----
 robot/src/main/include/ColorWheel.h |  6 ++---
 robot/src/main/include/Robot.h      |  1 +
 4 files changed, 28 insertions(+), 22 deletions(-)

diff --git a/robot/src/main/cpp/ColorWheel.cpp b/robot/src/main/cpp/ColorWheel.cpp
index 63c2090..4845f9d 100644
--- a/robot/src/main/cpp/ColorWheel.cpp
+++ b/robot/src/main/cpp/ColorWheel.cpp
@@ -30,43 +30,48 @@ int CurrentButton = 0;
 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(){
+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);
-    Solenoid = new frc::Solenoid(4);
+    colormotor = motor;
+    colorjoystick = joystick;
+    colorsolenoid = solenoid;
  }
 
 //Update RotateToNumber to not take in the sensor and get current color from m_colorMatch
-void ColorWheel::RotateToNumber(WPI_TalonSRX *motor, frc::Joystick *joystick){
+void ColorWheel::RotateToNumber(){
 
     //Put in some smart dashboard output to be helpful for debugging
     frc::SmartDashboard::PutNumber("SpinState", spinState);
     
-    if (spinState == WheelState::NotSpinning && joystick->GetRawButtonPressed(1))
+    if (spinState == WheelState::NotSpinning && colorjoystick->GetRawButtonPressed(1))
     {
         spinState = WheelState::InitSpinning;
         CurrentButton = 1;
-        Solenoid->Set(true);
+        colorsolenoid->Set(true);
 
     }
     if (spinState == WheelState::InitSpinning && CurrentButton == 1) 
     {
         NumSpins = 0;
-        motor->Set(ControlMode::PercentOutput,0.7);
+        colormotor->Set(ControlMode::PercentOutput,0.7);
         spinState = WheelState::Spinning;
     }
     if (spinState == WheelState::Spinning && CurrentButton == 1)
     {   
 
-        if (joystick->GetRawButtonPressed(1) || NumSpins>7)
+        if (colorjoystick->GetRawButtonPressed(1) || NumSpins>7)
         {
-            motor->Set(ControlMode::PercentOutput,0.0);
+            colormotor->Set(ControlMode::PercentOutput,0.0);
             spinState = WheelState::NotSpinning;
             CurrentButton = 0;
-            Solenoid->Set(false);
+            colorsolenoid->Set(false);
             return;
         
         }
@@ -90,21 +95,21 @@ void ColorWheel::RotateToNumber(WPI_TalonSRX *motor, frc::Joystick *joystick){
 }
 
 
-void ColorWheel::RotateToColor(WPI_TalonSRX *motor, frc::Joystick *joystick, frc::Color *targetcolor){
+void ColorWheel::RotateToColor(frc::Color *targetcolor){
     double colorConfidence = 0.0;
     frc::Color detectedColor = m_colorSensor.GetColor();
     frc::Color matchedColor = m_colorMatcher.MatchClosestColor(detectedColor, colorConfidence); 
     frc::SmartDashboard::PutNumber("SpinState", spinState);
-    if (spinState == WheelState::NotSpinning && joystick->GetRawButton(2))
+    if (spinState == WheelState::NotSpinning && colorjoystick->GetRawButton(2))
     {
         spinState = WheelState::InitSpinning;
         CurrentButton = 2;
-        Solenoid->Set(true);
+        colorsolenoid->Set(true);
     }
     if (spinState == WheelState::InitSpinning && CurrentButton == 2)
     {
         spinState = WheelState::Spinning;
-        motor->Set(ControlMode::PercentOutput, 0.7);
+        colormotor->Set(ControlMode::PercentOutput, 0.7);
     }
     if (spinState == WheelState::Spinning && CurrentButton == 2)
     {
@@ -112,10 +117,10 @@ void ColorWheel::RotateToColor(WPI_TalonSRX *motor, frc::Joystick *joystick, frc
         if (matchedColor == *targetcolor && colorConfidence >= ColorConfidenceTarget){
             if (NumColorSamples > 5){
             spinState = WheelState::NotSpinning;
-            motor->Set(ControlMode::PercentOutput, 0.0);
+            colormotor->Set(ControlMode::PercentOutput, 0.0);
             NumColorSamples = 0;
             CurrentButton = 0;
-            Solenoid->Set(false);
+            colorsolenoid->Set(false);
             }
             else {
                 NumColorSamples += 1;
diff --git a/robot/src/main/cpp/Robot.cpp b/robot/src/main/cpp/Robot.cpp
index 5d05151..ea62e93 100644
--- a/robot/src/main/cpp/Robot.cpp
+++ b/robot/src/main/cpp/Robot.cpp
@@ -44,9 +44,9 @@ void Robot::RobotInit() {
 
   joystick_1 = new frc::Joystick(kPrimaryDriverJoystickID);
   colorWheelmotor = new WPI_TalonSRX(4);
-  colorWheel = new ColorWheel();
+  solenoid = new frc::Solenoid(4);
+  colorWheel = new ColorWheel(colorWheelmotor, joystick_1, solenoid);
   targetcolor = new frc::Color(kGreenTarget);
-  
 }
 
 /**
@@ -83,8 +83,8 @@ void Robot::TeleopInit() {}
 void Robot::TeleopPeriodic() {
   
   //Call our color wheel class to execute code determining when to count rotations.
-  colorWheel->RotateToNumber(colorWheelmotor, joystick_1);
-  colorWheel->RotateToColor(colorWheelmotor, joystick_1, targetcolor);
+  colorWheel->RotateToNumber();
+  colorWheel->RotateToColor(targetcolor);
 }
   
 
diff --git a/robot/src/main/include/ColorWheel.h b/robot/src/main/include/ColorWheel.h
index 8e68fba..1501582 100644
--- a/robot/src/main/include/ColorWheel.h
+++ b/robot/src/main/include/ColorWheel.h
@@ -10,9 +10,9 @@
 
 class ColorWheel{
     public:
-     ColorWheel();
-     void RotateToNumber(WPI_TalonSRX *motor, frc::Joystick *joystick);
-     void RotateToColor(WPI_TalonSRX *motor, frc::Joystick *joystick, frc::Color *targetcolor);
+     ColorWheel(WPI_TalonSRX *motor, frc::Joystick *joystick, frc::Solenoid *solenoid);
+     void RotateToNumber();
+     void RotateToColor(frc::Color *targetcolor);
     private:
      void PrintColor(frc::Color color, double colorConfidence);
      frc::Solenoid *Solenoid;
diff --git a/robot/src/main/include/Robot.h b/robot/src/main/include/Robot.h
index ecd27a4..975d111 100644
--- a/robot/src/main/include/Robot.h
+++ b/robot/src/main/include/Robot.h
@@ -39,5 +39,6 @@ class Robot : public frc::TimedRobot {
   //Color wheel class
   WPI_TalonSRX *colorWheelmotor;
   ColorWheel* colorWheel;
+  frc::Solenoid *solenoid;
 
 };

From fca2ff1c1f2c5cfc4351227171712c5c0fb7d8a9 Mon Sep 17 00:00:00 2001
From: Grant <grant@abgames.com>
Date: Sat, 29 Feb 2020 14:52:53 -0500
Subject: [PATCH 19/23] erin and maura solenoid controls and confidence

---
 robot/src/main/cpp/ColorWheel.cpp   | 27 +++++++++++++++++++++------
 robot/src/main/cpp/Robot.cpp        |  1 +
 robot/src/main/include/ColorWheel.h |  1 +
 3 files changed, 23 insertions(+), 6 deletions(-)

diff --git a/robot/src/main/cpp/ColorWheel.cpp b/robot/src/main/cpp/ColorWheel.cpp
index 4845f9d..916290e 100644
--- a/robot/src/main/cpp/ColorWheel.cpp
+++ b/robot/src/main/cpp/ColorWheel.cpp
@@ -42,7 +42,21 @@ ColorWheel::ColorWheel(WPI_TalonSRX *motor, frc::Joystick *joystick, frc::Soleno
     colormotor = motor;
     colorjoystick = joystick;
     colorsolenoid = solenoid;
- }
+ } 
+
+void ColorWheel::ControlSolenoid(){
+    if (colorjoystick->GetRawButtonPressed(3)){
+        bool isup = colorsolenoid->Get();
+
+        if(isup == true){
+            colorsolenoid->Set(false);
+        }
+        else{
+            colorsolenoid->Set(true);
+        }
+    }
+
+}
 
 //Update RotateToNumber to not take in the sensor and get current color from m_colorMatch
 void ColorWheel::RotateToNumber(){
@@ -54,7 +68,7 @@ void ColorWheel::RotateToNumber(){
     {
         spinState = WheelState::InitSpinning;
         CurrentButton = 1;
-        colorsolenoid->Set(true);
+        //colorsolenoid->Set(true);
 
     }
     if (spinState == WheelState::InitSpinning && CurrentButton == 1) 
@@ -71,7 +85,7 @@ void ColorWheel::RotateToNumber(){
             colormotor->Set(ControlMode::PercentOutput,0.0);
             spinState = WheelState::NotSpinning;
             CurrentButton = 0;
-            colorsolenoid->Set(false);
+            //colorsolenoid->Set(false);
             return;
         
         }
@@ -104,12 +118,12 @@ void ColorWheel::RotateToColor(frc::Color *targetcolor){
     {
         spinState = WheelState::InitSpinning;
         CurrentButton = 2;
-        colorsolenoid->Set(true);
+        //colorsolenoid->Set(true);
     }
     if (spinState == WheelState::InitSpinning && CurrentButton == 2)
     {
         spinState = WheelState::Spinning;
-        colormotor->Set(ControlMode::PercentOutput, 0.7);
+        colormotor->Set(ControlMode::PercentOutput, 0.2);
     }
     if (spinState == WheelState::Spinning && CurrentButton == 2)
     {
@@ -120,7 +134,7 @@ void ColorWheel::RotateToColor(frc::Color *targetcolor){
             colormotor->Set(ControlMode::PercentOutput, 0.0);
             NumColorSamples = 0;
             CurrentButton = 0;
-            colorsolenoid->Set(false);
+            //colorsolenoid->Set(false);
             }
             else {
                 NumColorSamples += 1;
@@ -154,6 +168,7 @@ void ColorWheel::PrintColor(frc::Color color, double colorConfidence){
             cout << "no color detected" << endl;
         }
         frc::SmartDashboard::PutNumber("NumSpins", NumSpins);
+        frc::SmartDashboard::PutNumber("Confidence", colorConfidence);
 
     }
 
diff --git a/robot/src/main/cpp/Robot.cpp b/robot/src/main/cpp/Robot.cpp
index ea62e93..08ef2b3 100644
--- a/robot/src/main/cpp/Robot.cpp
+++ b/robot/src/main/cpp/Robot.cpp
@@ -85,6 +85,7 @@ void Robot::TeleopPeriodic() {
   //Call our color wheel class to execute code determining when to count rotations.
   colorWheel->RotateToNumber();
   colorWheel->RotateToColor(targetcolor);
+  colorWheel->ControlSolenoid();
 }
   
 
diff --git a/robot/src/main/include/ColorWheel.h b/robot/src/main/include/ColorWheel.h
index 1501582..d3ba352 100644
--- a/robot/src/main/include/ColorWheel.h
+++ b/robot/src/main/include/ColorWheel.h
@@ -13,6 +13,7 @@ class ColorWheel{
      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;

From dcdf634ae7dc6afbf72cffdc1b21e046c213e9e4 Mon Sep 17 00:00:00 2001
From: ColeScheller <schelcc@pobox.com>
Date: Tue, 3 Mar 2020 18:27:18 -0500
Subject: [PATCH 20/23] erin does not build work in progress

---
 robot/src/main/cpp/ColorWheel.cpp   | 118 ++++++++++++++++++++++------
 robot/src/main/include/ColorWheel.h |   3 +
 2 files changed, 97 insertions(+), 24 deletions(-)

diff --git a/robot/src/main/cpp/ColorWheel.cpp b/robot/src/main/cpp/ColorWheel.cpp
index 916290e..a177cd9 100644
--- a/robot/src/main/cpp/ColorWheel.cpp
+++ b/robot/src/main/cpp/ColorWheel.cpp
@@ -13,20 +13,21 @@ enum WheelState{
 WheelState spinState = WheelState::NotSpinning;
 
 frc::Color CurrentColor;
+
+frc::Color PreviousColor;
+
 int NumSpins = 0;
 
 bool OnRed = false;
 
-bool OnTargetColor = false;
-
 double ColorConfidenceTarget = 0.9;
 
 int NumColorSamples = 0;
 
 int CurrentButton = 0;
 
-//Notes after some research Saturday: We probably want to use the ColorMatch class to detect color rather than the 
-//ColorSensor. See example code https://github.com/REVrobotics/Color-Sensor-v3-Examples/blob/master/C%2B%2B/Color%20Match/src/main/cpp/Robot.cpp
+
+
 static constexpr auto i2cPort = frc::I2C::Port::kOnboard;
 rev::ColorSensorV3 m_colorSensor(i2cPort);
 rev::ColorMatch m_colorMatcher;
@@ -44,6 +45,62 @@ ColorWheel::ColorWheel(WPI_TalonSRX *motor, frc::Joystick *joystick, frc::Soleno
     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(3)){
         bool isup = colorsolenoid->Get();
@@ -58,17 +115,12 @@ void ColorWheel::ControlSolenoid(){
 
 }
 
-//Update RotateToNumber to not take in the sensor and get current color from m_colorMatch
-void ColorWheel::RotateToNumber(){
 
-    //Put in some smart dashboard output to be helpful for debugging
-    frc::SmartDashboard::PutNumber("SpinState", spinState);
-    
+void ColorWheel::RotateToNumber(){
     if (spinState == WheelState::NotSpinning && colorjoystick->GetRawButtonPressed(1))
     {
         spinState = WheelState::InitSpinning;
         CurrentButton = 1;
-        //colorsolenoid->Set(true);
 
     }
     if (spinState == WheelState::InitSpinning && CurrentButton == 1) 
@@ -85,7 +137,6 @@ void ColorWheel::RotateToNumber(){
             colormotor->Set(ControlMode::PercentOutput,0.0);
             spinState = WheelState::NotSpinning;
             CurrentButton = 0;
-            //colorsolenoid->Set(false);
             return;
         
         }
@@ -97,6 +148,7 @@ void ColorWheel::RotateToNumber(){
         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;
@@ -111,6 +163,7 @@ void ColorWheel::RotateToNumber(){
 
 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);
@@ -118,35 +171,51 @@ void ColorWheel::RotateToColor(frc::Color *targetcolor){
     {
         spinState = WheelState::InitSpinning;
         CurrentButton = 2;
-        //colorsolenoid->Set(true);
     }
     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 == *targetcolor && colorConfidence >= ColorConfidenceTarget){
-            if (NumColorSamples > 5){
-            spinState = WheelState::NotSpinning;
-            colormotor->Set(ControlMode::PercentOutput, 0.0);
-            NumColorSamples = 0;
-            CurrentButton = 0;
-            //colorsolenoid->Set(false);
-            }
-            else {
-                NumColorSamples += 1;
+        if (!(matchedColor == CurrentColor))
+        {
+            if (matchedColor == getNextColor(PreviousColor))
+            {
+                if (matchedColor == *targetcolor && 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::PrintColor(frc::Color color, double colorConfidence){
-  
         if(color == kBlueTarget){
             frc::SmartDashboard::PutString("color","Blue"); 
             cout << "Blue\t" << colorConfidence << endl;
@@ -169,6 +238,7 @@ void ColorWheel::PrintColor(frc::Color color, double colorConfidence){
         }
         frc::SmartDashboard::PutNumber("NumSpins", NumSpins);
         frc::SmartDashboard::PutNumber("Confidence", colorConfidence);
+        frc::SmartDashboard::PutNumber("SpinState", spinState);
 
     }
 
diff --git a/robot/src/main/include/ColorWheel.h b/robot/src/main/include/ColorWheel.h
index d3ba352..bb30b90 100644
--- a/robot/src/main/include/ColorWheel.h
+++ b/robot/src/main/include/ColorWheel.h
@@ -17,6 +17,9 @@ class ColorWheel{
     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);
 
 };
 

From f41cb2379f3a63303f2a037087f3a9b47fa531fc Mon Sep 17 00:00:00 2001
From: ColeScheller <schelcc@pobox.com>
Date: Thu, 5 Mar 2020 16:20:49 -0500
Subject: [PATCH 21/23] erin - builds but not tested

---
 robot/src/main/cpp/ColorWheel.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/robot/src/main/cpp/ColorWheel.cpp b/robot/src/main/cpp/ColorWheel.cpp
index a177cd9..ccc4b7d 100644
--- a/robot/src/main/cpp/ColorWheel.cpp
+++ b/robot/src/main/cpp/ColorWheel.cpp
@@ -163,7 +163,7 @@ void ColorWheel::RotateToNumber(){
 
 void ColorWheel::RotateToColor(frc::Color *targetcolor){
     double colorConfidence = 0.0;
-    frc::Color spinTargetColor = getSpinTargetColor(&targetcolor);
+    frc::Color spinTargetColor = getSpinTargetColor(*targetcolor);
     frc::Color detectedColor = m_colorSensor.GetColor();
     frc::Color matchedColor = m_colorMatcher.MatchClosestColor(detectedColor, colorConfidence); 
     frc::SmartDashboard::PutNumber("SpinState", spinState);
@@ -189,7 +189,7 @@ void ColorWheel::RotateToColor(frc::Color *targetcolor){
         {
             if (matchedColor == getNextColor(PreviousColor))
             {
-                if (matchedColor == *targetcolor && colorConfidence >= ColorConfidenceTarget)
+                if (matchedColor == spinTargetColor && colorConfidence >= ColorConfidenceTarget)
                 {
                     if (NumColorSamples > 1)
                     {

From 96e8709356a5b1ba185e4aca4241dc1fde96b2b7 Mon Sep 17 00:00:00 2001
From: ColeScheller <schelcc@pobox.com>
Date: Thu, 5 Mar 2020 18:27:09 -0500
Subject: [PATCH 22/23] erin - tested but doesn't work

---
 robot/src/main/cpp/ColorWheel.cpp       | 17 +++++++++---
 robot/src/main/cpp/Robot.cpp            |  4 +--
 robot/src/main/include/RobotUtilities.h | 37 ++++++++++++++++++++++---
 3 files changed, 48 insertions(+), 10 deletions(-)

diff --git a/robot/src/main/cpp/ColorWheel.cpp b/robot/src/main/cpp/ColorWheel.cpp
index ccc4b7d..0e944d3 100644
--- a/robot/src/main/cpp/ColorWheel.cpp
+++ b/robot/src/main/cpp/ColorWheel.cpp
@@ -3,6 +3,7 @@
 #include <ColorWheel.h>
 #include <iostream>
 #include <string>
+#include <RobotUtilities.h>
 using namespace std;
 
 enum WheelState{
@@ -26,6 +27,11 @@ 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;
@@ -102,7 +108,7 @@ frc::Color ColorWheel::getSpinTargetColor(frc::Color color){
 
 }
 void ColorWheel::ControlSolenoid(){
-    if (colorjoystick->GetRawButtonPressed(3)){
+    if (colorjoystick->GetRawButtonPressed(1)){
         bool isup = colorsolenoid->Get();
 
         if(isup == true){
@@ -117,7 +123,7 @@ void ColorWheel::ControlSolenoid(){
 
 
 void ColorWheel::RotateToNumber(){
-    if (spinState == WheelState::NotSpinning && colorjoystick->GetRawButtonPressed(1))
+    if (spinState == WheelState::NotSpinning && colorjoystick->GetRawButtonPressed (2))
     {
         spinState = WheelState::InitSpinning;
         CurrentButton = 1;
@@ -157,7 +163,7 @@ void ColorWheel::RotateToNumber(){
             OnRed = false;
         }
     }
-    
+    cout <<"Hi***************************"<< endl;
 }
 
 
@@ -167,7 +173,7 @@ void ColorWheel::RotateToColor(frc::Color *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(2))
+    if (spinState == WheelState::NotSpinning && colorjoystick->GetRawButton(3))
     {
         spinState = WheelState::InitSpinning;
         CurrentButton = 2;
@@ -212,6 +218,7 @@ void ColorWheel::RotateToColor(frc::Color *targetcolor){
         }
         
     }
+    cout <<"Hi***************************"<< endl;
    
 }
 
@@ -237,6 +244,8 @@ void ColorWheel::PrintColor(frc::Color color, double colorConfidence){
             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 08ef2b3..37a71fb 100644
--- a/robot/src/main/cpp/Robot.cpp
+++ b/robot/src/main/cpp/Robot.cpp
@@ -43,8 +43,8 @@ void Robot::RobotInit() {
   //frc::SmartDashboard::PutNumber("Aimpid",0.1);
 
   joystick_1 = new frc::Joystick(kPrimaryDriverJoystickID);
-  colorWheelmotor = new WPI_TalonSRX(4);
-  solenoid = new frc::Solenoid(4);
+  colorWheelmotor = new WPI_TalonSRX(9);
+  solenoid = new frc::Solenoid(1);
   colorWheel = new ColorWheel(colorWheelmotor, joystick_1, solenoid);
   targetcolor = new frc::Color(kGreenTarget);
 }
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

From 204ee9fb2af7f5c1ed8fce0d75c333444941bb43 Mon Sep 17 00:00:00 2001
From: Grant <grant@abgames.com>
Date: Mon, 9 Mar 2020 17:53:55 -0400
Subject: [PATCH 23/23] erin - chaged motor identifiers

---
 robot/src/main/cpp/ColorWheel.cpp | 11 +++++------
 robot/src/main/cpp/Robot.cpp      |  4 ++--
 2 files changed, 7 insertions(+), 8 deletions(-)

diff --git a/robot/src/main/cpp/ColorWheel.cpp b/robot/src/main/cpp/ColorWheel.cpp
index 0e944d3..6f56f02 100644
--- a/robot/src/main/cpp/ColorWheel.cpp
+++ b/robot/src/main/cpp/ColorWheel.cpp
@@ -109,6 +109,7 @@ frc::Color ColorWheel::getSpinTargetColor(frc::Color color){
 }
 void ColorWheel::ControlSolenoid(){
     if (colorjoystick->GetRawButtonPressed(1)){
+        cout <<"button 1 pressed"<< endl;
         bool isup = colorsolenoid->Get();
 
         if(isup == true){
@@ -125,6 +126,7 @@ void ColorWheel::ControlSolenoid(){
 void ColorWheel::RotateToNumber(){
     if (spinState == WheelState::NotSpinning && colorjoystick->GetRawButtonPressed (2))
     {
+        cout <<"button 2 pressed"<< endl;
         spinState = WheelState::InitSpinning;
         CurrentButton = 1;
 
@@ -136,9 +138,8 @@ void ColorWheel::RotateToNumber(){
         spinState = WheelState::Spinning;
     }
     if (spinState == WheelState::Spinning && CurrentButton == 1)
-    {   
-
-        if (colorjoystick->GetRawButtonPressed(1) || NumSpins>7)
+    { 
+        if (colorjoystick->GetRawButtonPressed(2) || NumSpins>7)
         {
             colormotor->Set(ControlMode::PercentOutput,0.0);
             spinState = WheelState::NotSpinning;
@@ -163,7 +164,6 @@ void ColorWheel::RotateToNumber(){
             OnRed = false;
         }
     }
-    cout <<"Hi***************************"<< endl;
 }
 
 
@@ -177,6 +177,7 @@ void ColorWheel::RotateToColor(frc::Color *targetcolor){
     {
         spinState = WheelState::InitSpinning;
         CurrentButton = 2;
+        cout <<"button 3 pressed"<< endl;
     }
     if (spinState == WheelState::InitSpinning && CurrentButton == 2)
     {
@@ -218,8 +219,6 @@ void ColorWheel::RotateToColor(frc::Color *targetcolor){
         }
         
     }
-    cout <<"Hi***************************"<< endl;
-   
 }
 
 void ColorWheel::PrintColor(frc::Color color, double colorConfidence){
diff --git a/robot/src/main/cpp/Robot.cpp b/robot/src/main/cpp/Robot.cpp
index 37a71fb..08ef2b3 100644
--- a/robot/src/main/cpp/Robot.cpp
+++ b/robot/src/main/cpp/Robot.cpp
@@ -43,8 +43,8 @@ void Robot::RobotInit() {
   //frc::SmartDashboard::PutNumber("Aimpid",0.1);
 
   joystick_1 = new frc::Joystick(kPrimaryDriverJoystickID);
-  colorWheelmotor = new WPI_TalonSRX(9);
-  solenoid = new frc::Solenoid(1);
+  colorWheelmotor = new WPI_TalonSRX(4);
+  solenoid = new frc::Solenoid(4);
   colorWheel = new ColorWheel(colorWheelmotor, joystick_1, solenoid);
   targetcolor = new frc::Color(kGreenTarget);
 }