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/EZ-Template/util.hpp

@@ -26,9 +26,9 @@ extern pros::Controller master;
 namespace ez {
 
 /**
- * Prints our branding all over your pros terminal.
+ * Prints your chooses logo in the terminal
  */
-void ez_template_print();
+void logo_print();
 
 /**
  * Prints to the brain screen in one string.

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/EZ-Template/util.cpp

@@ -14,18 +14,15 @@ pros::Controller master(pros::E_CONTROLLER_MASTER);
 namespace ez {
 int mode = DISABLE;
 
-void ez_template_print() {
+void logo_print() {
   std::cout << R"(
 
+       _________   ________ ____  __
+      <  <  /__ \ / ____/ // /\ \/ /
+      / // /__/ //___ \/ // /_ \  / 
+     / // // __/____/ /__  __/ / /  
+    /_//_//____/_____/  /_/   /_/  
 
-    _____ ______   _____                    _       _
-   |  ___|___  /  |_   _|                  | |     | |
-   | |__    / /_____| | ___ _ __ ___  _ __ | | __ _| |_ ___
-   |  __|  / /______| |/ _ \ '_ ` _ \| '_ \| |/ _` | __/ _ \
-   | |___./ /___    | |  __/ | | | | | |_) | | (_| | ||  __/
-   \____/\_____/    \_/\___|_| |_| |_| .__/|_|\__,_|\__\___|
-                                     | |
-                                     |_|
 )" << '\n';
 }
 std::string get_last_word(std::string text) {

src/autons.cpp

@@ -48,6 +48,11 @@ void default_constants() {
   chassis.pid_angle_behavior_set(ez::shortest);  // Changes the default behavior for turning, this defaults it to the shortest path there
 }
 
+///
+// Custom auton routes
+//
+
+
 ///
 // Drive Example
 ///

src/main.cpp

@@ -12,15 +12,15 @@ ez::Drive chassis(
     {11, 15, 16, 17},  // Right Chassis Ports (negative port will reverse it!)
 
     21,      // IMU Port
-    4.125,   // Wheel Diameter (Remember, 4" wheels without screw holes are actually 4.125!)
+    3.25,   // Wheel Diameter (Remember, 4" wheels without screw holes are actually 4.125!)
-    420.0);  // Wheel RPM = cartridge * (motor gear / wheel gear)
+    360.0);  // Wheel RPM = cartridge * (motor gear / wheel gear)
 
 // Uncomment the trackers you're using here!
 // - `8` and `9` are smart ports (making these negative will reverse the sensor)
 //  - 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
 
 /**
@@ -30,8 +30,8 @@ ez::Drive chassis(
  * to keep execution time for this mode under a few seconds.
  */
 void initialize() {
-  // Print our branding over your terminal :D
+  // Prints your logo in the terminal
-  ez::ez_template_print();
+  ez::logo_print();
 
   pros::delay(500);  // Stop the user from doing anything while legacy ports configure
 
@@ -58,7 +58,10 @@ void initialize() {
 
   // Autonomous Selector using LLEMU
   ez::as::auton_selector.autons_add({
-      {"Drive\n\nDrive forward and come back", drive_example},
+      //Custom autonomouse routes go here.
+
+
+      /*{"Drive\n\nDrive forward and come back", drive_example},
       {"Turn\n\nTurn 3 times.", turn_example},
       {"Drive and Turn\n\nDrive forward, turn, come back", drive_and_turn},
       {"Drive and Turn\n\nSlow down during drive", wait_until_change_speed},
@@ -71,7 +74,7 @@ void initialize() {
       {"Pure Pursuit Wait Until\n\nGo to (24, 24) but start running an intake once the robot passes (12, 24)", odom_pure_pursuit_wait_until_example},
       {"Boomerang\n\nGo to (0, 24, 45) then come back to (0, 0, 0)", odom_boomerang_example},
       {"Boomerang Pure Pursuit\n\nGo to (0, 24, 45) on the way to (24, 24) then come back to (0, 0, 0)", odom_boomerang_injected_pure_pursuit_example},
-      {"Measure Offsets\n\nThis will turn the robot a bunch of times and calculate your offsets for your tracking wheels.", measure_offsets},
+      {"Measure Offsets\n\nThis will turn the robot a bunch of times and calculate your offsets for your tracking wheels.", measure_offsets},*/
   });
 
   // Initialize chassis and auton selector
@@ -169,10 +172,10 @@ void ez_screen_task() {
                            1);  // Don't override the top Page line
 
           // Display all trackers that are being used
-          screen_print_tracker(chassis.odom_tracker_left, "l", 4);
+          screen_print_tracker(chassis.odom_tracker_left, "L", 4);
-          screen_print_tracker(chassis.odom_tracker_right, "r", 5);
+          screen_print_tracker(chassis.odom_tracker_right, "R", 5);
-          screen_print_tracker(chassis.odom_tracker_back, "b", 6);
+          screen_print_tracker(chassis.odom_tracker_back, "B", 6);
-          screen_print_tracker(chassis.odom_tracker_front, "f", 7);
+          screen_print_tracker(chassis.odom_tracker_front, "F", 7);
         }
       }
     }
@@ -241,14 +244,16 @@ void ez_template_extras() {
  */
 void opcontrol() {
   // This is preference to what you like to drive on
-  chassis.drive_brake_set(MOTOR_BRAKE_COAST);
+  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);
 
   while (true) {
     // Gives you some extras to make EZ-Template ezier
     ez_template_extras();
 
-    chassis.opcontrol_tank();  // Tank control
+    // Place to call custom functions for driver control
-    // chassis.opcontrol_arcade_standard(ez::SPLIT);   // Standard split arcade
+
+    // chassis.opcontrol_tank();  // Tank control
+    chassis.opcontrol_arcade_standard(ez::SPLIT);   // Standard split arcade
     // chassis.opcontrol_arcade_standard(ez::SINGLE);  // Standard single arcade
     // chassis.opcontrol_arcade_flipped(ez::SPLIT);    // Flipped split arcade
     // chassis.opcontrol_arcade_flipped(ez::SINGLE);   // Flipped single arcade