*DO NOT MERGE* Manual Debug on SerialController

configuration.json

@@ -1,9 +1,9 @@
 {
-    "version": "0.1.1",
+    "version": "0.2.1",
     "configurations": [
         {
-            "name": "Shooter_Big_Bot",
-            "controller_config": "AidanJoeShmo",
+            "name": "Big_Boy",
+            "controller_config": "DanTheMan",
             "controller_deadzone": 5,
             "mechanical_system": {
                 "starting_angle": 90,
@@ -17,7 +17,10 @@
                     "motor_ports": [1, 2, 3, 4],
                     "gear_ratio": "RED",
                     "cf": 1.0,
-                    "radius": 1.0
+                    "radius": 1.0,
+                    "wheel_base": 12.0,
+                    "k_pos": [8.5, 1.95, 0.8],
+                    "k_theta": [0.95, 0.08, 0.15]
                 },
                 "intake": {
                     "config": "IntakeUni",
@@ -59,7 +62,10 @@
                     "reverse_motors": [1, 1, 0, 0, 0, 1],
                     "gear_ratio": "BLUE",
                     "cf": 0.667,
-                    "radius": 2.0
+                    "radius": 2.0,
+                    "wheel_base": 12.0,
+                    "k_pos": [8.5, 1.95, 0.8],
+                    "k_theta": [0.95, 0.08, 0.15]
                 },
                 "roller": {
                     "config": "RollerDuo",
@@ -91,11 +97,14 @@
                 "imu": 11,
                 "drive": {
                     "config": "tank_drive_std",
-                    "motor_ports": [4, 5, 6, 1, 2, 3],
-                    "reverse_motors": [1, 1, 0, 0, 0, 1],
+                    "motor_ports": [1, 2, 3, 8, 9, 10],
+                    "reverse_motors": [1, 0, 1, 0, 1, 0],
                     "gear_ratio": "BLUE",
-                    "cf": 0.667,
-                    "radius": 1.5
+                    "cf": 0.5573,
+                    "radius": 1.5,
+                    "wheel_base": 12.0,
+                    "k_pos": [9.2, 0.0001, 1.8],
+                    "k_theta": [3.5, 0.002, 0.4]
                 },
                 "intake": {
                     "config": "IntakeUni",
@@ -106,17 +115,61 @@
                 },
                 "roller": {
                     "config": "RollerUni",
-                    "motor_ports": [19],
+                    "motor_ports": [6],
                     "reverse_motors": [0],
                     "gear_ratio": "GREEN",
                     "max_speed": 120
                 },
                 "shooter": {
                     "config": "ShooterFlyWheelSB",
-                    "motor_ports": [8, 9],
-                    "reverse_motors": [0, 0],
+                    "motor_ports": [19, 20],
+                    "reverse_motors": [1, 1],
                     "gear_ratio": "SpecialFlyWheel",
                     "max_speed": 127
+                },
+                "magazine": {
+                    "incPorts": "DE",
+                    "inc_thresholg": [0.64, 0.64],
+                    "decPorts": "B",
+                    "dec_threshold": [0.64]
+                }
+            }
+        },
+        {
+            "name": "Rocky",
+            "controller_config": "NateIsTank",
+            "controller_deadzone": 5,
+            "mechanical_system": {
+                "starting_angle": 90,
+                "starting_position": {
+                    "x": 0,
+                    "y": 0
+                },
+                "imu": 11,
+                "drive": {
+                    "config": "tank_drive_quad",
+                    "motor_ports": [4, 5, 6, 1, 2, 3],
+                    "reverse_motors": [1, 1, 0, 0, 0, 1],
+                    "gear_ratio": "BLUE",
+                    "cf": 1,
+                    "radius": 4.0,
+                    "wheel_base": 12.0,
+                    "k_pos": [8.5, 1.95, 0.8],
+                    "k_theta": [0.95, 0.05, 0.15]
+                },
+                "intake": {
+                    "config": "IntakeUni",
+                    "motor_ports": [5],
+                    "reverse_motors": [0],
+                    "gear_ratio": "GREEN",
+                    "max_speed": 120
+                },
+                "roller": {
+                    "config": "RollerUni",
+                    "motor_ports": [19],
+                    "reverse_motors": [0],
+                    "gear_ratio": "GREEN",
+                    "max_speed": 120
                 }
             }
         }

include/Controllers/InputController/Configurations/DanTheMan.hpp

@@ -0,0 +1,24 @@
+/**
+ * @file DanTheMan.hpp
+ * @author John Koch jkoch21@usf.edu
+ * @brief Bull Two Config for Driver: Dan is in fact, the man.
+ *
+ * @version 0.1
+ * @date 2023-03-10
+ *
+ * @copyright Copyright (c) 2022
+ *
+ */
+#ifndef DANTHEMAN_H
+#define DANTHEMAN_H
+
+#include "../InputController.hpp"
+
+class DanTheMan : public TerriBull::InputController {
+    public:
+    DanTheMan(TerriBull::RoboController* RoboController, int deadzone) : TerriBull::InputController(RoboController, deadzone) {}
+    void Init();
+    void Update(float delta);
+};
+
+#endif

include/Controllers/InputController/Configurations/NateIsTank.hpp

@@ -0,0 +1,25 @@
+//10.7
+/**
+ * @file NateIsTank.hpp
+ * @author John Koch jkoch21@usf.edu
+ * @brief Bull One Config for Driver: O' Captian my Captian, Nathaniel
+ *
+ * @version 0.1
+ * @date 2023-03-10
+ *
+ * @copyright Copyright (c) 2022
+ *
+ */
+#ifndef NATEISTANK_H
+#define NATEISTANK_H
+
+#include "../InputController.hpp"
+
+class NateIsTank : public TerriBull::InputController {
+    public:
+    NateIsTank(TerriBull::RoboController* RoboController, int deadzone) : TerriBull::InputController(RoboController, deadzone) {}
+    void Init();
+    void Update(float delta);
+};
+
+#endif

include/Controllers/InputController/Configurations/RyanIsTank.hpp

@@ -0,0 +1,24 @@
+/**
+ * @file RyanIsTank.hpp
+ * @author John Koch jkoch21@usf.edu
+ * @brief Bull One Config for Driver: The Man, the Myth, the Ryan
+ *
+ * @version 0.1
+ * @date 2023-02-28
+ *
+ * @copyright Copyright (c) 2022
+ *
+ */
+#ifndef RYANISTANK_H
+#define RYANISTANK_H
+
+#include "../InputController.hpp"
+
+class RyanIsTank : public TerriBull::InputController {
+    public:
+    RyanIsTank(TerriBull::RoboController* RoboController, int deadzone) : TerriBull::InputController(RoboController, deadzone) {}
+    void Init();
+    void Update(float delta);
+};
+
+#endif

include/Controllers/MechanicalSystem/MechanicalSystem.hpp

@@ -12,7 +12,8 @@
 #ifndef MECHANICAL_SYSTEM_H
 #define MECHANICAL_SYSTEM_H
 
-#include "TerriBull/TerriBull.hpp"
+#include "../../TerriBull/TerriBull.hpp"
+#include "../../TerriBull/lib/KalmanFilter.hpp"
 #include "pros/imu.hpp"
 #include "../../TerriBull/ProsAPI.h"
 #include <map>
@@ -30,6 +31,7 @@ class TerriBull::MechanicalSystem {
         TerriBull::Roller* pRoller;
         TerriBull::Expansion* pExpansion;
         TerriBull::Vector2 * pPosition;
+        KalmanFilter1D* pThetaFilter;
         double * pAngle;
         float pStartingAngle;
 
@@ -48,24 +50,34 @@ class TerriBull::MechanicalSystem {
     /* Tasking Specific */
     float getDriveError() const;
     float getDriveDError() const;
+    bool DriveNeedsAngleCorrection() const;
     float getRollerError() const;
     float getRollerDError() const;
     bool isShotCompleted() const;
+    bool isShooterLoaded() const;
     bool isRollerCompleted() const;
-
     void Init();
     void update(float delta);
-    /* API TO Mechanical System */
-    int GoToPosition(Vector2 pos);
-    void resetDrive();
+
+    /* API To Mechanical System */
+    int GoToPosition(Vector2 v_f, Vector2 v_i, bool reverse);
+    void ResetDrive();
     int TurnToAngle(float theta);
-    int turnOnIntake(float direction);
-    int turnOffIntake();
-    int spinRollerTo(float pos);
-    int spinRollerFor(int direction, float time);
-    int resetRoller();
+    int TurnOnIntake(float direction); //+
+    int TurnOffIntake();
+    int SpinRollerTo(float pos);
+    int SpinRollerFor(int direction, float time);
+    int ResetRoller();
     int ShootDisk();
-    int resetShooter();
+    int LoadShooter();
+    int TurnOnShooter();
+    int ResetShooter();
+    int ConstrictMotorGroupCurrent(TerriBull::MechanicalComponent::MotorRefs* refGroup);
+    int UnConstrictMotorGroupCurrent(TerriBull::MechanicalComponent::MotorRefs* refGroup);
+
+
+    float getIntakeRPM() const;
+
     /*Setters*/
     void setMotherSystem(RoboController* _motherSystem);
     void setIntake(TerriBull::Intake * _intake);
@@ -78,8 +90,6 @@ class TerriBull::MechanicalSystem {
     TerriBull::Roller * getRoller();
     TerriBull::Expansion * getExpansion();
     TerriBull::Drive * getDrive();
-
-
 
 };
 

include/Controllers/ObjectHandler/ObjectHandler.hpp

@@ -14,6 +14,9 @@
 #define OBJECTHANDLER_H
 
 #include "../../TerriBull/TerriBull.hpp"
+#include "../../TerriBull/lib/GameObjects/SpinUp/Disk.hpp"
+#include "../../TerriBull/lib/GameObjects/SpinUp/FieldRoller.hpp"
+#include "../../TerriBull/lib/GameObjects/SpinUp/Goal.hpp"
 #include <list>
 #include <map>
 #include <vector>
@@ -22,26 +25,26 @@
 
 class TerriBull::ObjectHandler { 
     private:
-    // Vector2 
+    std::map<GameObject::Types, std::vector<GameObject*>> Objects;
+    size_t pNumObjects;
     public:
-      ::std::list<TerriBull::GameObject> Objects;
-      const ::std::map<int, int> MaxOfTypes = {
-        {2, 1}, //REDGOAL
-        {6, 1}, //BLUEGOAL
-    };
-    const ::std::map<int, ::std::vector<float>> WidthHeights = {
-        {2, {13.5,13.5}}, //REDGOAL
-        {6, {13.5,13.5}} //BLUEGOAL
-      /* ADD MORE */
-    };
-
-    ObjectHandler() {}
-    ~ObjectHandler() {}
-    TerriBull::GameObject* query(::std::string identifier) {return nullptr;} /*TODO*/
-    TerriBull::GameObject* getClosestObj(GameObject::Types type, Vector2 pos) {return nullptr;} /*TODO*/
-    void updateObjPos(::std::string identifier,GameObject::Types type, Vector2 pos) {} /*TODO*/
-    void addObject(::std::string identifier, GameObject::Types type, Vector2 pos) {} /*TODO*/
-    void update() {} /*TODO*/
+
+    ObjectHandler() : pNumObjects(0) {
+      /** Initialize the Data Management System */
+      Objects[GameObject::Types::DISK] = std::vector<GameObject*>();
+      Objects[GameObject::Types::ROLLER] = std::vector<GameObject*>();
+      Objects[GameObject::Types::GOAL] = std::vector<GameObject*>();
+      /*TODO: Create and develop the Other Object Types */
+    }
+    ~ObjectHandler() {
+
+    }
+    TerriBull::GameObject* query(GameObject::Types type, char identifier) {return nullptr;} /*TODO*/
+    TerriBull::GameObject* getClosestObjByType(GameObject::Types type, Vector2* currentPos);
+    TerriBull::GameObject* getClosestObj(Vector2* currentPos);
+    void updateObjPos(char identifier,GameObject::Types type, Vector2 pos); /*TODO*/
+    void addObject(GameObject* _obj); /*TODO*/
+    void update(GameObject::Types type, char id, void* args); 
 };
 
 // TerriBull::GameObject* TerriBull::ObjectHandler::getClosestObj (GameObject::Types type, TerriBull::Vector2 pos) {