Added an initialize folder in include also set up the color sorter code

include/EZ-Template/api.hpp

@@ -9,6 +9,7 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/.
 #include "EZ-Template/PID.hpp"
 #include "EZ-Template/auton.hpp"
 #include "EZ-Template/auton_selector.hpp"
+#include "EZ-Template/color_sorter.hpp"
 #include "EZ-Template/drive/drive.hpp"
 #include "EZ-Template/piston.hpp"
 #include "EZ-Template/sdcard.hpp"

include/EZ-Template/color_sorter.hpp

@@ -0,0 +1,45 @@
+#pragma once
+
+#include "api.h"
+#include "pros/adi.hpp"
+#include "pros/colors.hpp"
+
+namespace ez {
+    
+    enum wanted_color {
+        red,
+        blue,
+        yellow,
+        green,
+        purple
+    };
+
+    enum control_period {
+        driver,
+        auton
+    };
+
+    class color_sorter {
+        public:
+        
+
+        color_sorter();
+
+        /**
+        *
+        * \param sensorNum
+        *        the sensor we want to use for the reading
+        * \param period
+        *        the current control period where in
+        * \param selected_color
+        *        the color we want to look for
+        * \param looped
+        *        do we want to continusly check for that color
+        */
+        color_sorter(int sensorNum, control_period period, wanted_color selected_color, bool looped);
+        
+        void sensor(int sensorAmount);
+
+        void DisplayColor();
+    };
+};

include/main.h

@@ -42,6 +42,7 @@
 // #include "okapi/api.hpp"
 // #include "pros/api_legacy.h"
 #include "EZ-Template/api.hpp"
+#include "setup/initialize.hpp"
 
 // More includes here...
 #include "autons.hpp"

include/setup/initialize.hpp

@@ -0,0 +1,79 @@
+#include "main.h"
+#include "EZ-Template/api.hpp"
+#include "api.h"
+
+#pragma once
+
+#ifndef CONFIG_H
+#define CONFIG_H
+
+extern bool gotRed;
+extern bool gotBlue;
+extern double currentColor;
+
+extern Drive chassis;
+
+// Declares the left side motors
+extern pros::Motor FrontLeft;
+extern pros::Motor MiddleLeft;
+extern pros::Motor BackLeft;
+
+// Declares the right side motors
+extern pros::Motor FrontRight;
+extern pros::Motor MiddleRight;
+extern pros::Motor BackRight;
+
+// Declares the left and right  drive motor groups.
+extern pros::MotorGroup RightSide;
+extern pros::MotorGroup LeftSide;
+
+// Declares all the drive motors as a group.
+extern pros::MotorGroup FullDrive;
+
+// Declaring the Intake motors.
+inline pros::Motor stage1({14});
+inline pros::Motor stage2({16});
+inline pros::Motor stage3({12});
+
+//
+inline ez::Piston ramp({'H',true});
+
+// Declaring the piston used to guide the balls into the basket.
+inline ez::Piston deScorer({'G',false});
+
+// Declaring the piston used to get the balls out of the tube.
+inline ez::Piston matchLoader({'F',false});
+
+//
+inline pros::Optical colorSensorOne({20});
+inline pros::Optical colorSensorTwo({11});
+inline pros::Optical colorSensorThree({18});
+
+//
+inline pros::Distance frontDistance({5});
+
+//inline pros::MotorGroup fillingGroup({20, 17, 18});
+//inline pros::MotorGroup scoringGroup({20, 17, 18, 19});
+
+// Function used for the Intake control
+void IntakePresets();
+void ToggleButtons();
+
+// Function used for the color sorter
+void ColorDisplay();
+void CheckRed(std::string Sensor, std::string controlPeriod, bool loop);
+void CheckBlue(std::string Sensor, std::string controlPeriod, bool loop);
+void ColorDetection(std::string wantedColor, bool checkForColor);
+
+// Auton voids
+void leftMain(std::string side);
+void rightMain(std::string side);
+void Skills();
+void BlueRight();
+void BlueLeft();
+void RedRight();
+void RedLeft();
+void testMain(std::string side);
+void Test();
+
+#endif

src/EZ-Template/color_sorter.cpp

@@ -0,0 +1,26 @@
+#include "main.h"
+
+using namespace ez;
+
+color_sorter::color_sorter(int sensorNum, control_period period, wanted_color selected_color, bool looped) {
+    while (looped == true) {
+      if (sensorNum == 1 && period == auton) {
+        pros::delay(100);
+        while (colorSensorOne.get_hue() <= 20 || colorSensorOne.get_hue() >= 340) {
+          //Code you want to execute goes here
+
+          looped = false;
+        }
+
+      }
+      else if (sensorNum == 2 && period== auton) {
+        pros::delay(100);
+        while (colorSensorTwo.get_hue() >= 160 && colorSensorTwo.get_hue() <= 280) {
+          //Code you want to execute goes here
+
+          looped = false;
+        }
+        
+      }
+    }
+}

src/main.cpp

@@ -20,7 +20,7 @@ ez::Drive chassis(
 //  - you should get positive values on the encoders going FORWARD and RIGHT
 // - `2.75` is the wheel diameter
 // - `4.0` is the distance from the center of the wheel to the center of the robot
-// ez::tracking_wheel horiz_tracker(8, 2.75, 4.0);  // This tracking wheel is perpendicular to the drive wheels
+ ez::tracking_wheel horiz_tracker(8, 2.75, 4.0);  // This tracking wheel is perpendicular to the drive wheels
 // ez::tracking_wheel vert_tracker(9, 2.75, 4.0);   // This tracking wheel is parallel to the drive wheels
 
 /**