diff --git a/configuration.json b/configuration.json index 17eb759..2d692af 100644 --- a/configuration.json +++ b/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 } } } diff --git a/include/Controllers/InputController/Configurations/DanTheMan.hpp b/include/Controllers/InputController/Configurations/DanTheMan.hpp new file mode 100644 index 0000000..8ccf571 --- /dev/null +++ b/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 diff --git a/include/Controllers/InputController/Configurations/NateIsTank.hpp b/include/Controllers/InputController/Configurations/NateIsTank.hpp new file mode 100644 index 0000000..1c4cc81 --- /dev/null +++ b/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 diff --git a/include/Controllers/InputController/Configurations/RyanIsTank.hpp b/include/Controllers/InputController/Configurations/RyanIsTank.hpp new file mode 100644 index 0000000..b50d060 --- /dev/null +++ b/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 diff --git a/include/Controllers/MechanicalSystem/MechanicalSystem.hpp b/include/Controllers/MechanicalSystem/MechanicalSystem.hpp index a40a010..12605f6 100644 --- a/include/Controllers/MechanicalSystem/MechanicalSystem.hpp +++ b/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 @@ -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(); - - }; diff --git a/include/Controllers/ObjectHandler/ObjectHandler.hpp b/include/Controllers/ObjectHandler/ObjectHandler.hpp index aa5129b..dbfbe65 100644 --- a/include/Controllers/ObjectHandler/ObjectHandler.hpp +++ b/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 #include #include @@ -22,26 +25,26 @@ class TerriBull::ObjectHandler { private: - // Vector2 + std::map> Objects; + size_t pNumObjects; public: - ::std::list Objects; - const ::std::map MaxOfTypes = { - {2, 1}, //REDGOAL - {6, 1}, //BLUEGOAL - }; - const ::std::map> 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(); + Objects[GameObject::Types::ROLLER] = std::vector(); + Objects[GameObject::Types::GOAL] = std::vector(); + /*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) { diff --git a/include/Controllers/RoboController/RoboController.hpp b/include/Controllers/RoboController/RoboController.hpp index d336bb9..060087f 100644 --- a/include/Controllers/RoboController/RoboController.hpp +++ b/include/Controllers/RoboController/RoboController.hpp @@ -26,11 +26,13 @@ class TerriBull::RoboController { /* Systems */ // ObjectHandler* objHandler; TerriBull::TaskManager* taskManager; + TerriBull::ObjectHandler* objHandler; TerriBull::MechanicalSystem* system; TerriBull::SerialController* serialController; TerriBull::InputController* inputController; ConfigurationParser* configParser; uint32_t currentTime, previousTime; + bool pDebug; public: @@ -39,16 +41,12 @@ class TerriBull::RoboController { /* Setter Methods */ - // void setObjHandler(ObjectHandler* ObjectHandler) { - // this->objHandler = ObjectHandler; - // } + void setObjHandler(ObjectHandler* objHandler); void setTaskManager(TaskManager* taskManager); void setSystem(MechanicalSystem* system); void setSerialController(SerialController* serialController); /* Getter Methods */ - // ObjectHandler* getObjHandler() { - // return this->objHandler; - // } + ObjectHandler* getObjHandler(); TaskManager* getTaskManager(); MechanicalSystem* getSystem(); SerialController* getSerialController(); @@ -59,6 +57,45 @@ class TerriBull::RoboController { void Run(); void Stop(); int ClearTasks(); + + void runSerialTask(void* args); + void runMainTask(void* args); + }; #endif + + +#ifndef __CALLBACK_METHODS__ +#define __CALLBACK_METHODS__ + +#include +#include +#include +/* Data Callbacks */ +extern void SetDiskObjectCallback(TerriBull::RoboController* robot, char * array, int start_index, int length); //[Jetson -> Brain] (id, x, y) +extern void GetDiskObjectCallback(TerriBull::RoboController* robot, char * array, int start_index, int length); //[Brain -> Jetson] (id) +extern void SetRollerObjectCallback(TerriBull::RoboController* robot, char * array, int start_index, int length); //[Jetson -> Brain] (id, x, y) +extern void GetRollerObjectCallback(TerriBull::RoboController* robot, char * array, int start_index, int length); //[Brain -> Jetson] (id) +extern void SetGoalObjectCallback(TerriBull::RoboController* robot, char * array, int start_index, int length); //[Jetson -> Brain] (id, x, y, color, in_contact) +extern void GetGoalObjectCallback(TerriBull::RoboController* robot, char * array, int start_index, int length); +extern void SetPositionCallback(TerriBull::RoboController* robot, char * array, int start_index, int length); +extern void GetPositionCallback(TerriBull::RoboController* robot, char * array, int start_index, int length); +/* API Callbacks */ +extern void GoToPositionCallback(TerriBull::RoboController* robot, char * array, int start_index, int length); +extern void GoToPositionDxDyCallback(TerriBull::RoboController* robot, char * array, int start_index, int length); +extern void GoToPositionDRDThetaCallback(TerriBull::RoboController* robot, char * array, int start_index, int length); +extern void CreateObjectCallback(TerriBull::RoboController* robot, char * array, int start_index, int length); +extern void GoToObjectCallback(TerriBull::RoboController* robot, char * array, int start_index, int length); +extern void TurnToAngleCallback(TerriBull::RoboController* robot, char * array, int start_index, int length); +extern void SpinRollerCallback(TerriBull::RoboController* robot, char * array, int start_index, int length); +extern void ShootDiskCallback(TerriBull::RoboController* robot, char * array, int start_index, int length); +extern void LoadShooterCallback(TerriBull::RoboController* robot, char * array, int start_index, int length); + +/* Task Management Callbacks */ +extern void ClearTasksCallback(TerriBull::RoboController* robot, char * array, int start, int length); + +/* Other Callbacks */ +extern void TagExchangeCallback(TerriBull::RoboController* robot, char * array, int start, int length); + +#endif \ No newline at end of file diff --git a/include/Controllers/SerialController/SerialController.hpp b/include/Controllers/SerialController/SerialController.hpp index bb67efd..62edbca 100644 --- a/include/Controllers/SerialController/SerialController.hpp +++ b/include/Controllers/SerialController/SerialController.hpp @@ -1,6 +1,6 @@ /** * @file SerialController.hpp - * @author John Koch jkoch21@usf.edu + * @author John Koch jkoch21@usf.edu, Bill Gate * @brief Manages Serial communication between the V5 and other devices * * @version 0.1 @@ -17,17 +17,81 @@ #include "pros/apix.h" //added this in #include #include +#include +#include +#include class TerriBull::SerialController { public: - char buffer[500]; + typedef void (*PacketCallback) (TerriBull::RoboController* robot, char * array, int start_index, int length); + + typedef struct { + PacketCallback callback; + std::string friendly_name; + int jetson_id = -1; + } CallbackItem; + + typedef struct { + CallbackItem* callbackItem; + float sumTime; + float frequency; + } ScheduledCallback; + + private: + vector __next_packet; + static constexpr char __packet_header[4] = { (char)115, (char)111, (char)117, (char)116 }; + static constexpr char __end_of_transmission[4] = { (char)0, (char)0, (char)10, (char)10 }; + static const int __header_length = (sizeof(__packet_header) / sizeof(char)); + static const int __footer_length = sizeof(__end_of_transmission) / sizeof(char); + static const int __packet_index_offset = 15; + bool isCollectingTags, tagExchange; + map Callbacks; + map tmpCallbacks; + vector ScheduledCallbacks; + TerriBull::RoboController* motherSys; + + bool CompareBuffer(vector buffer1, int start, int end, char* buffer2); + SerialController::CallbackItem* FindInternal(std::string tag_name); + + public: + // struct UpdateArgs { + // volatile bool* buffer_update; + + // }; + + static std::string input_buffer; + static pros::Mutex input_mutex; + + static void read_input_task(void* ignore) + { + char c; + while(true) + { + std::cin.get(c); + std::unique_lock lock(TerriBull::SerialController::input_mutex); + TerriBull::SerialController::input_buffer += c; + lock.unlock(); + } + } + SerialController(TerriBull::RoboController* _motherSys); - SerialController(); - void update(); - void readBuffer(); + static std::string SerializeNumber( double f ); + static double DeserializeNumber( char *array, int *si ); + static std::string SerializeString( std::string s ); + //static std::string SerializeString( const char *s ); + static std::string DeserializeString( char *array, int *si ); + void ExchangeTags(); + int RegisterCallback(std::string tag_name, PacketCallback callback); + void DeserializePacket(); + void Update(float delta); + bool ReadBuffer(); void processDataFromBuffer(); - void sendData(::std::string data); + void SendData(::std::string data); + void updateExchangeTags(); + bool isInitialized(); + void ProcessTagExchange(char * array, int start_index, int length); + int GetCallbackIndex(std::string tag_name); }; -#endif \ No newline at end of file +#endif diff --git a/include/Controllers/TaskManager/TaskManager.hpp b/include/Controllers/TaskManager/TaskManager.hpp index 5d0d647..dadd2fb 100644 --- a/include/Controllers/TaskManager/TaskManager.hpp +++ b/include/Controllers/TaskManager/TaskManager.hpp @@ -23,75 +23,92 @@ #include // #include "../../TerriBull/lib/Expressions/Expression.hpp" - class TerriBull::TaskManager { private: - TerriBull::TaskList *currentTaskSet; - TerriBull::PriorityQueue tasks; - int tasksCompleted; - // ::std::vector Expressions; + std::list tasks; + TerriBull::TaskList* currentTaskSet = nullptr; + int tasksCompleted = 0; + // std::vector Expressions; public: - TaskManager() : tasksCompleted(0) { - currentTaskSet = nullptr; - this->tasks = TerriBull::PriorityQueue(); + TaskManager() = default; + + ~TaskManager() { + ClearAllTasks(); } + void ClearAllTasks() { - this->tasks.deque_all(); - if (this->currentTaskSet!= nullptr) { - for ( auto tsk : *(this->currentTaskSet) ) { - delete tsk; + if (currentTaskSet != nullptr) { + for (auto tsk : *currentTaskSet) { + tsk->terminate(); + delete tsk; } - delete this->currentTaskSet; + delete currentTaskSet; + currentTaskSet = nullptr; } - this->currentTaskSet = nullptr; - } - void addTaskSet(TaskList *tasks) { - this->tasks.enqueue(tasks, 0); + for (auto taskList : tasks) { + for (auto tsk : *taskList) { + tsk->terminate(); + delete tsk; + } + delete taskList; + } + tasks.clear(); } - void Init() { + TerriBull::TaskList* InterruptCurrentTask() { + TerriBull::TaskList* tasks = currentTaskSet; + currentTaskSet = nullptr; + if (tasks != nullptr) { + for (auto tsk : *tasks) { + tsk->terminate(); + } + } + return tasks; + } + void addTaskSet(TerriBull::TaskList* tasks) { + this->tasks.push_back(tasks); } - void run(int delta) { - if (this->currentTaskSet != nullptr) - { /* Update and chek our current task*/ - pros::lcd::set_text(7, "Task Number: " + std::to_string(this->tasksCompleted+1)); + void Init() {} + + void run(float delta) { + if (currentTaskSet != nullptr) { /* Update and check our current task */ + pros::lcd::set_text(7, "Task Number: " + std::to_string(tasksCompleted + 1)); int finishedTasks = 0, totalTasks = 0; - for ( auto tsk : *(this->currentTaskSet)) { + for (auto tsk : *currentTaskSet) { totalTasks++; - if (tsk->finishedFlag != true) tsk->update(delta); - else { + if (tsk->finishedFlag != true) { + tsk->update(delta); + } else { finishedTasks++; /* Task is complete */ - if (!tsk->terminated) tsk->terminate(); - } + if (!tsk->terminated) { + tsk->terminate(); + } + } } if (totalTasks == finishedTasks) { - for ( auto tsk : *(this->currentTaskSet) ) { + for (auto tsk : *currentTaskSet) { delete tsk; } - delete this->currentTaskSet; - this->currentTaskSet = nullptr; - this->tasksCompleted++; + delete currentTaskSet; + currentTaskSet = nullptr; + tasksCompleted++; } - } - else if (tasks.isEmpty() == false) - { /* Obtain the next task*/ - this->currentTaskSet = this->tasks.deque(); - for ( auto tsk : *(this->currentTaskSet) ) + } else if (!tasks.empty()) { /* Obtain the next task */ + currentTaskSet = tasks.front(); + tasks.pop_front(); + for (auto tsk : *currentTaskSet) { tsk->init(); + } + } else { /* Do Nothing? */ + pros::lcd::set_text(7, "All Tasks Completed."); } - else - {/* Do Nothing?*/ - // pros::lcd::print(3, "All Tasks Completed."); - // pros::lcd::set_text(7, "All Tasks Completed."); - - } - /* TODO: Optimize the way Expressions are updated such that the Expressions being updated are only those that are realavent to the Current Task */ - // for (auto expression : this->Expressions) { + + /* TODO: Optimize the way Expressions are updated such that the Expressions being updated are only those that are relevant to the Current Task */ + // for (auto expression : Expressions) { // expression->updateTotal(); // } } - }; #endif diff --git a/include/MechanicalComponents/Drive/configurations/tank_drive_quad.hpp b/include/MechanicalComponents/Drive/configurations/tank_drive_quad.hpp index 945e863..14b4da0 100644 --- a/include/MechanicalComponents/Drive/configurations/tank_drive_quad.hpp +++ b/include/MechanicalComponents/Drive/configurations/tank_drive_quad.hpp @@ -29,7 +29,7 @@ class Tank_Drive_Quad : public TerriBull::Drive { public: void setVoltage(float* vals); - Tank_Drive_Quad(int portA, bool aReverse, int portB, bool bReverse, int portC, bool cReverse, int portD, bool dReverse, int gearSet, float conversion, float radius) : targetDirection(0), TerriBull::Drive(gearSet, conversion, radius), pA(portA), pB(portB), pC(portC), pD(portD), kPThetaTranslation(0.2) { + Tank_Drive_Quad(int portA, bool aReverse, int portB, bool bReverse, int portC, bool cReverse, int portD, bool dReverse, int gearSet, float conversion, float radius, float _wheelBase, float kP_Pos, float KI_Pos, float KD_Pos, float kP_Theta, float kI_Theta, float kD_Theta) : targetDirection(0), TerriBull::Drive(gearSet, conversion, radius, _wheelBase, 127, kP_Pos, KI_Pos, KD_Pos, kP_Theta, kI_Theta, kD_Theta), pA(portA), pB(portB), pC(portC), pD(portD), kPThetaTranslation(0.2) { this->pType = "Tank-Drive-Quad"; this->pMotorA = new pros::Motor(pA, (pros::motor_gearset_e)this->gearSet, aReverse); this->pMotorB = new pros::Motor(pB, (pros::motor_gearset_e)this->gearSet, bReverse); @@ -39,16 +39,24 @@ class Tank_Drive_Quad : public TerriBull::Drive { this->pMotorB->set_encoder_units(pros::E_MOTOR_ENCODER_COUNTS); this->pMotorC->set_encoder_units(pros::E_MOTOR_ENCODER_COUNTS); this->pMotorD->set_encoder_units(pros::E_MOTOR_ENCODER_COUNTS); - this->setPID(8.5, 1.95, 0.8); - this->kPTheta = 2.5; this->kDTheta = 0.8; + // this->setPID(8.5, 1.95, 0.8); + // this->kPTheta = 2.5; this->kDTheta = 0.8; pMotorA->set_brake_mode(pros::E_MOTOR_BRAKE_HOLD); pMotorB->set_brake_mode(pros::E_MOTOR_BRAKE_HOLD); pMotorC->set_brake_mode(pros::E_MOTOR_BRAKE_HOLD); pMotorD->set_brake_mode(pros::E_MOTOR_BRAKE_HOLD); + this->pMotorRefs = (pros::Motor**) malloc(sizeof(pros::Motor*)*4); + this->pMotorRefs[0] = pMotorA; + this->pMotorRefs[1] = pMotorB; + this->pMotorRefs[2] = pMotorC; + this->pMotorRefs[3] = pMotorD; + this->motorRefs = new MechanicalComponent::MotorRefs { + this->pType, this->pMotorRefs, 4 + }; } ~Tank_Drive_Quad(); - int drive(TerriBull::Vector2 pos, float delta); + int drive(TerriBull::Vector2 v_f, TerriBull::Vector2 v_i, float delta, bool reverse); void reset(); Vector2* resultant_vector(); void tare_encoders() { @@ -58,7 +66,9 @@ class Tank_Drive_Quad : public TerriBull::Drive { this->pMotorD->tare_position(); } int change_orientation(float theta, float delta); - + void maneuverAngle(float theta, float delta); + float getRPM() const; + }; #endif \ No newline at end of file diff --git a/include/MechanicalComponents/Drive/configurations/tank_drive_std.hpp b/include/MechanicalComponents/Drive/configurations/tank_drive_std.hpp index 9b8a4d7..88c1569 100644 --- a/include/MechanicalComponents/Drive/configurations/tank_drive_std.hpp +++ b/include/MechanicalComponents/Drive/configurations/tank_drive_std.hpp @@ -28,7 +28,7 @@ class Tank_Drive_Std : public TerriBull::Drive { public: void setVoltage(float* vals); - Tank_Drive_Std(int portA, bool aReverse, int portB, bool bReverse, int portC, bool cReverse, int portD, bool dReverse, int portE,bool eReverse, int portF, bool fReverse, int gearSet, float conversion, float radius) : targetDirection(0), TerriBull::Drive(gearSet, conversion, radius), pA(portA), pB(portB), pC(portC), pD(portD), pE(portE), pF(portF), kPThetaTranslation(2.8) { + Tank_Drive_Std(int portA, bool aReverse, int portB, bool bReverse, int portC, bool cReverse, int portD, bool dReverse, int portE,bool eReverse, int portF, bool fReverse, int gearSet, float conversion, float radius, float _wheelBase, float kP_Pos, float KI_Pos, float KD_Pos, float kP_Theta, float kI_Theta, float kD_Theta) : targetDirection(0), TerriBull::Drive(gearSet, conversion, radius, _wheelBase, 127, kP_Pos, KI_Pos, KD_Pos, kP_Theta, kI_Theta, kD_Theta), pA(portA), pB(portB), pC(portC), pD(portD), pE(portE), pF(portF), kPThetaTranslation(15) { this->pType = "Tank-Drive-Std"; this->pMotorA = new pros::Motor(pA, (pros::motor_gearset_e)this->gearSet, aReverse); this->pMotorB = new pros::Motor(pB, (pros::motor_gearset_e)this->gearSet, bReverse); @@ -42,18 +42,31 @@ class Tank_Drive_Std : public TerriBull::Drive { this->pMotorD->set_encoder_units(pros::E_MOTOR_ENCODER_COUNTS); this->pMotorE->set_encoder_units(pros::E_MOTOR_ENCODER_COUNTS); this->pMotorF->set_encoder_units(pros::E_MOTOR_ENCODER_COUNTS); - this->setPID(9.2, 0.16, 0.05); - this->kPTheta = 2.2; this->kDTheta = 0.6; + // this->setPID(9.2, 0.16, 0.05); + // this->kPTheta = 2.2; this->kDTheta = 0.6; pMotorA->set_brake_mode(pros::E_MOTOR_BRAKE_HOLD); pMotorB->set_brake_mode(pros::E_MOTOR_BRAKE_HOLD); pMotorC->set_brake_mode(pros::E_MOTOR_BRAKE_HOLD); pMotorD->set_brake_mode(pros::E_MOTOR_BRAKE_HOLD); pMotorE->set_brake_mode(pros::E_MOTOR_BRAKE_HOLD); pMotorF->set_brake_mode(pros::E_MOTOR_BRAKE_HOLD); + /* Debug */ + this->pUseVoltageRegulator = false; + this->pVoltageRegulator = new TerriBull::VoltageRegulator(6, 2.2); + this->pMotorRefs = (pros::Motor**) malloc(sizeof(pros::Motor*)*6); + this->pMotorRefs[0] = pMotorA; + this->pMotorRefs[1] = pMotorB; + this->pMotorRefs[2] = pMotorC; + this->pMotorRefs[3] = pMotorD; + this->pMotorRefs[4] = pMotorE; + this->pMotorRefs[5] = pMotorF; + this->motorRefs = new MechanicalComponent::MotorRefs { + this->pType, this->pMotorRefs, 6 + }; } ~Tank_Drive_Std(); - int drive(TerriBull::Vector2 pos, float delta); + int drive(TerriBull::Vector2 v_f, TerriBull::Vector2 v_i, float delta, bool reverse); void reset(); Vector2* resultant_vector(); void tare_encoders() { @@ -65,6 +78,8 @@ class Tank_Drive_Std : public TerriBull::Drive { this->pMotorF->tare_position(); } int change_orientation(float theta, float delta); + void maneuverAngle(float theta, float delta, float r, int errorMod); + float getRPM() const; }; diff --git a/include/MechanicalComponents/Drive/configurations/x_drive.hpp b/include/MechanicalComponents/Drive/configurations/x_drive.hpp index cbca605..656b527 100644 --- a/include/MechanicalComponents/Drive/configurations/x_drive.hpp +++ b/include/MechanicalComponents/Drive/configurations/x_drive.hpp @@ -26,7 +26,7 @@ class X_Drive : public TerriBull::Drive { public: void setVoltage(float* vals); - X_Drive(int portA, int portB, int portC, int portD, int gearSet, float conversion, float radius) : TerriBull::Drive(gearSet, conversion, radius), pA(portA), pB(portB), pC(portC), pD(portD) { + X_Drive(int portA, int portB, int portC, int portD, int gearSet, float conversion, float radius, float _wheelBase, float kP_Pos, float KI_Pos, float KD_Pos, float kP_Theta, float kI_Theta, float kD_Theta) : TerriBull::Drive(gearSet, conversion, radius, _wheelBase, 127, kP_Pos, KI_Pos, KD_Pos, kP_Theta, kI_Theta, kD_Theta), pA(portA), pB(portB), pC(portC), pD(portD) { this->pType = "X-Drive"; this->pMotorA = new pros::Motor(pA, (pros::motor_gearset_e)this->gearSet, false); this->pMotorB = new pros::Motor(pB, (pros::motor_gearset_e)this->gearSet, false); @@ -36,21 +36,33 @@ class X_Drive : public TerriBull::Drive { this->pMotorB->set_encoder_units(pros::E_MOTOR_ENCODER_COUNTS); this->pMotorC->set_encoder_units(pros::E_MOTOR_ENCODER_COUNTS); this->pMotorD->set_encoder_units(pros::E_MOTOR_ENCODER_COUNTS); - this->setPID(1.5, 0.2, 0.3); - this->kPTheta = 1.5; this->kDTheta = 0.2; pMotorA->set_brake_mode(pros::E_MOTOR_BRAKE_HOLD); pMotorB->set_brake_mode(pros::E_MOTOR_BRAKE_HOLD); pMotorC->set_brake_mode(pros::E_MOTOR_BRAKE_HOLD); pMotorD->set_brake_mode(pros::E_MOTOR_BRAKE_HOLD); + this->pMotorRefs = (pros::Motor**) malloc(sizeof(pros::Motor*)*4); + this->pMotorRefs[0] = pMotorA; + this->pMotorRefs[1] = pMotorB; + this->pMotorRefs[2] = pMotorC; + this->pMotorRefs[3] = pMotorD; + this->motorRefs = new MechanicalComponent::MotorRefs { + this->pType, this->pMotorRefs, 4 + }; } ~X_Drive(); - int drive(TerriBull::Vector2 pos, float delta); + int drive(TerriBull::Vector2 v_f, TerriBull::Vector2 v_i, float delta, bool reverse); void reset(); - Vector2* resultant_vector() {return nullptr; } - void tare_encoders() {} + Vector2* resultant_vector(); + void tare_encoders() { + this->pMotorA->tare_position(); + this->pMotorB->tare_position(); + this->pMotorC->tare_position(); + this->pMotorD->tare_position(); + } int change_orientation(float theta, float delta); + float getRPM() const; }; diff --git a/include/MechanicalComponents/Drive/drive.hpp b/include/MechanicalComponents/Drive/drive.hpp index b3c60dd..b96b46f 100644 --- a/include/MechanicalComponents/Drive/drive.hpp +++ b/include/MechanicalComponents/Drive/drive.hpp @@ -19,22 +19,37 @@ class TerriBull::Drive : public TerriBull::MechanicalComponent { double * pCurrentAngle; float conversionFactor; float wheelRadius; + float wheelBase; + float maxSpeed; Vector2* pCurrentPos; Vector2 pPreviousPos; float motorPowerThreshold; /* Should be tested */ - float kPTheta, kDTheta; - // Vector2 pCurrentError; - // Vector2 pPreviousError; - public: - Drive(int gearSet, float _conversionFactor, float _wheelRadius) : TerriBull::MechanicalComponent(gearSet), motorPowerThreshold(127), kPTheta(0), kDTheta(0), conversionFactor(_conversionFactor), wheelRadius(_wheelRadius) {} + float kPTheta, kITheta, kDTheta; + + /* TASK SPECIFIC VALUES */ + bool pNeedsAngleCorrection; + bool pUseVoltageRegulator; + VoltageRegulator* pVoltageRegulator; + + public: + Drive(int gearSet, float _conversionFactor, float _wheelRadius, float _wheelBase, float maxSpeed, float kP_Pos, float KI_Pos, float KD_Pos, float kP_Theta, float kI_Theta, float kD_Theta) : TerriBull::MechanicalComponent(gearSet), motorPowerThreshold(127), maxSpeed(127), conversionFactor(_conversionFactor), wheelRadius(_wheelRadius), wheelBase(_wheelBase), kPTheta(kP_Theta), kITheta(kI_Theta), kDTheta(kD_Theta) { + this->kP = kP_Pos; + this->kI = KI_Pos; + this->kD = KD_Pos; + } virtual void setVoltage(float* vals) = 0; virtual void setAnglePtr(double * ptr) final { this->pCurrentAngle = ptr; } virtual void setPosPtr(Vector2* ptr) final { this->pCurrentPos = ptr; } - virtual int drive(Vector2 pos, float delta) = 0; + virtual void setMaxSpeed(float maxSpeed) final { this->maxSpeed = maxSpeed; } + virtual float* getRefMaxSpeed() final { return &this->maxSpeed; } + virtual bool needsAngleCorrection() final { return this->pNeedsAngleCorrection; } + virtual void updateAngleCorrection(bool update) final { this->pNeedsAngleCorrection = update; } + virtual int drive(Vector2 v_f, Vector2 v_i, float delta, bool reverse) = 0; virtual int change_orientation(float theta, float delta) = 0; virtual void reset() = 0; virtual Vector2* resultant_vector() = 0; virtual void tare_encoders() = 0; + virtual float getRPM() const = 0; }; #endif // DRIVE_H diff --git a/include/MechanicalComponents/Intakes/Configurations/Intake_Duo.hpp b/include/MechanicalComponents/Intakes/Configurations/Intake_Duo.hpp index 41d8bb9..57d1fe6 100644 --- a/include/MechanicalComponents/Intakes/Configurations/Intake_Duo.hpp +++ b/include/MechanicalComponents/Intakes/Configurations/Intake_Duo.hpp @@ -26,9 +26,16 @@ class Intake_Duo : public TerriBull::Intake { this->pMotorI = new pros::Motor(i, (pros::motor_gearset_e) this->gearSet, iReverse); this->pMotorJ = new pros::Motor(j, (pros::motor_gearset_e) this->gearSet, jReverse); this->pMotorI->set_encoder_units(pros::E_MOTOR_ENCODER_COUNTS); + this->pMotorRefs = (pros::Motor**) malloc(sizeof(pros::Motor*)*2); + this->pMotorRefs[0] = pMotorI; + this->pMotorRefs[1] = pMotorJ; + this->motorRefs = new MechanicalComponent::MotorRefs { + this->pType, this->pMotorRefs, 2 + }; } int TurnOn(float dir); // button is held + float getRPM() const; int TurnOff(); }; diff --git a/include/MechanicalComponents/Intakes/Configurations/Intake_Uni.hpp b/include/MechanicalComponents/Intakes/Configurations/Intake_Uni.hpp index 47850c0..05650e6 100644 --- a/include/MechanicalComponents/Intakes/Configurations/Intake_Uni.hpp +++ b/include/MechanicalComponents/Intakes/Configurations/Intake_Uni.hpp @@ -25,9 +25,15 @@ class Intake_Uni : public TerriBull::Intake { this->pType = "Intake-Uni"; this->pMotorI = new pros::Motor(i, (pros::motor_gearset_e) this->gearSet, iReverse); this->pMotorI->set_encoder_units(pros::E_MOTOR_ENCODER_COUNTS); + this->pMotorRefs = (pros::Motor**) malloc(sizeof(pros::Motor*)*1); + this->pMotorRefs[0] = pMotorI; + this->motorRefs = new MechanicalComponent::MotorRefs { + this->pType, this->pMotorRefs, 1 + }; } int TurnOn(float dir); // button is held + float getRPM() const; int TurnOff(); }; diff --git a/include/MechanicalComponents/Intakes/intake.hpp b/include/MechanicalComponents/Intakes/intake.hpp index 4218b7b..af971f7 100644 --- a/include/MechanicalComponents/Intakes/intake.hpp +++ b/include/MechanicalComponents/Intakes/intake.hpp @@ -24,6 +24,7 @@ class TerriBull::Intake : public TerriBull::MechanicalComponent { virtual bool isToggled() const final { return this->toggled; } virtual float getDirection() const final { return this->currentDir; } virtual int TurnOn(float dir) = 0; // button is held + virtual float getRPM() const = 0; virtual int TurnOff() = 0; }; diff --git a/include/MechanicalComponents/MechanicalComponent.hpp b/include/MechanicalComponents/MechanicalComponent.hpp index 35d1622..ff3178a 100644 --- a/include/MechanicalComponents/MechanicalComponent.hpp +++ b/include/MechanicalComponents/MechanicalComponent.hpp @@ -15,6 +15,12 @@ #include "../TerriBull/TerriBull.hpp" class TerriBull::MechanicalComponent { + public: + struct MotorRefs { + ::std::string ComponentName; + pros::Motor** Motors; + size_t NumMotors; + }; protected: ::std::string pType; int pVoltageCap; @@ -22,11 +28,14 @@ class TerriBull::MechanicalComponent { float currentError; float previousError; float sumError; - + pros::Motor** pMotorRefs; int gearSet; float kP, kD, kI; + public: + MotorRefs *motorRefs; + public: MechanicalComponent(int gearSet) : kP(0), kD(0), kI(0), currentError(0), previousError(0), pVoltageCap(TerriBull::MAX_VOLTAGE), gearSet(gearSet) {} @@ -34,7 +43,7 @@ class TerriBull::MechanicalComponent { /** * @brief Gets Current Derror and automatically sets the previous error value to the current error value. - * WARNING: this function is not READ-ONLY. + * WARNING: this function is not READONLY * @return float */ virtual float dError() final{ @@ -63,6 +72,15 @@ class TerriBull::MechanicalComponent { virtual ::std::string getType() const final { return this->pType; }; + + virtual float getRPM() const { + return 0; + } + + virtual MotorRefs* getMotorRefs() const final { + return this->motorRefs; + } + }; diff --git a/include/MechanicalComponents/Rollers/Configurations/RollerDuo.hpp b/include/MechanicalComponents/Rollers/Configurations/RollerDuo.hpp index c11d029..e8e5c07 100644 --- a/include/MechanicalComponents/Rollers/Configurations/RollerDuo.hpp +++ b/include/MechanicalComponents/Rollers/Configurations/RollerDuo.hpp @@ -29,9 +29,17 @@ class Roller_Duo : public TerriBull::Roller { this->pMotorI->set_encoder_units(pros::E_MOTOR_ENCODER_COUNTS); this->pMotorJ->set_encoder_units(pros::E_MOTOR_ENCODER_COUNTS); this->setPID(3.5, 0.01, 0.8); + this->pMotorRefs = (pros::Motor**) malloc(sizeof(pros::Motor*)*2); + this->pMotorRefs[0] = pMotorI; + this->pMotorRefs[1] = pMotorJ; + this->motorRefs = new MechanicalComponent::MotorRefs { + this->pType, this->pMotorRefs, 2 + }; } int Spin(int direction, float time, float delta); + int TurnOn(int direction, float pwr); + float getRPM() const; int SpinToPos(float pos); void reset(); void update(); diff --git a/include/MechanicalComponents/Rollers/Configurations/RollerUni.hpp b/include/MechanicalComponents/Rollers/Configurations/RollerUni.hpp index 05a17a3..72fcb4b 100644 --- a/include/MechanicalComponents/Rollers/Configurations/RollerUni.hpp +++ b/include/MechanicalComponents/Rollers/Configurations/RollerUni.hpp @@ -25,10 +25,17 @@ class Roller_Uni : public TerriBull::Roller { this->pType = "Roller-Uni"; this->pMotorI = new pros::Motor(i, (pros::motor_gearset_e) this->gearSet, iReverse); this->pMotorI->set_encoder_units(pros::E_MOTOR_ENCODER_COUNTS); - this->setPID(4.5, 0.01, 0.8); + this->setPID(4.5, 0, 0.8); + this->pMotorRefs = (pros::Motor**) malloc(sizeof(pros::Motor*)*1); + this->pMotorRefs[0] = pMotorI; + this->motorRefs = new MechanicalComponent::MotorRefs { + this->pType, this->pMotorRefs, 1 + }; } int Spin(int direction, float time, float delta); + int TurnOn(int direction, float pwr); + float getRPM() const; int SpinToPos(float pos); void reset(); void update(); diff --git a/include/MechanicalComponents/Rollers/roller.hpp b/include/MechanicalComponents/Rollers/roller.hpp index d71973d..ce611d2 100644 --- a/include/MechanicalComponents/Rollers/roller.hpp +++ b/include/MechanicalComponents/Rollers/roller.hpp @@ -24,6 +24,8 @@ class TerriBull::Roller : public TerriBull::MechanicalComponent { bool timeFlag; Roller(int maxSpeed, int gearSet) : TerriBull::MechanicalComponent(gearSet), maxSpeed(maxSpeed), timeFlag(false), currentPos(0) {} virtual int Spin(int direction, float time, float delta) = 0; + virtual int TurnOn(int direction, float pwr) = 0; + virtual float getRPM() const = 0; virtual float* posPtr() final { return ¤tPos; } virtual float getPos() const final { return currentPos; } virtual int SpinToPos(float pos) = 0; diff --git a/include/MechanicalComponents/Shooters/Configurations/Catapult/CatapultZolt.hpp b/include/MechanicalComponents/Shooters/Configurations/Catapult/CatapultZolt.hpp index f2ff84b..de94e34 100644 --- a/include/MechanicalComponents/Shooters/Configurations/Catapult/CatapultZolt.hpp +++ b/include/MechanicalComponents/Shooters/Configurations/Catapult/CatapultZolt.hpp @@ -20,6 +20,8 @@ class CatapultZolt : public TerriBull::Shooter { pros::Motor *pMotorX, *pMotorY; pros::ADIDigitalIn *limitSwitch; bool engagedOne; float cntNoVal; float sumTime; + float currentPos; + int x, y; public: @@ -27,12 +29,21 @@ class CatapultZolt : public TerriBull::Shooter { this->pType = "Catapult-Zolt"; this->pMotorX = new pros::Motor(x, xReverse); this->pMotorY = new pros::Motor(y, yReverse); + this->setPID(120, 2.5, 0); this->limitSwitch = new pros::ADIDigitalIn(limitSwitchPort); this->pMotorX->set_encoder_units(pros::E_MOTOR_ENCODER_COUNTS); this->pMotorY->set_encoder_units(pros::E_MOTOR_ENCODER_COUNTS); + this->pMotorRefs = (pros::Motor**) malloc(sizeof(pros::Motor*)*2); + this->pMotorRefs[0] = pMotorX; + this->pMotorRefs[1] = pMotorY; + this->motorRefs = new MechanicalComponent::MotorRefs { + this->pType, this->pMotorRefs, 2 + }; } int Shoot(float delta);// button is held + int Load(float delta, void* args); + float getRPM(); int turnOn(); int reset(); bool shotCompleted(); diff --git a/include/MechanicalComponents/Shooters/Configurations/FlyWheel/FlyWheelSB.hpp b/include/MechanicalComponents/Shooters/Configurations/FlyWheel/FlyWheelSB.hpp index e69de29..71515f4 100644 --- a/include/MechanicalComponents/Shooters/Configurations/FlyWheel/FlyWheelSB.hpp +++ b/include/MechanicalComponents/Shooters/Configurations/FlyWheel/FlyWheelSB.hpp @@ -0,0 +1,60 @@ +/** + * @file FlyWheelSB.hpp + * @author John Koch jkoch21@usf.edu + * @brief Uses a Flywheel to propell the Disc Game Pieces into the goal. + * + * @version 0.1 + * @date 2023-03-01 + * + * @copyright Copyright (c) 2022 + * +*/ +#ifndef FLYWHEELSB_H +#define FLYWHEELSB_H + +#include "../../shooter.hpp" + + +class FlyWheelSB : public TerriBull::Shooter { + protected: + pros::Motor *pMotorX, *pMotorY; + bool engagedOne; float cntNoVal; float sumTime; + float currentPos; + TerriBull::Magazine* pMag; + TerriBull::MechanicalSystem* pSystem; + int x, y; + + + public: + FlyWheelSB(int _x, bool xReverse, int _y, int yReverse, TerriBull::Magazine* _mag, int gearSet, TerriBull::MechanicalSystem* _system) : Shooter(gearSet), x(_x), y(_y), engagedOne(false), cntNoVal(0), sumTime(0), pMag(_mag), pSystem(_system) { + this->pType = "FlyWheel-SB"; + this->pMotorX = new pros::Motor(x, xReverse); + this->pMotorY = new pros::Motor(y, yReverse); + this->setPID(120, 2.5, 0); + this->pMotorX->set_encoder_units(pros::E_MOTOR_ENCODER_COUNTS); + this->pMotorY->set_encoder_units(pros::E_MOTOR_ENCODER_COUNTS); + this->pMotorX->set_brake_mode(pros::E_MOTOR_BRAKE_COAST); + this->pMotorY->set_brake_mode(pros::E_MOTOR_BRAKE_COAST); + this->pMotorRefs = (pros::Motor**) malloc(sizeof(pros::Motor*)*2); + this->pMotorRefs[0] = pMotorX; + this->pMotorRefs[1] = pMotorY; + this->motorRefs = new MechanicalComponent::MotorRefs { + this->pType, this->pMotorRefs, 2 + }; + } + + ~FlyWheelSB() { + delete this->pMotorX; + delete this->pMotorY; + delete this->pMag; + } + + int Shoot(float delta);// button is held + int Load(float delta, void* args); + float getRPM() const; + int turnOn(); + int reset(); + bool shotCompleted(); +}; + +#endif // SHOOTER_H diff --git a/include/MechanicalComponents/Shooters/Magazines/Magazine.hpp b/include/MechanicalComponents/Shooters/Magazines/Magazine.hpp new file mode 100644 index 0000000..9ad32f1 --- /dev/null +++ b/include/MechanicalComponents/Shooters/Magazines/Magazine.hpp @@ -0,0 +1,116 @@ +/** + * @file magazine.hpp + * @author John Koch jkoch21@usf.edu + * @brief Magazine mechanism for Robots to count the number of contained disks. + * + * @version 0.1 + * @date 2023-03-01 + * + * @copyright Copyright (c) 2022 + * CHANGELOG: + * - Converted Digital to Analog Logic 04-18-2023 + * +*/ +#ifndef MAGAZINE_H +#define MAGAZINE_H + +#include "../../TerriBull/TerriBull.hpp" + + +class TerriBull::Magazine : public TerriBull::MechanicalComponent { + protected: + bool toggledInc; + bool toggledDec; + pros::ADIAnalogIn **incSensors; + uint8_t numIncSensors, numDecSensors; + int8_t pMagazineCnt; /* Maintain Number of Disks in the Magazine */ + pros::ADIAnalogIn **decSensors; + bool previousIncState, previousDecState; + float* incDefaults,* decDefaults; + public: /* TODO: + - some of our sensors might be in state high as default as opposed to low. + - if a state is high as default, then we need to multiply that sensor readings like this: + 1. add to config file a way to modify the magazines default state. + + */ + Magazine(std::string incSensorPorts, float* inc_defaults, std::string decSensorPorts, float* dec_defaults) : TerriBull::MechanicalComponent(0), numIncSensors(incSensorPorts.size()), numDecSensors(decSensorPorts.size()), incDefaults(inc_defaults), decDefaults(dec_defaults) { + incSensors = new pros::ADIAnalogIn*[incSensorPorts.size()]; + decSensors = new pros::ADIAnalogIn*[decSensorPorts.size()]; + for (int i = 0; i < incSensorPorts.size(); i++) { + incSensors[i] = new pros::ADIAnalogIn(incSensorPorts[i]); + incSensors[i]->calibrate(); + } + for (int i = 0; i < decSensorPorts.size(); i++) { + decSensors[i] = new pros::ADIAnalogIn(decSensorPorts[i]); + decSensors[i]->calibrate(); + } + } + ~Magazine() { + /* Delete the Sensors and Data Pointers */ + for (int i = 0; i < numIncSensors; i++) { + delete incSensors[i]; + } + for (int i = 0; i < numDecSensors; i++) { + delete decSensors[i]; + } + delete[] incSensors; + delete[] decSensors; + } + + void __inc__(bool inc) { + if (inc != this->previousIncState) { + if (inc) { + this->pMagazineCnt++; + this->toggledInc = true; + } + } + this->previousIncState = inc; + } + + void __dec__(bool dec) { + if (dec != this->previousDecState) { + if (dec) { + this->pMagazineCnt--; + this->toggledDec = true; + } + } + this->previousDecState = dec; + /* Clamp our Dec to 0*/ + if (this->pMagazineCnt < 0) this->pMagazineCnt = 0; + } + + int update(float delta) { + int inc = 1; + for (int i = 0; i < this->numIncSensors; i++) { + pros::lcd::set_text(i, to_string(incSensors[i]->get_value_calibrated())); + inc *= int(incSensors[i]->get_value() < incDefaults[i]); /* 1 - 0 */ + } + this->__inc__(inc); + int dec = 1; + for (int i = 0; i < this->numDecSensors; i++) { + pros::lcd::set_text(this->numIncSensors+i, to_string(decSensors[i]->get_value_calibrated())); + dec *= int(decSensors[i]->get_value() < decDefaults[i]); + } + this->__dec__(dec); + return 0; + } + + int reset() { + this->toggledInc = false; + this->toggledDec = false; + return 0; + } + + int8_t getMagazineCount() { + return this->pMagazineCnt; + } + bool getIncToggle() { + return this->toggledInc; + } + bool getDecToggle() { + return this->toggledDec; + } + +}; + +#endif // SHOOTER_H diff --git a/include/MechanicalComponents/Shooters/shooter.hpp b/include/MechanicalComponents/Shooters/shooter.hpp index 0ee754a..71bb51e 100644 --- a/include/MechanicalComponents/Shooters/shooter.hpp +++ b/include/MechanicalComponents/Shooters/shooter.hpp @@ -18,16 +18,20 @@ class TerriBull::Shooter : public TerriBull::MechanicalComponent { protected: bool toggled; + bool loaded; bool shotComplete; + public: - Shooter(int gearSet) : TerriBull::MechanicalComponent(gearSet), toggled(false), shotComplete(false) {} + Shooter(int gearSet) : TerriBull::MechanicalComponent(gearSet), toggled(false), shotComplete(false), loaded(false) {} virtual int Shoot(float delta) = 0;// button is held virtual int turnOn() = 0; + virtual int Load(float delta, void* args) = 0; virtual int reset() = 0; virtual bool shotCompleted() = 0; virtual bool isToggled() const final { return toggled; } + virtual bool isLoaded() const final { return loaded; } + virtual bool* getLoadedPtr() { return &loaded; } }; - #endif // SHOOTER_H diff --git a/include/TerriBull/TerriBull.hpp b/include/TerriBull/TerriBull.hpp index 2c097ac..890a4a4 100644 --- a/include/TerriBull/TerriBull.hpp +++ b/include/TerriBull/TerriBull.hpp @@ -17,6 +17,7 @@ #include "../pros/apix.h" #include "./lib/Logger.hpp" #include +#include #include #include #include @@ -38,6 +39,7 @@ namespace TerriBull { // typedef ::std::map Str2SizeMap; /* Vectorization Class */ class Vector2; + typedef std::vector> matrix; /* Manages Tasks for the Robot */ class TaskManager; /* Task Data Containers */ @@ -47,6 +49,7 @@ namespace TerriBull { class IntakeTask; class ShooterTask; class TimeTask; + class VariableTask; typedef ::std::vector TaskList; /* Manages Serial Communication of the Robot */ class SerialController; @@ -60,6 +63,7 @@ namespace TerriBull { class Intake; class Roller; class Shooter; /*FlyWheel Catapult*/ + class Magazine; /* Main Controller for the Robot */ class RoboController; /* Takes Controller Input and Controls the Robot */ @@ -68,7 +72,8 @@ namespace TerriBull { class ObjectHandler; /* Game Object Class */ class GameObject; - + /*Subsystem Tools*/ + class VoltageRegulator; /* TerriBull Type Definitions */ typedef ::std::vector<::pros::Motor> MotorGroup; @@ -88,6 +93,7 @@ namespace TerriBull { extern float DEG2RAD( const float deg ); extern float RAD2DEG( const float rad ); extern float GetDTheta(float tf, float ti); + extern matrix matrix_mult(matrix &M1, int M1_rows, int M1_cols, matrix &M2, int M2_rows, int M2_cols); extern pros::Imu mu; @@ -108,6 +114,8 @@ namespace TerriBull { template class Node { + + public: Node *prev; Node *next; int16_t priority; @@ -174,7 +182,7 @@ namespace TerriBull { template class linkedlist { - + int size; Node *g(int index, int cur, Node *curr); public: @@ -184,8 +192,8 @@ namespace TerriBull { linkedlist() { this->size = 0; - this->head = nullptr; - this->tail = nullptr; + this->head = nullptr; + this->tail = nullptr; } ~linkedlist() { @@ -196,10 +204,10 @@ namespace TerriBull { tempNode = node; node = node->getNext(); delete tempNode; - } + } delete node; - } } + } linkedlist(Node *h){ this->size = 0; @@ -223,7 +231,7 @@ namespace TerriBull { bool isEmpty() { return this->size < 1; - } + } void add(T* data, int priority) { bool insertion = false; @@ -236,7 +244,7 @@ namespace TerriBull { this->tail= node; this->size++; insertion = true; - } + } /* The List has only one node, check the prioritys and insert accordingly */ @@ -247,10 +255,10 @@ namespace TerriBull { this->head->setPrev(node); node->setNext(this->head); this->tail = this->head; - this->head = node; + this->head = node; insertion = true; this->size++; - } + } else { /*Create Linkage*/ this->tail = node; @@ -258,7 +266,7 @@ namespace TerriBull { this->tail->setPrev(this->head); insertion = true; this->size++; - } + } } else { /* Our list has multiple nodes, back->front approach until we find our insertion spot @@ -267,7 +275,7 @@ namespace TerriBull { while(priority>current->getPriority() && insertion==false) { //Our current node has not found the right insertion spot, and we haven't reached the end of the list if (current != this->head) { - + current = current->getPrev(); } //Our current node has not found the right insertion spot, and we have reached our head node. @@ -275,11 +283,11 @@ namespace TerriBull { /*Create Linkage*/ node->setNext(this->head); this->head->setPrev(node); - this->head = node; + this->head = node; insertion = true; this->size++; - } - } + } + } // WE found our spot! if(!insertion) { /* @@ -291,7 +299,7 @@ namespace TerriBull { /*Create Linkage*/ this->tail->setNext(node); node->setPrev(this->tail); - this->tail = node; + this->tail = node; this->size++; } // Insertion in middle of list @@ -304,9 +312,9 @@ namespace TerriBull { this->size++; } } - } + } } - + bool remove(T* data){ /* To Prevent a break, we want to return null if index isn't in our list.*/ Node *node = this->head; @@ -324,12 +332,12 @@ namespace TerriBull { next->setPrev(nullptr); this->head = next; delete node; - } + } else if(next == nullptr) { prev->setNext(nullptr); this->tail = prev; delete node; - } + } else { /*Create Linkage*/ @@ -347,7 +355,7 @@ namespace TerriBull { this->head = nullptr; this->tail = nullptr; delete node; - } + } else {/* We are deleting the head node of a length-2 list */ if(node == this->head) @@ -358,16 +366,16 @@ namespace TerriBull { else { this->tail = this->head; delete node; - } - } - } + } + } + } this->size--; return true; - } + } node = node->getNext(); } return false; - } + } void clear() { if (this->head != nullptr) { @@ -393,7 +401,7 @@ namespace TerriBull { } return nullptr; } - + T* pop() { T* returnValue = this->head->getData(); this->head->setData(nullptr); @@ -422,7 +430,7 @@ namespace TerriBull { // } // return stream; // } - }; +}; #endif @@ -489,10 +497,12 @@ namespace TerriBull { #include "../api.h" #include "./lib/Vector2.hpp" + #include "./lib/SubsystemTools/VoltageRegulator.hpp" #include "../MechanicalComponents/MechanicalComponent.hpp" #include "../MechanicalComponents/Drive/drive.hpp" #include "../MechanicalComponents/Intakes/intake.hpp" #include "../MechanicalComponents/Rollers/roller.hpp" + #include "../MechanicalComponents/Shooters/Magazines/magazine.hpp" #include "../MechanicalComponents/Shooters/shooter.hpp" #include "../Controllers/MechanicalSystem/MechanicalSystem.hpp" #include "./lib/Tasking/Task.hpp" @@ -501,6 +511,7 @@ namespace TerriBull { #include "./lib/Tasking/Types/IntakeTask.hpp" #include "./lib/Tasking/Types/ShooterTask.hpp" #include "./lib/Tasking/Types/TimeTask.hpp" + #include "./lib/Tasking/Types/VariableTask.hpp" #include "../Controllers/TaskManager/TaskManager.hpp" #include "./lib/ConfigurationParser.hpp" #include "./lib/GameObjects/GameObject.hpp" diff --git a/include/TerriBull/lib/ConfigurationParser.hpp b/include/TerriBull/lib/ConfigurationParser.hpp index cac0a85..dc07c0c 100644 --- a/include/TerriBull/lib/ConfigurationParser.hpp +++ b/include/TerriBull/lib/ConfigurationParser.hpp @@ -29,18 +29,25 @@ class ConfigurationParser { typedef enum {NO_ERROR, FILE_NOT_FOUND, FILE_OPEN_ERROR, VARIABLE_PARSE_ERROR} Error; uint8_t errCode = 0; /* MECHANICAL SYSTEM VARIABLES */ - + typedef struct { - Json::Value Config; /* User Selected Config */ /* Drive Variables */ - Json::Value DriveConfig; - Json::Value DriveMotorPorts; - Json::Value DriveMotorReverse; - Json::Value DriveMotorGearset; - Json::Value DriveConversionFactor; - Json::Value DriveWheelRadius; + Json::Value Config; + Json::Value MotorPorts; + Json::Value MotorReverse; + Json::Value MotorGearset; + Json::Value ConversionFactor; + Json::Value WheelRadius; + Json::Value WheelBase; + Json::Value KPos; + Json::Value KTheta; + } RoboDriveConfig; + + typedef struct { + Json::Value Config; /* User Selected Config */ + RoboDriveConfig DriveConfig; /* IMU Sensor */ Json::Value IMUConfig; Json::Value StartingAngle; @@ -84,18 +91,20 @@ class ConfigurationParser { /* Load In Configuration Variables */ this->pConfigVariables.ControllerConfig = this->pConfigVariables.Config["controller_config"]; this->pConfigVariables.ControllerDeadzone = this->pConfigVariables.Config["controller_deadzone"]; - this->pConfigVariables.DriveConfig = this->pConfigVariables.Config["mechanical_system"]["drive"]["config"]; - this->pConfigVariables.DriveMotorPorts = this->pConfigVariables.Config["mechanical_system"]["drive"]["motor_ports"]; - this->pConfigVariables.DriveMotorReverse = this->pConfigVariables.Config["mechanical_system"]["drive"]["reverse_motors"]; - this->pConfigVariables.DriveMotorGearset = this->pConfigVariables.Config["mechanical_system"]["drive"]["gear_ratio"]; - this->pConfigVariables.DriveConversionFactor = this->pConfigVariables.Config["mechanical_system"]["drive"]["cf"]; - this->pConfigVariables.DriveWheelRadius = this->pConfigVariables.Config["mechanical_system"]["drive"]["radius"]; - + this->pConfigVariables.DriveConfig.Config = this->pConfigVariables.Config["mechanical_system"]["drive"]["config"]; + this->pConfigVariables.DriveConfig.MotorPorts = this->pConfigVariables.Config["mechanical_system"]["drive"]["motor_ports"]; + this->pConfigVariables.DriveConfig.MotorReverse = this->pConfigVariables.Config["mechanical_system"]["drive"]["reverse_motors"]; + this->pConfigVariables.DriveConfig.MotorGearset = this->pConfigVariables.Config["mechanical_system"]["drive"]["gear_ratio"]; + this->pConfigVariables.DriveConfig.ConversionFactor = this->pConfigVariables.Config["mechanical_system"]["drive"]["cf"]; + this->pConfigVariables.DriveConfig.WheelRadius = this->pConfigVariables.Config["mechanical_system"]["drive"]["radius"]; + this->pConfigVariables.DriveConfig.WheelBase = this->pConfigVariables.Config["mechanical_system"]["drive"]["wheel_base"]; + this->pConfigVariables.DriveConfig.KPos = this->pConfigVariables.Config["mechanical_system"]["drive"]["k_pos"]; + this->pConfigVariables.DriveConfig.KTheta = this->pConfigVariables.Config["mechanical_system"]["drive"]["k_theta"]; this->pConfigVariables.IMUConfig = this->pConfigVariables.Config["mechanical_system"]["imu"]; this->pConfigVariables.StartingAngle = this->pConfigVariables.Config["mechanical_system"]["starting_angle"]; this->pConfigVariables.StartingPos = this->pConfigVariables.Config["mechanical_system"]["starting_position"]; - pros::lcd::set_text(1, "Parsed DriveType: " + this->pConfigVariables.DriveConfig.asString()); - // pros::lcd::set_text(3, "Parsed Start Pos: " + std::to_string(this->pConfigVariables.StartingPos["x"].asFloat()) + " " + std::to_string(this->pConfigVariables.StartingPos["y"].asFloat())); + pros::lcd::set_text(1, "Parsed DriveType: " + this->pConfigVariables.DriveConfig.Config.asString()); + // pros::lcd6::set_text(3, "Parsed Start Pos: " + std::to_string(this->pConfigVariables.StartingPos["x"].asFloat()) + " " + std::to_string(this->pConfigVariables.StartingPos["y"].asFloat())); // pros::lcd::set_text(4, "Parsed Start Angle: " + std::to_string(this->pConfigVariables.StartingAngle.asFloat())); } ~ConfigurationParser(); @@ -105,7 +114,7 @@ class ConfigurationParser { TerriBull::Drive* getDriveConfig(); TerriBull::Intake* getIntakeConfig(); TerriBull::Roller* getRollerConfig(); - TerriBull::Shooter* getShooterConfig(); + TerriBull::Shooter* getShooterConfig(TerriBull::MechanicalSystem* _system); TerriBull::MechanicalSystem* getMechanicalSystemConfig(); TerriBull::InputController* getInputControllerConfig(TerriBull::RoboController* roboController); diff --git a/include/TerriBull/lib/GameObjects/GameObject.hpp b/include/TerriBull/lib/GameObjects/GameObject.hpp index b33a9d0..6f08588 100644 --- a/include/TerriBull/lib/GameObjects/GameObject.hpp +++ b/include/TerriBull/lib/GameObjects/GameObject.hpp @@ -1,7 +1,7 @@ /** * @file GameObject.hpp * @author John Koch jkoch21@usf.edu - * @brief Data structure for implementing game objects on the field + * @brief Abstract Data structure for implementing game objects on the field * * @version 0.1 * @date 2023-01-04 @@ -21,11 +21,11 @@ class TerriBull::GameObject { private: public: typedef enum { - DISK, ROBOT, ROLLER + DISK, ROBOT, ROLLER, GOAL, LOWER_GOAL } Types; - ::std::string identifier; - Vector2 pos; + char id; + Vector2* pos; float width; float height; Types type; @@ -33,22 +33,23 @@ class TerriBull::GameObject { ~GameObject() { } - GameObject() { - - } - GameObject(Vector2 pos, TerriBull::string identifier, int type, float width, float height) : GameObject() { + + GameObject(Vector2* pos, char identifier, Types type, float width, float height) : pos(pos), id(identifier), type(type), width(width), height(height) { } + + + virtual void setPos(Vector2* pos) final { this->pos = pos; - this->identifier = identifier; } - void setPos(Vector2 pos) { - this->pos = pos; + virtual Vector2* getPos() const final { + return new Vector2(this->pos); } - bool checkID(::std::string identifier) { - if (this->identifier != identifier) return false; + virtual Vector2* getPosPtr() final { return this->pos; } + - return true; + virtual bool checkID(char identifier) final { + return this->id == identifier; } }; diff --git a/include/TerriBull/lib/GameObjects/SpinUp/Disk.hpp b/include/TerriBull/lib/GameObjects/SpinUp/Disk.hpp new file mode 100644 index 0000000..3dc67cf --- /dev/null +++ b/include/TerriBull/lib/GameObjects/SpinUp/Disk.hpp @@ -0,0 +1,46 @@ +/** + * @file Disk.hpp + * @author John Koch jkoch21@usf.edu + * @brief Disk Game Object Class for containing Disk Specific information + * + * @version 0.1 + * @date 2023-03-13 + * + * @copyright Copyright (c) 2022 + * + */ +#ifndef DISK_H +#define DISK_H + +#include "../GameObject.hpp" + +#include + + +class Disk : public TerriBull::GameObject { + private: + public: + + struct UpdateArgs { + float x; + float y; + }; + + static void* ConstructUpdateArgs(float x, float y) { + size_t size = sizeof(float) * 2; /* Calculate Size*/ + void* data = malloc(size); + size_t offset = 0; + memcpy((char*)data + offset, &x, sizeof(float)); + offset += sizeof(float); + memcpy((char*)data + offset, &y, sizeof(float)); + return data; + } + + ~Disk() { + + } + Disk(TerriBull::Vector2* pos, char identifier) :GameObject(pos, identifier, DISK, 5, 5) { } + +}; + +#endif \ No newline at end of file diff --git a/include/TerriBull/lib/GameObjects/SpinUp/FieldRoller.hpp b/include/TerriBull/lib/GameObjects/SpinUp/FieldRoller.hpp new file mode 100644 index 0000000..0842190 --- /dev/null +++ b/include/TerriBull/lib/GameObjects/SpinUp/FieldRoller.hpp @@ -0,0 +1,77 @@ +/** + * @file Roller.hpp + * @author John Koch jkoch21@usf.edu + * @brief Roller Game Object Class for containing Roller Specific Information + * + * @version 0.1 + * @date 2023-03-13 + * + * @copyright Copyright (c) 2022 + * + */ +#ifndef FIELD_ROLLER_H +#define FIELD_ROLLER_H + +#include "../GameObject.hpp" + +#include + + +class FieldRoller : public TerriBull::GameObject { /*TODO: Change name not to conflict with Mech-Sys Roller*/ + public: + typedef enum { + RED, BLUE, NEUTRAL + } Color; + + struct UpdateArgs { + float x; + float y; + int color; + bool is_in_contact; + }; + + static void* ConstructUpdateArgs(float x, float y, int color, bool is_in_contact) { + size_t size = sizeof(float) * 2 + sizeof(int) + sizeof(bool); /* Calculate Size*/ + void* data = malloc(size); + size_t offset = 0; + memcpy((char*)data + offset, &x, sizeof(float)); + offset += sizeof(float); + memcpy((char*)data + offset, &y, sizeof(float)); + offset += sizeof(float); + int color_as_int = static_cast(color); + memcpy((char*)data + offset, &color_as_int, sizeof(int)); + offset += sizeof(int); + memcpy((char*)data + offset, &is_in_contact, sizeof(bool)); + return data; + } + + private: + Color pCurrentColor; + bool pIsInContact; + + ~FieldRoller() { + + } + public: + + FieldRoller(TerriBull::Vector2* pos, char identifier) : GameObject(pos, identifier, ROLLER, 6, 1) { } + + bool isInContact() { + return pIsInContact; + } + + Color getCurrentColor() { + return pCurrentColor; + } + + void setCurrentColor(Color color) { + pCurrentColor = color; + } + + void setIsInContact(bool isInContact) { + pIsInContact = isInContact; + } + +}; + +#endif \ No newline at end of file diff --git a/include/TerriBull/lib/GameObjects/SpinUp/Goal.hpp b/include/TerriBull/lib/GameObjects/SpinUp/Goal.hpp new file mode 100644 index 0000000..f5822ae --- /dev/null +++ b/include/TerriBull/lib/GameObjects/SpinUp/Goal.hpp @@ -0,0 +1,63 @@ +/** + * @file Goal.hpp + * @author John Koch jkoch21@usf.edu + * @brief Goal Object Class for containing Roller Specific Information + * + * @version 0.1 + * @date 2023-03-13 + * + * @copyright Copyright (c) 2022 + * + */ +#ifndef GOAL_H +#define GOAL_H + +#include "../GameObject.hpp" + +#include + + +class Goal : public TerriBull::GameObject { + public: + typedef enum { + RED, BLUE, NEUTRAL + } Color; + + private: + float pDTheta; + + ~Goal() {} + + public: + + struct UpdateArgs { + float x; + float y; + float dTheta; + }; + + static void* ConstructUpdateArgs(float x, float y, float dtheta) { + size_t size = sizeof(float) * 3; /* Calculate Size*/ + void* data = malloc(size); + size_t offset = 0; + memcpy((char*)data + offset, &x, sizeof(float)); + offset += sizeof(float); + memcpy((char*)data + offset, &y, sizeof(float)); + offset += sizeof(float); + memcpy((char*)data + offset, &dtheta, sizeof(bool)); + return data; + } + + Goal(TerriBull::Vector2* pos, char identifier) : GameObject(pos, identifier, DISK, 8, 8) { } + + float getDTheta() { + return pDTheta; + } + + void setDTheta(float pDTheta) { + this->pDTheta = pDTheta; + } + +}; + +#endif \ No newline at end of file diff --git a/include/TerriBull/lib/KalmanFilter.hpp b/include/TerriBull/lib/KalmanFilter.hpp new file mode 100644 index 0000000..6cbde17 --- /dev/null +++ b/include/TerriBull/lib/KalmanFilter.hpp @@ -0,0 +1,58 @@ +/** + * @file KalmanFilter.hpp + * @author John Koch jkoch21@usf.edu + * @brief 1 Dimensional Kalman Filter class + * + * @version 0.1 + * @date 2023-04-07 + * + * @copyright Copyright (c) 2022 + * + */ +#include +#include + +class KalmanFilter1D { +public: + KalmanFilter1D(float dt, float process_noise, float measurement_noise) { + this->dt = dt; + this->process_noise = process_noise; + this->measurement_noise = measurement_noise; + + x.resize(2, 0.0); + P.resize(2, std::vector(2, 0.0)); + P[0][0] = 1.0; + P[1][1] = 1.0; + } + + void predict() { + x[0] += x[1] * dt; + P[0][0] += dt * (P[1][0] + dt * P[1][1]); + P[0][1] += dt * P[1][1]; + P[1][0] += dt * P[1][1]; + P[1][1] += process_noise * dt * dt; + } + + void update(float measured_angle, float dt) { + this->dt = dt; + float y = measured_angle - x[0]; + float S = P[0][0] + measurement_noise; + float K0 = P[0][0] / S; + float K1 = P[1][0] / S; + x[0] += K0 * y; + x[1] += K1 * y; + P[0][0] -= K0 * P[0][0]; + P[0][1] -= K0 * P[0][1]; + P[1][0] -= K1 * P[0][0]; + P[1][1] -= K1 * P[0][1]; + } + + std::vector getState() { + return x; + } + +private: + float dt, process_noise, measurement_noise; + std::vector x; + std::vector> P; +}; \ No newline at end of file diff --git a/include/TerriBull/lib/SubsystemTools/VoltageRegulator.hpp b/include/TerriBull/lib/SubsystemTools/VoltageRegulator.hpp new file mode 100644 index 0000000..de70520 --- /dev/null +++ b/include/TerriBull/lib/SubsystemTools/VoltageRegulator.hpp @@ -0,0 +1,87 @@ +/** + * @file VoltageRegulator.hpp + * @author John Koch jkoch21@usf.edu + * @brief Abstract Data structure for implementing game objects on the field + * + * @version 0.1 + * @date 2023-01-04 + * + * @copyright Copyright (c) 2022 + * + */ +#ifndef VOLTAGEREGULATOR_H +#define VOLTAGEREGULATOR_H + +#include "../../TerriBull.hpp" + +#include + + +class TerriBull::VoltageRegulator { + private: + float* pCurrentVoltages; + float* pPreviousVoltages; + float* pGradientVoltages; + uint16_t pLength; + float pMaxGradient; + + bool allZeros(float* ptr, uint16_t length) { + for (uint16_t i = 0; i < length; i++) { + if (ptr[i]!= 0) { + return false; + } + } + return true; + } + public: + VoltageRegulator(uint16_t _lengthVoltages, float _maxGradient) : pLength(_lengthVoltages), pMaxGradient(_maxGradient) { + this->pCurrentVoltages = (float*)malloc(sizeof(float) * _lengthVoltages); + this->pPreviousVoltages = (float*)malloc(sizeof(float) * _lengthVoltages); + this->pGradientVoltages = (float*)malloc(sizeof(float) * _lengthVoltages); + + } + + ~VoltageRegulator() { + + } + + float* getRegulatedVoltages(float* targetVoltages) { + /* Check to see if it is a Hard Reset */ + if(allZeros(targetVoltages, pLength) ) { + this->ResetGradients(); + return targetVoltages; + } + /* Copy our Old voltages into our previous container */ + for (uint16_t i = 0; i < this->pLength; i++) this->pPreviousVoltages[i] = this->pCurrentVoltages[i]; + /* Go through Each index and clamp the Gradient to the Max Gradient */ + for (uint16_t i = 0; i < this->pLength; i++) { + /* Handle Case where current voltages are zero */ + if (this->pPreviousVoltages[i] == 0) { + this->pCurrentVoltages[i] = (this->pMaxGradient * this->pMaxGradient ) * targetVoltages[i]; + } + else { + /* Compute the Current index Gradient*/ + this->pGradientVoltages[i] = (targetVoltages[i] - this->pPreviousVoltages[i]); + int dir = this->pGradientVoltages[i]/ fabs(this->pGradientVoltages[i]); + /* Gradient at each index should be the clamped version of the Graident w.r.t the current value */ + this->pGradientVoltages[i] = dir * min(fabs(this->pMaxGradient*this->pPreviousVoltages[i]), fabs(this->pGradientVoltages[i])); + /* Then we add that Gradient to our Current Voltages */ + this->pCurrentVoltages[i] += this->pGradientVoltages[i]; + } + } + return this->pCurrentVoltages; + } + + void ResetGradients() { + for (uint16_t i = 0; i < this->pLength; i++) { /* Initialize to Zero*/ + this->pCurrentVoltages[i] = 0; + this->pPreviousVoltages[i] = 0; + this->pGradientVoltages[i] = 0; + } + } + + + +}; + +#endif \ No newline at end of file diff --git a/include/TerriBull/lib/Tasking/Task.hpp b/include/TerriBull/lib/Tasking/Task.hpp index b41dcb8..b45bc5d 100644 --- a/include/TerriBull/lib/Tasking/Task.hpp +++ b/include/TerriBull/lib/Tasking/Task.hpp @@ -16,7 +16,7 @@ #include "../../TerriBull.hpp" -typedef enum TaskTypes {DRIVE, ROLLER, INTAKE, SHOOTER, TIME, INTERNAL_VARIABLES} TaskTypes; +typedef enum TaskTypes {DRIVE, ROLLER, INTAKE, SHOOTER, TIME, VARIABLE} TaskTypes; class TerriBull::Task { diff --git a/include/TerriBull/lib/Tasking/Types/DriveTask.hpp b/include/TerriBull/lib/Tasking/Types/DriveTask.hpp index aff7fb0..537e151 100644 --- a/include/TerriBull/lib/Tasking/Types/DriveTask.hpp +++ b/include/TerriBull/lib/Tasking/Types/DriveTask.hpp @@ -17,22 +17,29 @@ class TerriBull::DriveTask : public TerriBull::Task { public: - typedef enum {TRANSLATION, ORIENTATION} DriveType; + typedef enum {TRANSLATION, ORIENTATION, OBJECT} DriveType; private: - float approachOrientation; - TerriBull::Vector2* pos; + float targetTheta; + TerriBull::Vector2* v_f; + TerriBull::Vector2* v_i; DriveType driveType; + TerriBull::Vector2* offset; bool calculateOnInit; + bool needsInitialize; bool deleteOnCleanup; bool reversed; + bool lastNeedsCorrection; + bool hitTarget; + DriveTask(TerriBull::MechanicalSystem* _system); public: DriveTask(TerriBull::Vector2* pos, float _orientation, bool reversed, DriveType _driveType, TerriBull::MechanicalSystem* _system); - DriveTask(TerriBull::Vector2 pos, float _orientation, bool reversed, DriveType _driveType, TerriBull::MechanicalSystem* _system); + static DriveTask* GoToObject(TerriBull::GameObject* object, bool reversed,TerriBull::MechanicalSystem* system); + static DriveTask* DynamicInitialize(Vector2* offset, bool reversed, DriveType driveType, TerriBull::MechanicalSystem* system); ~DriveTask(); diff --git a/include/TerriBull/lib/Tasking/Types/RollerTask.hpp b/include/TerriBull/lib/Tasking/Types/RollerTask.hpp index 4facf5e..fc4f06d 100644 --- a/include/TerriBull/lib/Tasking/Types/RollerTask.hpp +++ b/include/TerriBull/lib/Tasking/Types/RollerTask.hpp @@ -17,7 +17,7 @@ class TerriBull::RollerTask : public TerriBull::Task { public: - typedef enum {POS, TIME} RollerType; + typedef enum {POS, TIME, ON, OFF, OBJ} RollerType; private: float* initialPos; float offset; @@ -37,7 +37,8 @@ class TerriBull::RollerTask : public TerriBull::Task { RollerTask(float pos, TerriBull::MechanicalSystem* _system); static RollerTask* DynamicInitialize(float* pos, float offset, TerriBull::MechanicalSystem* _system); - + static RollerTask* TurnOn(int dir, float pwr, TerriBull::MechanicalSystem* _system); + static RollerTask* TurnOff(TerriBull::MechanicalSystem* _system); ~RollerTask(); void init(); diff --git a/include/TerriBull/lib/Tasking/Types/ShooterTask.hpp b/include/TerriBull/lib/Tasking/Types/ShooterTask.hpp index 1eb789e..511c777 100644 --- a/include/TerriBull/lib/Tasking/Types/ShooterTask.hpp +++ b/include/TerriBull/lib/Tasking/Types/ShooterTask.hpp @@ -17,15 +17,13 @@ class TerriBull::ShooterTask : public TerriBull::Task { public: - // typedef enum {ON, } RollerType; + typedef enum {LOAD, SHOOT} ShooterType; private: - // RollerType rollerType; + ShooterType shooterType; public: - ShooterTask(TerriBull::MechanicalSystem* _system); - - + ShooterTask(ShooterType _shooterType, TerriBull::MechanicalSystem* _system); ~ShooterTask(); void init(); diff --git a/include/TerriBull/lib/Tasking/Types/VariableTask.hpp b/include/TerriBull/lib/Tasking/Types/VariableTask.hpp new file mode 100644 index 0000000..2cb53f9 --- /dev/null +++ b/include/TerriBull/lib/Tasking/Types/VariableTask.hpp @@ -0,0 +1,38 @@ +/** + * @file VariableTask.hpp + * @author John Koch jkoch21@usf.edu + * @brief Task Targetting the drive system of the BullBot For modifying program variables. + * + * @version 0.1 + * @date 2023-03-09 + * + * @copyright Copyright (c) 2022 + * + */ +#ifndef VARIABLETASK_H +#define VARIABLETASK_H + +// #include "../Task.hpp" +#include "../../../TerriBull.hpp" + +class TerriBull::VariableTask : public TerriBull::Task { + public: + typedef enum { INT, FLOAT, DOUBLE, CHAR, BOOL } VarType; + private: + void* targetVariable; + void* targetValue; + VarType varType; + public: + + VariableTask(void* targetVariable, void* targetValue, VarType _varType, TerriBull::MechanicalSystem* _system); + + + ~VariableTask(); + + void init(); + void update(float delta); + void terminate(); +}; + + +#endif /* SHOOTERTASK_H */ \ No newline at end of file diff --git a/src/Controllers/InputControllers/AidanJoeShmo.cpp b/src/Controllers/InputControllers/AidanJoeShmo.cpp index 9ad0bea..a162962 100644 --- a/src/Controllers/InputControllers/AidanJoeShmo.cpp +++ b/src/Controllers/InputControllers/AidanJoeShmo.cpp @@ -19,58 +19,54 @@ void AidanJoeShmo::Update(float delta) { int yInput = controller.get_analog(::pros::E_CONTROLLER_ANALOG_LEFT_Y); if (abs(yInput) < deadzone) yInput = 0; if (yInput != 0) { - yInput/=5; + float voltages[] = {0, 0, 0, 0, 0, 0}; drive_engaged = true; - Vector2* currentPos = this->roboController->getSystem()->getPosition(); - int angleMod = (yInput > 0) ? 1 : 0; - Vector2* unitPos = currentPos->unit(); - Vector2* scalePos = *unitPos * yInput; - Vector2* goalPos = *scalePos - *currentPos; - this->roboController->getSystem()->GoToPosition(*goalPos); - delete unitPos; - delete goalPos; - delete scalePos; + yInput/=10.7; + float V = yInput*fabs(yInput); + voltages[0] = V; voltages[1] = V; voltages[2] = V; voltages[3] = V; voltages[4] = V; voltages[5] = V; + this->roboController->getSystem()->getDrive()->setVoltage(voltages); } + if (!drive_engaged) { + int turnInput = controller.get_analog(::pros::E_CONTROLLER_ANALOG_RIGHT_X); + if (abs(turnInput) < deadzone) turnInput = 0; + if (turnInput != 0) { + drive_engaged = true; - int turnInput = controller.get_analog(::pros::E_CONTROLLER_ANALOG_RIGHT_X); - if (abs(turnInput) < deadzone) turnInput = 0; - if (turnInput != 0) { - drive_engaged = true; - - turnInput /= 5; - float angle = fmod((this->roboController->getSystem()->getAngle() - turnInput), 360); - this->roboController->getSystem()->TurnToAngle(angle); + turnInput /= 5; + float angle = fmod((this->roboController->getSystem()->getAngle() - turnInput), 360); + this->roboController->getSystem()->TurnToAngle(angle); + } } - if (!drive_engaged) this->roboController->getSystem()->resetDrive(); + if (!drive_engaged) this->roboController->getSystem()->ResetDrive(); /* Intake */ int in = controller.get_digital(pros::E_CONTROLLER_DIGITAL_X); int out = controller.get_digital(pros::E_CONTROLLER_DIGITAL_Y); if (in || out) { - this->roboController->getSystem()->turnOnIntake(in - out); - } else this->roboController->getSystem()->turnOffIntake(); + this->roboController->getSystem()->TurnOnIntake(in - out); + } else this->roboController->getSystem()->TurnOffIntake(); /* Roller */ int l1 = controller.get_digital(pros::E_CONTROLLER_DIGITAL_L1); int r1 = controller.get_digital(pros::E_CONTROLLER_DIGITAL_R1); if (l1 || r1) { float currentPos = this->roboController->getSystem()->getRoller()->getPos(); - if(this->roboController->getSystem()->spinRollerTo(currentPos + 50*(l1 - r1))) { + if(this->roboController->getSystem()->SpinRollerTo(currentPos + 50*(l1 - r1))) { pros::lcd::set_text(2, "Error with Roller"); }; - } else this->roboController->getSystem()->resetRoller(); + } else this->roboController->getSystem()->ResetRoller(); /* Shooter */ + int shoot = controller.get_digital(pros::E_CONTROLLER_DIGITAL_R2); TerriBull::Shooter* shooter = this->roboController->getSystem()->getShooter(); - if (shooter->isToggled()) { - shooter->Shoot(delta); - if (shooter->shotCompleted()) { - shooter->reset(); - } - } else { - int shoot = controller.get_digital(pros::E_CONTROLLER_DIGITAL_R2); - if (shoot) { + if (shoot || shooter->isToggled() ) { + pros::lcd::set_text(2, "Shooting"); + if (shooter->isLoaded()) { shooter->Shoot(delta); } - } + else { + shooter->Load(delta, nullptr); + } + } + else shooter->reset(); } \ No newline at end of file diff --git a/src/Controllers/InputControllers/DanTheMan.cpp b/src/Controllers/InputControllers/DanTheMan.cpp new file mode 100644 index 0000000..c9cff8e --- /dev/null +++ b/src/Controllers/InputControllers/DanTheMan.cpp @@ -0,0 +1,68 @@ +/** + * @file DanTheMan.cpp + * @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 + * + */ +#include "../../../include/Controllers/InputController/Configurations/DanTheMan.hpp" +void DanTheMan::Init() { + // this->roboController->getTaskManager()->ClearAllTasks(); +} + +void DanTheMan::Update(float delta) { + /* Drive */ + bool drive_engaged = false; + int xInput = controller.get_analog(::pros::E_CONTROLLER_ANALOG_LEFT_X); + int yInput = controller.get_analog(::pros::E_CONTROLLER_ANALOG_LEFT_Y); + if (abs(xInput) < deadzone) xInput = 0; + if (abs(yInput) < deadzone) yInput = 0; + if (xInput || yInput) { + xInput /= 10; + yInput /= 10; + drive_engaged = true; + Vector2* currentPos = this->roboController->getSystem()->getPosition(); + int angleMod = (yInput > 0) ? 1 : 0; + Vector2* dPos = Vector2::cartesianToVector2( xInput, yInput ); + Vector2* goalPos = *currentPos + *dPos; + // this->roboController->getSystem()->GoToPosition(*goalPos); + delete goalPos; + delete dPos; + } + + int turnInput = controller.get_analog(::pros::E_CONTROLLER_ANALOG_RIGHT_X); + if (abs(turnInput) < deadzone) turnInput = 0; + if (turnInput != 0) { + drive_engaged = true; + + turnInput /= 5; + float angle = fmod((this->roboController->getSystem()->getAngle() - turnInput), 360); + this->roboController->getSystem()->TurnToAngle(angle); + } + if (!drive_engaged) this->roboController->getSystem()->ResetDrive(); + + /* Intake */ + int in = controller.get_digital(pros::E_CONTROLLER_DIGITAL_X); + int out = controller.get_digital(pros::E_CONTROLLER_DIGITAL_Y); + if (in || out) { + this->roboController->getSystem()->TurnOnIntake(in - out); + } else this->roboController->getSystem()->TurnOffIntake(); + + /* Shooter */ + // TerriBull::Shooter* shooter = this->roboController->getSystem()->getShooter(); + // if (shooter->isToggled()) { + // shooter->Shoot(delta); + // if (shooter->shotCompleted()) { + // shooter->reset(); + // } + // } else { + // int shoot = controller.get_digital(pros::E_CONTROLLER_DIGITAL_R2); + // if (shoot) { + // shooter->Shoot(delta); + // } + // } +} \ No newline at end of file diff --git a/src/Controllers/InputControllers/NateIsTank.cpp b/src/Controllers/InputControllers/NateIsTank.cpp new file mode 100644 index 0000000..d125c87 --- /dev/null +++ b/src/Controllers/InputControllers/NateIsTank.cpp @@ -0,0 +1,51 @@ +/** + * @file NateIsTank.cpp + * @author John Koch jkoch21@usf.edu + * @brief Bull Two Config for Driver: O' Captian my Captian, Nathaniel + * + * @version 0.1 + * @date 2023-03-10 + * + * @copyright Copyright (c) 2022 + * + */ +#include "../../../include/Controllers/InputController/Configurations/NateIsTank.hpp" +void NateIsTank::Init() { + // this->roboController->getTaskManager()->ClearAllTasks(); +} + +void NateIsTank::Update(float delta) { + /* Drive */ + bool drive_engaged = false; + int lInput = controller.get_analog(::pros::E_CONTROLLER_ANALOG_LEFT_Y); + int rInput = controller.get_analog(::pros::E_CONTROLLER_ANALOG_RIGHT_Y); + if (abs(lInput) < deadzone) lInput = 0; + if (abs(rInput) < deadzone) rInput = 0; + float voltages[] = {0, 0, 0, 0}; + if (lInput != 0) { + drive_engaged = true; + lInput/=10.7; + float lV = lInput*fabs(lInput); + voltages[0] = lV; voltages[1] = lV; + } + if (rInput != 0) { + drive_engaged = true; + rInput/=10.7; + float rV = rInput*fabs(rInput); + voltages[0] = rV; voltages[1] = rV; + } + + if (drive_engaged) this->roboController->getSystem()->getDrive()->setVoltage(voltages); + else this->roboController->getSystem()->ResetDrive(); + + /* Roller */ + int l1 = controller.get_digital(pros::E_CONTROLLER_DIGITAL_L1); + int r1 = controller.get_digital(pros::E_CONTROLLER_DIGITAL_R1); + if (l1 || r1) { + float currentPos = this->roboController->getSystem()->getRoller()->getPos(); + if(this->roboController->getSystem()->SpinRollerTo(currentPos + 50*(l1 - r1))) { + pros::lcd::set_text(2, "Error with Roller"); + }; + } else this->roboController->getSystem()->ResetRoller(); + +} \ No newline at end of file diff --git a/src/Controllers/InputControllers/RyanIsTank.cpp b/src/Controllers/InputControllers/RyanIsTank.cpp new file mode 100644 index 0000000..94fd328 --- /dev/null +++ b/src/Controllers/InputControllers/RyanIsTank.cpp @@ -0,0 +1,69 @@ +/** + * @file RyanIsTank.cpp + * @author John Koch jkoch21@usf.edu + * @brief Bull Two Config for Driver: O' Captian my Captian, Nathaniel + * + * @version 0.1 + * @date 2023-03-10 + * + * @copyright Copyright (c) 2022 + * + */ +#include "../../../include/Controllers/InputController/Configurations/RyanIsTank.hpp" +void RyanIsTank::Init() { + // this->roboController->getTaskManager()->ClearAllTasks(); +} + +void RyanIsTank::Update(float delta) { + /* Drive */ + bool drive_engaged = false; + int lInput = controller.get_analog(::pros::E_CONTROLLER_ANALOG_LEFT_Y); + int rInput = controller.get_analog(::pros::E_CONTROLLER_ANALOG_RIGHT_Y); + if (abs(lInput) < deadzone) lInput = 0; + if (abs(rInput) < deadzone) rInput = 0; + float voltages[] = {0, 0, 0, 0, 0, 0}; + if (lInput != 0) { + drive_engaged = true; + lInput/=10.7; + float lV = lInput*fabs(lInput); + voltages[0] = lV; voltages[1] = lV; voltages[2] = lV; + } + if (rInput != 0) { + drive_engaged = true; + rInput/=10.7; + float rV = rInput*fabs(rInput); + voltages[3] = rV; voltages[4] = rV; voltages[5] = rV; + } + + if (drive_engaged) this->roboController->getSystem()->getDrive()->setVoltage(voltages); + else this->roboController->getSystem()->ResetDrive(); + /* TODO: Change Constriction such that the motor Groups are checked for their Current first.*/ + /* Roller */ + int l1 = controller.get_digital(pros::E_CONTROLLER_DIGITAL_L1); + int r1 = controller.get_digital(pros::E_CONTROLLER_DIGITAL_R1); + if (l1 || r1) { + this->roboController->getSystem()->UnConstrictMotorGroupCurrent(this->roboController->getSystem()->getRoller()->getMotorRefs()); + float currentPos = this->roboController->getSystem()->getRoller()->getPos(); + if(this->roboController->getSystem()->SpinRollerTo(currentPos + 50*(l1 - r1))) { + pros::lcd::set_text(2, "Error with Roller"); + } + } + else { + this->roboController->getSystem()->ResetRoller(); + if (this->roboController->getSystem()->getRoller()->getRPM() < 5) { + this->roboController->getSystem()->ConstrictMotorGroupCurrent(this->roboController->getSystem()->getRoller()->getMotorRefs()); + } + } + /* Shooter */ /*TODO: Change To Toggle */ + if (controller.get_digital(pros::E_CONTROLLER_DIGITAL_A)) { + this->roboController->getSystem()->ConstrictMotorGroupCurrent(this->roboController->getSystem()->getShooter()->getMotorRefs()); + this->roboController->getSystem()->TurnOnShooter(); + this->roboController->getSystem()->getShooter()->Shoot(delta); + } + else { + this->roboController->getSystem()->ResetShooter(); + if (this->roboController->getSystem()->getShooter()->getRPM() < 5) { + this->roboController->getSystem()->ConstrictMotorGroupCurrent(this->roboController->getSystem()->getShooter()->getMotorRefs()); + } + } +} \ No newline at end of file diff --git a/src/Controllers/MechanicalSystem.cpp b/src/Controllers/MechanicalSystem.cpp index 15659fa..9eff161 100644 --- a/src/Controllers/MechanicalSystem.cpp +++ b/src/Controllers/MechanicalSystem.cpp @@ -20,7 +20,9 @@ MechanicalSystem::MechanicalSystem(int _imu, TerriBull::Drive* _drive) : pIntake mu.tare(); pros::delay(2000); /*Drive Setup*/ - this->pDrive = _drive; + this->pDrive = _drive; + /*Initalize*/ + this->pThetaFilter = new KalmanFilter1D(0, 0.2, 0.1); } void MechanicalSystem::Init() { @@ -41,6 +43,10 @@ float MechanicalSystem::getDriveDError() const { return this->pDrive->ROdError(); } +bool MechanicalSystem::DriveNeedsAngleCorrection() const { + return this->pDrive->needsAngleCorrection(); +} + float MechanicalSystem::getRollerError() const { if (this->pRoller != nullptr) return this->pRoller->getError(); return 0; @@ -51,11 +57,22 @@ float MechanicalSystem::getRollerDError() const { return 0; } +float TerriBull::MechanicalSystem::getIntakeRPM() const { + if (this->pIntake!= nullptr) { + return this->pIntake->getRPM(); + } return 0; +} + bool MechanicalSystem::isShotCompleted() const { if (this->pShooter!= nullptr) return this->pShooter->shotCompleted(); return true; } +bool MechanicalSystem::isShooterLoaded() const { + if (this->pShooter!= nullptr) return this->pShooter->isLoaded(); + return true; +} + bool MechanicalSystem::isRollerCompleted() const { if (this->pShooter!= nullptr) return this->pRoller->timeFlag; return true; @@ -65,12 +82,15 @@ TerriBull::Vector2* MechanicalSystem::getPosition() { return this->pPosition; } -void TerriBull::MechanicalSystem::resetDrive() { +void TerriBull::MechanicalSystem::ResetDrive() { this->pDrive->reset(); } float TerriBull::MechanicalSystem::getAngle() { float theta = mu.get_heading(); +// if( this->motherSystem) this->pThetaFilter->update(theta, this->motherSystem->delta()); +// this->pThetaFilter->predict(); +// theta = this->pThetaFilter->getState()[0]; *(this->pAngle) = ::std::fmod(((360 - theta) + this->pStartingAngle), 360.0);//90;// return *(this->pAngle); } @@ -90,39 +110,39 @@ void TerriBull::MechanicalSystem::update(float delta) { } /* MECHANICAL SYSTEM API FUNCTIONS */ -int TerriBull::MechanicalSystem::GoToPosition(Vector2 pos) { - return this->pDrive->drive(pos, this->motherSystem->delta()); +int TerriBull::MechanicalSystem::GoToPosition(Vector2 v_f, Vector2 v_i, bool reverse) { + return this->pDrive->drive(v_f, v_i, this->motherSystem->delta(), reverse); } int TerriBull::MechanicalSystem::TurnToAngle(float theta) { return this->pDrive->change_orientation(theta, this->motherSystem->delta()); } -int TerriBull::MechanicalSystem::turnOnIntake(float direction) { +int TerriBull::MechanicalSystem::TurnOnIntake(float direction) { if (this->pIntake != nullptr) { return this->pIntake->TurnOn(direction); } return -1; } -int TerriBull::MechanicalSystem::turnOffIntake() { +int TerriBull::MechanicalSystem::TurnOffIntake() { if (this->pIntake != nullptr) { return this->pIntake->TurnOff(); } return -1; } -int TerriBull::MechanicalSystem::spinRollerTo(float pos) { +int TerriBull::MechanicalSystem::SpinRollerTo(float pos) { if (this->pRoller != nullptr) { return this->pRoller->SpinToPos(pos); } return -1; } -int TerriBull::MechanicalSystem::spinRollerFor(int direction, float time) { +int TerriBull::MechanicalSystem::SpinRollerFor(int direction, float time) { if (this->pRoller != nullptr) { return this->pRoller->Spin(direction, time, this->motherSystem->delta()); } return -1; } -int TerriBull::MechanicalSystem::resetRoller() { +int TerriBull::MechanicalSystem::ResetRoller() { if (this->pRoller != nullptr) { this->pRoller->reset(); return 0; @@ -135,12 +155,44 @@ int TerriBull::MechanicalSystem::ShootDisk() { } return -1; } -int TerriBull::MechanicalSystem::resetShooter() { +int TerriBull::MechanicalSystem::LoadShooter() { + if (this->pShooter != nullptr) { + return this->pShooter->Load(this->motherSystem->delta(), nullptr); + } return -1; +} + +int TerriBull::MechanicalSystem::TurnOnShooter() { + if (this->pShooter != nullptr) { + return this->pShooter->turnOn(); + } return -1; +} + +int TerriBull::MechanicalSystem::ResetShooter() { if (this->pShooter != nullptr) { return this->pShooter->reset(); } return -1; } +int TerriBull::MechanicalSystem::ConstrictMotorGroupCurrent(TerriBull::MechanicalComponent::MotorRefs* refGroup) { + /* TODO: Log the Constriction to the Log File refGroup.ComponentName */ + for (int i = 0; i < refGroup->NumMotors; i++) { + if (refGroup->Motors[i]!= nullptr) { + refGroup->Motors[i]->set_current_limit(1000); + } + } + return 0; +} + +int TerriBull::MechanicalSystem::UnConstrictMotorGroupCurrent(TerriBull::MechanicalComponent::MotorRefs* refGroup) { + /* TODO: Log the Un-Constriction to the Log File refGroup.ComponentName */ + for (int i = 0; i < refGroup->NumMotors; i++) { + if (refGroup->Motors[i]!= nullptr) { + refGroup->Motors[i]->set_current_limit(2500); + } + } + return 0; +} + /* SETTERS AND GETTERS */ void TerriBull::MechanicalSystem::setMotherSystem(RoboController* _motherSystem) { this->motherSystem = _motherSystem; diff --git a/src/Controllers/ObjectHandler.cpp b/src/Controllers/ObjectHandler.cpp new file mode 100644 index 0000000..4f75ce1 --- /dev/null +++ b/src/Controllers/ObjectHandler.cpp @@ -0,0 +1,103 @@ +/** + * @file ObjectHandler.cpp + * @author John Koch jkoch21@usf.edu + * @brief Controller of all physical components on the BullBot + * + * @version 0.1 + * @date 2023-02-28 + * + * @copyright Copyright (c) 2022 + * + */ +#include "../../include/Controllers/ObjectHandler/ObjectHandler.hpp" +#include +#include +void ObjectHandler::addObject(GameObject* _obj) { + this->pNumObjects++; + this->Objects[_obj->type].push_back(_obj); +} + +GameObject* ObjectHandler::getClosestObjByType(GameObject::Types type, Vector2* currentPos) { + GameObject* closestObj = nullptr; + float minRadius = INFINITY; + vector desired_query = this->Objects[type]; + for( auto it = desired_query.begin(); it!= desired_query.end(); it++) { + GameObject* currentObj = *(it); + Vector2* currentVector = *(currentPos) - *(currentObj->getPosPtr()); + if (closestObj == nullptr || currentVector->r < minRadius) { + closestObj = currentObj; + minRadius = currentVector->r; + } + delete currentVector; + } + return closestObj; +} + + +GameObject* ObjectHandler::getClosestObj(Vector2* currentPos) { + GameObject* closestObj = nullptr; + float minRadius = INFINITY; + for (auto it = this->Objects.begin(); it!= this->Objects.end(); it++) { + for (auto it2 = it->second.begin(); it2!= it->second.end(); it2++) { + GameObject* currentObj = *(it2); + Vector2* currentVector = *(currentPos) - *(currentObj->getPosPtr()); + if (closestObj == nullptr || currentVector->r < minRadius) { + closestObj = currentObj; + minRadius = currentVector->r; + } + delete currentVector; + } + } + return closestObj; +} + +void ObjectHandler::updateObjPos(char identifier, GameObject::Types type, Vector2 pos) { + GameObject* queryObj = this->query(type, identifier); + if (queryObj!= nullptr) { + Vector2* updateVector = queryObj->getPosPtr(); + updateVector->x = pos.x; + updateVector->y = pos.y; + } +} + + +void ObjectHandler::update(GameObject::Types type, char id, void* args) { + GameObject* obj = this->query(type, id); // Find the GameObject with the specified type and id + if(obj != nullptr) { // If the object exists + switch(type) { + case GameObject::Types::DISK: { + Disk* disk = dynamic_cast(obj); + if (disk != nullptr) { + Disk::UpdateArgs* diskArgs = static_cast(args); + disk->pos->x = diskArgs->x; + disk->pos->y = diskArgs->y; + } + break; + } + case GameObject::Types::GOAL: { + Goal* goal = dynamic_cast(obj); + if (goal != nullptr) { + Goal::UpdateArgs* goalArgs = static_cast(args); + goal->pos->x = goalArgs->x; + goal->pos->y = goalArgs->y; + goal->setDTheta(goalArgs->dTheta); + } + break; + } + case GameObject::Types::ROLLER: { + FieldRoller* roller = dynamic_cast(obj); + if (roller != nullptr) { + FieldRoller::UpdateArgs* rollerArgs = static_cast(args); + roller->pos->x = rollerArgs->x; + roller->pos->y = rollerArgs->y; + roller->setCurrentColor((FieldRoller::Color)rollerArgs->color); + roller->setIsInContact(rollerArgs->is_in_contact); + } + break; + } + default: + // Handle unsupported types or throw an exception + break; + } + } +} \ No newline at end of file diff --git a/src/Controllers/RoboController.cpp b/src/Controllers/RoboController.cpp index d47a35b..3a1c64b 100644 --- a/src/Controllers/RoboController.cpp +++ b/src/Controllers/RoboController.cpp @@ -11,6 +11,14 @@ * */ #include "../../include/Controllers/RoboController/RoboController.hpp" +#include "../../include/TerriBull/lib/GameObjects/SpinUp/Disk.hpp" +#include "../../include/TerriBull/lib/GameObjects/SpinUp/FieldRoller.hpp" +#include "../../include/TerriBull/lib/GameObjects/SpinUp/Goal.hpp" +#include +#include +void RoboController::setObjHandler(ObjectHandler* objHandler) { + this->objHandler = objHandler; +} void RoboController::setTaskManager(TaskManager* taskManager) { this->taskManager = taskManager; } @@ -21,9 +29,9 @@ void RoboController::setSerialController(SerialController* serialController) { this->serialController = serialController; } /* Getter Methods */ -// ObjectHandler* getObjHandler() { -// return this->objHandler; -// } +ObjectHandler* RoboController::getObjHandler() { + return this->objHandler; +} TaskManager* RoboController::getTaskManager() { return this->taskManager; } @@ -34,10 +42,17 @@ SerialController* RoboController::getSerialController() { return this->serialController; } +void RoboController::runSerialTask(void* args) { + this->serialController->Update(this->delta()); +} +void RoboController::runMainTask(void* args) { + this->Run(); +} + void TerriBull::RoboController::Init() { - /* TBD <-Fix this file path-> */ - this->configParser = new ConfigurationParser("/usd/configuration.json", "Left_Nut"); + /* * Config File * * Config Type * */ + this->configParser = new ConfigurationParser("/usd/configuration.json", "TinyBull"); if ( this->configParser->success()) { /* Init Mech Sys */ this->system = this->configParser->getMechanicalSystemConfig(); @@ -51,38 +66,128 @@ void TerriBull::RoboController::Init() { if (inputController == nullptr) { // exit(1); } - /* Init Task Manager */ - this->taskManager = new TaskManager(); /* TODO: Needs TaskManager::Init() */ + /* THIS VAR WILL PUT THE SYSTEM INTO DEBUG MODE */ + this->pDebug = false; //false; + /* Init Task Manager */ + this->taskManager = new TaskManager(); /* Init Serial Controller */ - this->serialController = new SerialController(); /* TODO: Needs TaskManager::Init() */ - /* Init Object Handler */ + this->serialController = new SerialController(this); /* TODO: Needs SerialController::Init() */ + //this->serialController->RegisterCallback("serial_test_v5_to_jetson", (SerialController::PacketCallback)SerialTestV5ToJetsonCallback); + //this->serialController->RegisterCallback("serial_test_jetson_to_v5", (SerialController::PacketCallback)SerialTestJetsonToV5Callback); + this->serialController->RegisterCallback("set_disk_obj", (SerialController::PacketCallback)SetDiskObjectCallback); + this->serialController->RegisterCallback("get_disk_obj", (SerialController::PacketCallback)GetDiskObjectCallback); + this->serialController->RegisterCallback("set_goal_obj", (SerialController::PacketCallback)SetGoalObjectCallback); + this->serialController->RegisterCallback("get_goal_obj", (SerialController::PacketCallback)GetGoalObjectCallback); + this->serialController->RegisterCallback("set_roller_obj", (SerialController::PacketCallback)SetRollerObjectCallback); + this->serialController->RegisterCallback("get_roller_obj", (SerialController::PacketCallback)GetRollerObjectCallback); + this->serialController->RegisterCallback("get_pos", (SerialController::PacketCallback)GetPositionCallback); + this->serialController->RegisterCallback("set_pos", (SerialController::PacketCallback)SetPositionCallback); + this->serialController->RegisterCallback("go_to_pos", (SerialController::PacketCallback)GoToPositionCallback); + this->serialController->RegisterCallback("go_to_pos_dx_dy", (SerialController::PacketCallback)GoToPositionDxDyCallback); + this->serialController->RegisterCallback("go_to_pos_dr_dtheta", (SerialController::PacketCallback)GoToPositionDRDThetaCallback); + this->serialController->RegisterCallback("go_to_obj", (SerialController::PacketCallback)GoToObjectCallback); + this->serialController->RegisterCallback("turn_to_angle", (SerialController::PacketCallback)TurnToAngleCallback); + this->serialController->RegisterCallback("spin_roller", (SerialController::PacketCallback)SpinRollerCallback); + this->serialController->RegisterCallback("shoot_disk", (SerialController::PacketCallback)ShootDiskCallback); + this->serialController->RegisterCallback("load_shooter", (SerialController::PacketCallback)LoadShooterCallback); + if(!this->pDebug) this->serialController->ExchangeTags(); + // /* Init Object Handler */ // this->objHandler = new ObjectHandler(); /* TODO: ObjHandler Class Needs serious Update */ /* TASKS SECTION */ - Vector2 *dest1 = Vector2::cartesianToVector2((this->system->getPosition())->x, (this->system->getPosition())->y - 10); - this->taskManager->addTaskSet( - new TaskList({{ - new TerriBull::DriveTask(*dest1, 0, false, TerriBull::DriveTask::TRANSLATION, this->getSystem()), - // new TerriBull::RollerTask(0.75, 1, this->getSystem()) - }}) - ); - Vector2 *dest2 = Vector2::cartesianToVector2(dest1->x + 10, dest1->y); - this->taskManager->addTaskSet( - new TaskList({{ - new TerriBull::DriveTask(*dest2, 420, true, TerriBull::DriveTask::ORIENTATION, this->getSystem()), - // new TerriBull::RollerTask(0.75, 1, this->getSystem()) - }}) - ); + // this->taskManager->addTaskSet(new TaskList({new TerriBull::VariableTask(this->system->getDrive()->getRefMaxSpeed(), new float(70), VariableTask::FLOAT, this->getSystem())})); + // this->taskManager->addTaskSet(new TaskList({new TerriBull::TimeTask(1000, this->getSystem())})); + this->taskManager->addTaskSet(new TaskList({new TerriBull::IntakeTask(1, IntakeTask::ON, this->getSystem())})); + this->taskManager->addTaskSet(new TaskList({new TerriBull::TimeTask(0.5, this->getSystem())})); + this->taskManager->addTaskSet(new TaskList({new TerriBull::RollerTask(1000,this->getSystem())})); + this->taskManager->addTaskSet(new TaskList({new TerriBull::TimeTask(0.5, this->getSystem())})); this->taskManager->addTaskSet( new TaskList({{ - new TerriBull::DriveTask(*dest2, 0, true, TerriBull::DriveTask::TRANSLATION, this->getSystem()), + TerriBull::DriveTask::DynamicInitialize(Vector2::cartesianToVector2(0, -17.0), true, TerriBull::DriveTask::TRANSLATION, this->getSystem()), // new TerriBull::RollerTask(0.75, 1, this->getSystem()) }}) ); - delete dest1; - delete dest2; + // this->taskManager->addTaskSet(new TaskList({new TerriBull::TimeTask(0.5, this->getSystem())})); + + // this->taskManager->addTaskSet( + // new TaskList({{ + // TerriBull::DriveTask::DynamicInitialize(Vector2::cartesianToVector2(-6.8, 0), true, TerriBull::DriveTask::ORIENTATION, this->getSystem()), + // // new TerriBull::RollerTask(0.75, 1, this->getSystem()) + // }}) + // ); + // this->taskManager->addTaskSet(new TaskList({new TerriBull::TimeTask(0.5, this->getSystem())})); + + // this->taskManager->addTaskSet( + // new TaskList({{ + // TerriBull::DriveTask::DynamicInitialize(Vector2::cartesianToVector2(-25, 0), true, TerriBull::DriveTask::TRANSLATION, this->getSystem()), + // // new TerriBull::RollerTask(0.75, 1, this->getSystem()) + // }}) + // ); + + // this->taskManager->addTaskSet(new TaskList({new TerriBull::TimeTask(0.5, this->getSystem())})); + // // this->taskManager->addTaskSet(new TaskList({TerriBull::RollerTask::DynamicInitialize(this->system->getRoller()->posPtr(), -1, this->getSystem() )})); + // this->taskManager->addTaskSet(new TaskList({TerriBull::RollerTask::DynamicInitialize(this->system->getRoller()->posPtr(), -300, this->getSystem() )})); + + // this->taskManager->addTaskSet( + // new TaskList({{ + // TerriBull::DriveTask::DynamicInitialize(Vector2::cartesianToVector2(24, 0), false, TerriBull::DriveTask::TRANSLATION, this->getSystem()), + // // new TerriBull::RollerTask(0.75, 1, this->getSystem()) + // }}) + // ); + // this->taskManager->addTaskSet(new TaskList({new TerriBull::TimeTask(0.5, this->getSystem())})); + + + // this->taskManager->addTaskSet( + // new TaskList({{ + // TerriBull::DriveTask::DynamicInitialize(Vector2::cartesianToVector2(0, 3), true, TerriBull::DriveTask::ORIENTATION, this->getSystem()), + // // new TerriBull::RollerTask(0.75, 1, this->getSystem()) + // }}) + // ); + // this->taskManager->addTaskSet(new TaskList({new TerriBull::TimeTask(0.5, this->getSystem())})); + + // this->taskManager->addTaskSet( + // new TaskList({{ + // TerriBull::DriveTask::DynamicInitialize(Vector2::cartesianToVector2(0, 20), false, TerriBull::DriveTask::TRANSLATION, this->getSystem()), + // // new TerriBull::RollerTask(0.75, 1, this->getSystem()) + // }}) + // ); + + + // this->taskManager->addTaskSet(new TaskList({new TerriBull::TimeTask(0.5, this->getSystem())})); + // this->taskManager->addTaskSet(new TaskList({TerriBull::RollerTask::DynamicInitialize(this->system->getRoller()->posPtr(), -300, this->getSystem() )})); + + // this->taskManager->addTaskSet( + // new TaskList({{ + // TerriBull::DriveTask::DynamicInitialize(Vector2::cartesianToVector2(0, -12), false, TerriBull::DriveTask::TRANSLATION, this->getSystem()), + // // new TerriBull::RollerTask(0.75, 1, this->getSystem()) + // }}) + // ); + + // // this->taskManager->addTaskSet(new TaskList({new TerriBull::TimeTask(0.5, this->getSystem())})); + // this->taskManager->addTaskSet( + // new TaskList({{ + // TerriBull::DriveTask::DynamicInitialize(Vector2::cartesianToVector2(1, 1), false, TerriBull::DriveTask::ORIENTATION, this->getSystem()), + // // new TerriBull::RollerTask(0.75, 1, this->getSystem()) + // }}) + // ); + // this->taskManager->addTaskSet(new TaskList({new TerriBull::TimeTask(0.5, this->getSystem())})); + // this->taskManager->addTaskSet( + // new TaskList({{ + // TerriBull::DriveTask::DynamicInitialize(Vector2::cartesianToVector2(-12, -12), true, TerriBull::DriveTask::TRANSLATION, this->getSystem()), + // // new TerriBull::RollerTask(0.75, 1, this->getSystem()) + // }}) + // ); + + // this->taskManager->addTaskSet(new TaskList({new TerriBull::TimeTask(0.5, this->getSystem())})); + // this->taskManager->addTaskSet( + // new TaskList({{ + // TerriBull::DriveTask::DynamicInitialize(Vector2::cartesianToVector2(0, 1), false, TerriBull::DriveTask::ORIENTATION, this->getSystem()), + // // new TerriBull::RollerTask(0.75, 1, this->getSystem()) + // }}) + // ); + this->previousTime = pros::millis(); this->currentTime = pros::millis(); @@ -97,10 +202,18 @@ void TerriBull::RoboController::Init() { void TerriBull::RoboController::Run() { this->updateTime(); this->system->update(this->delta()); + this->serialController->Update(this->delta()); if (pros::competition::is_autonomous()) { /*TODO: Or engaged Autonomous Control */ this->taskManager->run(this->delta()); } else { - this->inputController->Update(this->delta()); + if(!this->pDebug) this->inputController->Update(this->delta()); + else { + float voltages[] = {0, 0, 0, 0, 0, 0}; + this->system->getDrive()->setVoltage(voltages); + if (controller.get_digital(pros::E_CONTROLLER_DIGITAL_RIGHT)) { + this->taskManager->run(this->delta()); + } + } } // this->serialController->update(); @@ -122,3 +235,345 @@ int TerriBull::RoboController::ClearTasks() { this->taskManager->ClearAllTasks(); return 0; } + + + +/* Callback Methods */ +/** + * @brief Set the Disk Object Callback object + * DATA: [int id, float x, float y] + * @param robot + * @param array + * @param start_index + * @param length + */ +void SetDiskObjectCallback(TerriBull::RoboController* robot, char * array, int start_index, int length) { //[Jetson -> Brain] (id, x, y) + char id = (char) (int) SerialController::DeserializeNumber(array, &start_index); + float x = SerialController::DeserializeNumber(array, &start_index); + float y = SerialController::DeserializeNumber(array, &start_index); + /*Update Sys*/ + void* update_args = Disk::ConstructUpdateArgs(x, y); + robot->getObjHandler()->update(GameObject::ROLLER, id, update_args); + delete static_cast(update_args); +} +/** + * @brief Get the Disk Object Callback object + * DATA: [int id] + * @param robot + * @param array + * @param start_index + * @param length + */ +void GetDiskObjectCallback(TerriBull::RoboController* robot, char * array, int start_index, int length) { //[Brain -> Jetson] (id) + /* Parse for ID */ +} +/** + * @brief Set the Roller Object Callback object + * DATA: [int id, float x, float y, int color, bool is_in_contact(int)] + * @param robot + * @param array + * @param start_index + * @param length + */ +void SetRollerObjectCallback(TerriBull::RoboController* robot, char * array, int start_index, int length) { //[Jetson -> Brain] (id, x, y) + char id = (char) (int) SerialController::DeserializeNumber(array, &start_index); + float x = SerialController::DeserializeNumber(array, &start_index); + float y = SerialController::DeserializeNumber(array, &start_index); + int color = (int) SerialController::DeserializeNumber(array, &start_index); + bool isInContact = (bool) SerialController::DeserializeNumber(array, &start_index); + /*Update Sys*/ + void* update_args = FieldRoller::ConstructUpdateArgs(x, y, color, isInContact); + robot->getObjHandler()->update(GameObject::ROLLER, id, update_args); + delete static_cast(update_args); +} +/** + * @brief Get the Roller Object Callback object + * DATA: [int id] + * @param robot + * @param array + * @param start_index + * @param length + */ +void GetRollerObjectCallback(TerriBull::RoboController* robot, char * array, int start_index, int length) { //[Brain -> Jetson] (id) + /* Send Request*/ +} +/** + * @brief Set the Goal Object Callback object + * DATA: [int id, float x, float y, d_theta] + * @param robot + * @param array + * @param start_index + * @param length + */ +void SetGoalObjectCallback(TerriBull::RoboController* robot, char * array, int start_index, int length) { //[Jetson -> Brain] (id, x, y, color, in_contact) + char id = (char) (int) SerialController::DeserializeNumber(array, &start_index); + float x = SerialController::DeserializeNumber(array, &start_index); + float y = SerialController::DeserializeNumber(array, &start_index); + float dTheta = SerialController::DeserializeNumber(array, &start_index); + /*Update Sys*/ + void* update_args = Goal::ConstructUpdateArgs(x, y, dTheta); + robot->getObjHandler()->update(GameObject::ROLLER, id, update_args); + delete static_cast(update_args); +} +/** + * @brief Get the Goal Object Callback object + * DATA: [int id] + * @param robot + * @param array + * @param start_index + * @param length + */ +void GetGoalObjectCallback(TerriBull::RoboController* robot, char * array, int start_index, int length) { + +} +/** + * @brief Set the Position Callback object + * DATA: [] + * @param robot + * @param array + * @param start_index + * @param length + */ +void SetPositionCallback(TerriBull::RoboController* robot, char * array, int start_index, int length) { + +} +/** + * @brief Get the Position Callback object -> x, y, theta + * DATA: [] + * @param robot + * @param array + * @param start_index + * @param length + */ +void GetPositionCallback(TerriBull::RoboController* robot, char * array, int start_index, int length) { + std::stringstream s3; + Vector2* pos = robot->getSystem()->getPosition(); + float theta = robot->getSystem()->getAngle(); + s3 << (unsigned char) robot->getSerialController()->GetCallbackIndex("get_pos"); + s3 << SerialController::SerializeNumber(pos->x); + s3 << SerialController::SerializeNumber(pos->y); + s3 << SerialController::SerializeNumber(theta); + robot->getSerialController()->SendData(s3.str()); +} + +/* API Callbacks */ +/** + * @brief + * DATA: [float x, float y, bool reversed] <- we could implement as Dx, Dy + * @param robot + * @param array + * @param start_index + * @param length + */ +void GoToPositionCallback(TerriBull::RoboController* robot, char * array, int start_index, int length) { + float x = SerialController::DeserializeNumber(array, &start_index); + float y = SerialController::DeserializeNumber(array, &start_index); + bool reversed = (bool) SerialController::DeserializeNumber(array, &start_index); + Vector2* pos = Vector2::cartesianToVector2(x, y); + robot->getTaskManager()->addTaskSet(new TerriBull::TaskList({ + new TerriBull::DriveTask(*pos, 420, reversed, TerriBull::DriveTask::ORIENTATION, robot->getSystem()), + })); + robot->getTaskManager()->addTaskSet(new TerriBull::TaskList({ + new TerriBull::DriveTask(*pos, 420, reversed, TerriBull::DriveTask::TRANSLATION, robot->getSystem()), + })); + delete pos; +} +/** + * @brief + * DATA: [float x, float y, bool reversed] + * @param robot + * @param array + * @param start_index + * @param length + */ +void GoToPositionDxDyCallback(TerriBull::RoboController* robot, char * array, int start_index, int length) { + float x = SerialController::DeserializeNumber(array, &start_index); + float y = SerialController::DeserializeNumber(array, &start_index); + bool reversed = (bool) SerialController::DeserializeNumber(array, &start_index); + Vector2* pos = Vector2::cartesianToVector2(x, y); + robot->getTaskManager()->addTaskSet(new TerriBull::TaskList({ + TerriBull::DriveTask::DynamicInitialize(pos, reversed, TerriBull::DriveTask::ORIENTATION, robot->getSystem()), + })); + robot->getTaskManager()->addTaskSet(new TerriBull::TaskList({ + TerriBull::DriveTask::DynamicInitialize(pos, reversed, TerriBull::DriveTask::TRANSLATION, robot->getSystem()), + })); + delete pos; +} +/** + * @brief + * DATA: [float r, float d_theta, bool reversed] + * @param robot + * @param array + * @param start_index + * @param length + */ +void GoToPositionDRDThetaCallback(TerriBull::RoboController* robot, char * array, int start_index, int length) { + float r = SerialController::DeserializeNumber(array, &start_index); + float dtheta = SerialController::DeserializeNumber(array, &start_index); + bool reversed = (bool) SerialController::DeserializeNumber(array, &start_index); + Vector2* pos = Vector2::polarToVector2(r, robot->getSystem()->getAngle()+ dtheta); + robot->getTaskManager()->addTaskSet(new TerriBull::TaskList({ + TerriBull::DriveTask::DynamicInitialize(pos, reversed, TerriBull::DriveTask::ORIENTATION, robot->getSystem()), + })); + robot->getTaskManager()->addTaskSet(new TerriBull::TaskList({ + TerriBull::DriveTask::DynamicInitialize(pos, reversed, TerriBull::DriveTask::TRANSLATION, robot->getSystem()), + })); + delete pos; +} +/** + * @brief Create a Object Callback object + * + * @param robot + * @param array + * @param start_index + * @param length + */ +void CreateObjectCallback(TerriBull::RoboController* robot, char * array, int start_index, int length) { + int id = (int) SerialController::DeserializeNumber(array, &start_index); + TerriBull::GameObject::Types objectType = (TerriBull::GameObject::Types) (int) SerialController::DeserializeNumber(array, &start_index); + int x = SerialController::DeserializeNumber(array, &start_index); + int y = SerialController::DeserializeNumber(array, &start_index); + GameObject* obj = nullptr; + Vector2* pos = Vector2::cartesianToVector2(x, y); + switch (objectType) { + case GameObject::Types::DISK: { + obj = new Disk(pos, id); + break; + } + case GameObject::Types::ROLLER: { + obj = new FieldRoller(pos, id); + break; + } + case GameObject::Types::GOAL: { + obj = new Goal(pos, id); + break; + } + default: + break; + } +} +/** + * @brief Goes to an Object based on a identifier and type. + * DATA: [char OBJECT_ID, int OBJECT_TYPE, bool reversed] + * @param robot + * @param array + * @param start_index + * @param length + * NOTE: Note: IT IS NOT RECOMMENDED TO GO TO OBJECTS IN THE REVERSE DIRECTION + */ +void GoToObjectCallback(TerriBull::RoboController* robot, char * array, int start_index, int length) { + int id = (int) SerialController::DeserializeNumber(array, &start_index); + TerriBull::GameObject::Types objectType = (TerriBull::GameObject::Types) (int) SerialController::DeserializeNumber(array, &start_index); + bool reversed = (bool) SerialController::DeserializeNumber(array, &start_index); + TerriBull::GameObject* obj = robot->getObjHandler()->query(objectType, id); + if (obj == nullptr) { return; } /* TODO: Needs ERRNO-like Message Passing */ + robot->getTaskManager()->addTaskSet(new TerriBull::TaskList({ + TerriBull::DriveTask::GoToObject(obj, reversed, robot->getSystem()), + })); +} +/** + * @brief + * DATA: [float angle] <- we could implement as Dtheta + * @param robot + * @param array + * @param start_index + * @param length + */ +void TurnToAngleCallback(TerriBull::RoboController* robot, char * array, int start_index, int length) { + float angle = (int) SerialController::DeserializeNumber(array, &start_index); + bool reversed = (bool) SerialController::DeserializeNumber(array, &start_index); + robot->getTaskManager()->addTaskSet(new TerriBull::TaskList({ + new TerriBull::DriveTask(nullptr, angle, reversed, DriveTask::DriveType::ORIENTATION, robot->getSystem()), + })); +} +/** + * @brief + * DATA: [] + * @param robot + * @param array + * @param start_index + * @param length + */ +void SpinRollerCallback(TerriBull::RoboController* robot, char * array, int start_index, int length) { + int dir = SerialController::DeserializeNumber(array, &start_index); + robot->getSystem()->SpinRollerFor(dir, 1000); /*TODO: Change such that there is a Power Parameter */ +} +/** + * @brief + * DATA: [int GOAL_ID] + * @param robot + * @param array + * @param start_index + * @param length + */ +void ShootDiskCallback(TerriBull::RoboController* robot, char * array, int start_index, int length) { + int goal_id = SerialController::DeserializeNumber(array, &start_index); + TerriBull::GameObject* obj = robot->getObjHandler()->query(GameObject::Types::GOAL, goal_id); + if (obj == nullptr) { return; } /* TODO: Needs ERRNO-like Message Passing */ + robot->getTaskManager()->addTaskSet(new TerriBull::TaskList({ + new TerriBull::ShooterTask(ShooterTask::SHOOT,robot->getSystem()), + })); +} +/** + * @brief + * DATA: [int DISK_ID] + * @param robot + * @param array + * @param start_index + * @param length + */ +void LoadShooterCallback(TerriBull::RoboController* robot, char * array, int start_index, int length) { + int disk_id = SerialController::DeserializeNumber(array, &start_index); + TerriBull::GameObject* obj = robot->getObjHandler()->query(GameObject::Types::DISK, disk_id); + if (obj == nullptr) { return; } /* TODO: Needs ERRNO-like Message Passing */ + robot->getTaskManager()->addTaskSet(new TerriBull::TaskList({ + new TerriBull::ShooterTask(ShooterTask::SHOOT,robot->getSystem()), + })); +} +/* Task Management Callbacks */ +/** + * @brief + * DATA: [] + * @param robot + * @param array + * @param start + * @param length + */ +void ClearTasksCallback(TerriBull::RoboController* robot, char * array, int start, int length) { + robot->getTaskManager()->ClearAllTasks(); +} +/* Other Callbacks */ +void TagExchangeCallback(TerriBull::RoboController* robot, char * array, int start, int length) { + robot->getSerialController()->updateExchangeTags(); +} +/** + * @brief Sends Test Data to the Jetson + * + * @param robot + * @param array + * @param start + * @param length + */ +/* +void SerialTestV5ToJetsonCallback(TerriBull::RoboController* robot, char * array, int start, int length) { + std::stringstream s3; + float theta = robot->getSystem()->getAngle(); + s3 << (unsigned char) robot->getSerialController()->GetCallbackIndex("serial_test_v5_to_jetson"); + s3 << SerialController::SerializeNumber(theta); + s3 << SerialController::SerializeString("Digits of Pi:"); + s3 << SerialController::SerializeNumber(3.14159); + robot->getSerialController()->SendData(s3.str()); +} +*/ +/** + * @brief Recieves Test Data from the Jetson + * + * @param robot + * @param array + * @param start + * @param length + */ +/* +void SerialTestJetsonToV5Callback(TerriBull::RoboController* robot, char * array, int start, int length) { + +}*/ \ No newline at end of file diff --git a/src/Controllers/SerialController.cpp b/src/Controllers/SerialController.cpp index c6f0be3..69ee07e 100644 --- a/src/Controllers/SerialController.cpp +++ b/src/Controllers/SerialController.cpp @@ -1,53 +1,392 @@ /** * @file SerialController.cpp - * @author John Koch jkoch21@usf.edu + * @author John Koch jkoch21@usf.edu, Bill Gate * @brief Manages Serial communication between the V5 and other devices - * + * * @version 0.1 - * @date 2023-02-28 - * + * @date 2023-03-28 + * * @copyright Copyright (c) 2022 - * + * */ +#include +#include +#include + #include "../../include/Controllers/SerialController/SerialController.hpp" -TerriBull::SerialController::SerialController() { + +std::string TerriBull::SerialController::input_buffer; +pros::Mutex TerriBull::SerialController::input_mutex; + +TerriBull::SerialController::SerialController(TerriBull::RoboController *_motherSys) : motherSys(_motherSys), tagExchange(false), isCollectingTags(false) +{ ::pros::c::serctl(SERCTL_DISABLE_COBS, nullptr); + pros::Task input_task(SerialController::read_input_task); +} + +void TerriBull::SerialController::Update(float delta) +{ + this->ReadBuffer(); + for (auto it = this->ScheduledCallbacks.begin(); it != this->ScheduledCallbacks.end(); ++it) + { + ScheduledCallback *callback = *it; + callback->sumTime += delta; + if (callback->sumTime >= callback->frequency) + { + callback->callbackItem->callback(this->motherSys, nullptr, 0, 0); + callback->sumTime = 0; + } + } +} + +bool TerriBull::SerialController::isInitialized() +{ + return this->tagExchange; +} + +bool TerriBull::SerialController::ReadBuffer() +{ + std::string input; + std::unique_lock lock(TerriBull::SerialController::input_mutex); + if (!TerriBull::SerialController::input_buffer.empty()) + { + input = TerriBull::SerialController::input_buffer; + TerriBull::SerialController::input_buffer.clear(); + } + lock.unlock(); + + if (input.size() == 0) + return false; + + // return; + + for (char _int : input) + { + int packet_length = __next_packet.size(); + if (packet_length >= __header_length) + { + __next_packet.push_back(_int); + /* Checks to see if Transmition is terminated */ + if (std::equal(__next_packet.end() - __footer_length, __next_packet.end(), std::begin(__end_of_transmission), + [](int a, char b) + { return a == (int)b; })) + { + this->DeserializePacket(); + __next_packet.clear(); + } + continue; + } + else if (_int == __packet_header[packet_length]) + { + __next_packet.push_back(_int); + continue; + } + __next_packet.clear(); + } + + return true; +} + +// 0 and 10 unusable +// 50-200 data types +// 11-255 data length - 10 +// 11-255 integer - 11 +// 7 for decimal +// 11 end of transmission +// [start][function_id][data_type][data_length][data][end] +std::string TerriBull::SerialController::SerializeNumber(double f) +{ + std::ostringstream stream; + std::string ff = ""; + + if (f == (int)f) + ff = std::to_string((int)f); + else + ff = std::to_string(f); + + for (int c = 0; c < ff.length(); c++) + { + unsigned char chr = ff[c]; + + if (chr >= 48 and chr <= 57) + { + if (c + 1 < ff.length() && ff[c + 1] >= 48 && ff[c + 1] <= 57) + { + unsigned char nchar = ((chr - 48) * 10) + (ff[c + 1] - 37); + stream << (unsigned char)nchar; + c += 1; + } + else + { + stream << (unsigned char)(chr - 37); + } + } + else if (chr == 46) + { + stream << (unsigned char)7; + } + } + std::string ret = stream.str(); + ret = (char)(ret.length() + 10) + ret; + ret = (char)1 + ret; + + return ret; +} + +double TerriBull::SerialController::DeserializeNumber(char *array, int *si) +{ + int length = array[*si] - 10; + (*si)++; + double number = 0; + double decimal = 0; + int decimal_length = 0; + bool switch_to_decimal = false; + for (int i = *si; i < *si + length; i++) + { + if (!switch_to_decimal) + { + if ((int)array[i] != 7) + { + number *= 10; + auto k = (int)(array[i] - 11); + number += k; + if (k > 10 && i < length && array[i+1] - 11 > 10) + { + number *= 10; + } + } + else + { + switch_to_decimal = !switch_to_decimal; + } + } + else + { + if ((int)array[i] != 7) + { + auto k = (int)(array[i] - 11); + auto p = to_string(k).length(); + decimal *= pow(10, p); + decimal += k; + decimal_length += p; + } + else + { + switch_to_decimal = !switch_to_decimal; + } + } + } + int divisor = pow(10, decimal_length); + number += decimal / divisor; + (*si) += length; + + return number; +} + +char *LongerLength(int length) +{ + char first_byte = (length & 0xFF00) >> 8; + char second_byte = length & 0xFF; + return new char[2]{first_byte, second_byte}; +} + +// MAX length 65,535 - 10 +std::string TerriBull::SerialController::SerializeString(std::string s) +{ + std::ostringstream stream; + stream << (unsigned char)2; + char *string_length = LongerLength(s.size() + 12); + stream << (unsigned char)string_length[0]; + stream << (unsigned char)string_length[1]; + stream << s; + return stream.str(); } -void TerriBull::SerialController::update() { - readBuffer(); +std::string TerriBull::SerialController::DeserializeString(char *array, int *si) +{ + std::ostringstream stream; + int length = (array[*si] << 8) + array[(*si) + 1]; + (*si) += 2; + for (int i = *si; i < *si + length; i++) + { + stream << array[i]; + } + (*si) += length; + return stream.str(); } -void TerriBull::SerialController::readBuffer() { - if (this->buffer[0] != '\0' ) { - processDataFromBuffer(); - readBuffer(); +void TerriBull::SerialController::ExchangeTags() +{ + while (!this->tagExchange) + { + if (!this->isCollectingTags) + { + for (auto it = this->Callbacks.begin(); it != this->Callbacks.end(); ++it) + { + CallbackItem *item = it->second; + if (item->callback != nullptr) + { + std::stringstream s3; + s3 << (char)1; + s3 << SerialController::SerializeNumber(this->Callbacks.size() - 1 == it->first ? 0 : 1); + s3 << SerialController::SerializeNumber(it->first); + s3 << SerialController::SerializeString(item->friendly_name); + this->SendData(s3.str()); + } + } + } + this->ReadBuffer(); + pros::delay(5); } - ::std::cin >> this->buffer; } -/* This needs to be changed in acoordance to the data transmittion protocol */ -void TerriBull::SerialController::processDataFromBuffer() { - ::std::vector<::std::string> args; - ::std::string currentArg = ""; - for(int i = 0; this->buffer[i] != '\0' || this->buffer[i] != '\n'; i++) { - if (this->buffer[i] == ',') { - args.push_back(currentArg); - currentArg = ""; +int TerriBull::SerialController::RegisterCallback(std::string tag_name, PacketCallback callback) +{ + bool found = false; + int returnIndex = -1; + for (auto it = this->Callbacks.begin(); it != this->Callbacks.end(); ++it) + { + CallbackItem *item = it->second; + if (item->friendly_name == tag_name) + { + found = true; + break; } - else currentArg+=this->buffer[i]; } - args.push_back(currentArg); // ZYU FORGUT ZE VUN IN ZE CHAMBER - if (args[0] == "update_obj") { - //UPDATE OBJ POS: std::string identifier,int type, float x, float y - // ObjHandler.updateObjPos(args[1],stoi(args[2]), stof(args[3]), stof(args[4])); + if (!found) + { + CallbackItem *item = new CallbackItem(); + item->friendly_name = tag_name; + item->callback = callback; + item->jetson_id = -1; + returnIndex = this->Callbacks.size(); + this->Callbacks[returnIndex] = item; + } + else + { + throw new exception; + } + return returnIndex + SerialController::__packet_index_offset; +} + +void TerriBull::SerialController::DeserializePacket() +{ + int func_id = __next_packet[__header_length]; + int pSize = __next_packet.size() - __header_length; + int end = pSize - __footer_length; + char buffer[end]; + memcpy(buffer, &__next_packet[__header_length + 1], end); + + if (func_id == 1) + { + this->ProcessTagExchange(buffer, 0, end); + } + else if (this->tagExchange && !this->isCollectingTags) + { + auto itr = Callbacks.find(func_id - SerialController::__packet_index_offset); + if (itr == Callbacks.end()) + return; + itr->second->callback(this->motherSys, buffer, 0, end); + } +} + +/** + * @brief + * DATA: [float step, int tag_id, string friendly_name] + * @param array + * @param start_index + * @param length + */ +int k =1; +void TerriBull::SerialController::ProcessTagExchange(char *array, int start_index, int length) +{ + if (!this->isCollectingTags) + { + this->isCollectingTags = true; + this->tmpCallbacks.clear(); + } + + int ind = start_index + 1; + + int step = (int)SerialController::DeserializeNumber(array, &ind); + + ind++; + int tag_id = (int)SerialController::DeserializeNumber(array, &ind); + + ind++; + std::string friendly_name = SerialController::DeserializeString(array, &ind); + + SerialController::CallbackItem *item; + + item = new SerialController::CallbackItem(); + item->jetson_id = tag_id; + item->friendly_name = friendly_name; + + SerialController::CallbackItem* tmp = this->FindInternal(friendly_name); + if (tmp != nullptr) + { + item->callback = tmp->callback; + } + + // pros::lcd::set_text(k++, (to_string(tag_id)+":"+item->friendly_name).c_str()); + + this->tmpCallbacks[tag_id] = item; + + if (step == 0) + { + this->Callbacks.clear(); + for (auto it = this->tmpCallbacks.begin(); it!= this->tmpCallbacks.end(); ++it) + { + this->Callbacks[it->first] = it->second; + } + this->tmpCallbacks.clear(); + this->isCollectingTags = false; + this->tagExchange = true; } - else if (args[0] == "update_pos") { - //UPDATE POS: std::string identifier,float x, float y +} + +SerialController::CallbackItem* TerriBull::SerialController::FindInternal(std::string tag_name) +{ + for (auto it = this->Callbacks.begin(); it != this->Callbacks.end(); ++it) + { + SerialController::CallbackItem *item = it->second; + if (item->friendly_name.find(tag_name) != std::string::npos) + { + return item; + } } + //return nullptr; +} +int TerriBull::SerialController::GetCallbackIndex(std::string tag_name) +{ + for (auto it = this->Callbacks.begin(); it != this->Callbacks.end(); ++it) + { + SerialController::CallbackItem *item = it->second; + + if (item->friendly_name.find(tag_name) != std::string::npos ) + { + if (item->jetson_id == -1) /* We have not initialized the Packet index*/ + return it->first + SerialController::__packet_index_offset; + else /* Packet Callback has been initialized on the Jetson */ + return item->jetson_id + SerialController::__packet_index_offset; + } + } + return -1; } -void TerriBull::SerialController::sendData(::std::string data) { - ::std::cout<sumTime+=delta; pros::lcd::set_text(6, ""+ std::to_string(this->sumTime)); if (!this->shotComplete) { - if (this->sumTime > 1.5) { + if (this->sumTime > 0) { if (this->limitSwitch->get_new_press()) { this->engagedOne = true; this->cntNoVal = 0; @@ -26,17 +26,38 @@ int CatapultZolt::Shoot(float delta) { if (this->engagedOne) { pros::lcd::set_text(6, ""+ std::to_string(this->limitSwitch->get_value())); if (this->limitSwitch->get_value() == 0) this->cntNoVal+=delta; - else this->cntNoVal = 0; - shotComplete = this->cntNoVal > 0.5; + // else this->cntNoVal = 0; + shotComplete = this->cntNoVal > 0.2; + if (shotComplete){ this->loaded = false; this->toggled = false;} } - } - else { - this->pMotorX->move(-100); - this->pMotorY->move(-100); - } + } } return 0; } + +int CatapultZolt::Load(float delta, void* args) { + toggled = true; + this->sumTime+=delta; + this->currentError = 0.3 - this->currentPos; + float pwr = 120; + this->pMotorX->move(pwr); + this->pMotorY->move(pwr); + this->loaded = fabs(this->currentError) < 0.1 || this->limitSwitch->get_value(); + + pros::lcd::set_text(4, ""+std::to_string(this->currentError)); + if (this->loaded) { + this->pMotorX->move(0); + this->pMotorY->move(0); + this->toggled = false; + } + if (this->pMotorX->get_torque() > 0.15) { + float dP = (this->pMotorX->get_position() + this->pMotorY->get_position()) / (2 * ENCODER_UNIT[this->gearSet]); + this->currentPos +=dP; + this->pMotorX->tare_position(); + this->pMotorY->tare_position(); + } + return 0; +} int CatapultZolt::turnOn() { return 0; } @@ -46,10 +67,17 @@ int CatapultZolt::reset() { this->engagedOne = false; this->cntNoVal = 0; this->sumTime = 0; + if(!loaded) this->currentPos = 0; this->pMotorX->move(0); this->pMotorY->move(0); + this->pMotorX->tare_position(); + this->pMotorY->tare_position(); return 0; } bool CatapultZolt::shotCompleted() { return this->shotComplete; +} + +float CatapultZolt::getRPM() { + return 0; /* we dont use catapults :> */ } \ No newline at end of file diff --git a/src/MechanicalComponents/FlyWheelSB.cpp b/src/MechanicalComponents/FlyWheelSB.cpp new file mode 100644 index 0000000..4650394 --- /dev/null +++ b/src/MechanicalComponents/FlyWheelSB.cpp @@ -0,0 +1,58 @@ +/** + * @file FlyWheelSB.cpp + * @author John Koch jkoch21@usf.edu + * @brief Uses a Flywheel to propell the Disc Game Pieces into the goal. + * + * @version 0.1 + * @date 2023-03-01 + * + * @copyright Copyright (c) 2022 + * +*/ +#include "../../include/MechanicalComponents/Shooters/Configurations/FlyWheel/FlyWheelSB.hpp" +int FlyWheelSB::Shoot(float delta) { + toggled = true; + this->sumTime+=delta; + this->pMag->update(delta); + return 0; +} + +int FlyWheelSB::Load(float delta, void* args) { + toggled = true; + this->sumTime+=delta; + /** + * @brief Assuming we are Querying the + * + */ + return 0; +} +int FlyWheelSB::turnOn() { + float pwr = 127; + this->pMotorX->move(pwr); + this->pMotorY->move(pwr); + return 0; +} + +float FlyWheelSB::getRPM() const { + float rpmI = fabs(this->pMotorX->get_actual_velocity()); + float rpmJ = fabs(this->pMotorY->get_actual_velocity()); + return (0.5 *(rpmI + rpmJ)); +} + +int FlyWheelSB::reset() { + this->shotComplete = false; + this->toggled = false; + this->engagedOne = false; + this->cntNoVal = 0; + this->sumTime = 0; + if(!loaded) this->currentPos = 0; + this->pMotorX->move(0); + this->pMotorY->move(0); + this->pMotorX->tare_position(); + this->pMotorY->tare_position(); + this->pMag->reset(); + return 0; +} +bool FlyWheelSB::shotCompleted() { + return this->shotComplete; +} diff --git a/src/MechanicalComponents/Intake_Duo.cpp b/src/MechanicalComponents/Intake_Duo.cpp index a06d54f..c41295b 100644 --- a/src/MechanicalComponents/Intake_Duo.cpp +++ b/src/MechanicalComponents/Intake_Duo.cpp @@ -25,4 +25,9 @@ int Intake_Duo::TurnOff() { this->currentDir = 0; this->toggled = false; return 0; -} \ No newline at end of file +} +float Intake_Duo::getRPM() const { + float rpmI = this->pMotorI->get_actual_velocity(); + float rpmJ = this->pMotorJ->get_actual_velocity(); + return 0.5 *(rpmI + rpmJ); +} diff --git a/src/MechanicalComponents/Intake_Uni.cpp b/src/MechanicalComponents/Intake_Uni.cpp index c59d241..6d1458e 100644 --- a/src/MechanicalComponents/Intake_Uni.cpp +++ b/src/MechanicalComponents/Intake_Uni.cpp @@ -23,4 +23,8 @@ int Intake_Uni::TurnOff() { this->currentDir = 0; this->toggled = false; return 0; +} + +float Intake_Uni::getRPM() const { + return fabs(this->pMotorI->get_actual_velocity()); } \ No newline at end of file diff --git a/src/MechanicalComponents/RollerDuo.cpp b/src/MechanicalComponents/RollerDuo.cpp index 5617ba3..e368c11 100644 --- a/src/MechanicalComponents/RollerDuo.cpp +++ b/src/MechanicalComponents/RollerDuo.cpp @@ -26,6 +26,18 @@ int Roller_Duo::Spin(int direction, float time, float delta) { return 0; } +int Roller_Duo::TurnOn(int direction, float pwr) { + this->pMotorI->move(direction * pwr); + this->pMotorJ->move(direction * pwr); + return 0; +} + +float Roller_Duo::getRPM() const { + float rpmI = this->pMotorI->get_actual_velocity(); + float rpmJ = this->pMotorJ->get_actual_velocity(); + return 0.5 *(rpmI + rpmJ); +} + int Roller_Duo::SpinToPos(float pos) { this->currentError = pos - this->currentPos; this->sumError += this->currentError; @@ -48,6 +60,6 @@ void Roller_Duo::reset() { void Roller_Duo::update() { float dPos = (this->pMotorI->get_position() + this->pMotorJ->get_position()) / 2; - this->currentPos += dPos * (2*PI) / ENCODER_UNIT[this->gearSet]; + this->currentPos += 100 * dPos * (2*PI) / ENCODER_UNIT[this->gearSet]; this->pMotorI->tare_position(); } diff --git a/src/MechanicalComponents/RollerUni.cpp b/src/MechanicalComponents/RollerUni.cpp index 0038ab0..10e5db0 100644 --- a/src/MechanicalComponents/RollerUni.cpp +++ b/src/MechanicalComponents/RollerUni.cpp @@ -18,8 +18,16 @@ int Roller_Uni::Spin(int direction, float time, float delta) { this->update(); return 0; } +int Roller_Uni::TurnOn(int direction, float pwr) { + this->pMotorI->move(direction * pwr); + return 0; +} + +float Roller_Uni::getRPM() const { + return this->pMotorI->get_actual_velocity(); +} -int Roller_Uni::SpinToPos(float pos) { +int Roller_Uni::SpinToPos(float pos) {/* TODO: Fix this */ this->currentError = pos - this->currentPos; this->sumError += this->currentError; float pwr = kP*this->currentError + kI*this->sumError + kD*this->dError(); @@ -38,6 +46,6 @@ void Roller_Uni::reset() { void Roller_Uni::update() { float dPos = this->pMotorI->get_position(); - this->currentPos += dPos * (2*PI) / ENCODER_UNIT[this->gearSet]; + this->currentPos += 25 * dPos * (2*PI) / ENCODER_UNIT[this->gearSet]; this->pMotorI->tare_position(); } diff --git a/src/MechanicalComponents/tank_drive_quad.cpp b/src/MechanicalComponents/tank_drive_quad.cpp index 27cec7b..90107f8 100644 --- a/src/MechanicalComponents/tank_drive_quad.cpp +++ b/src/MechanicalComponents/tank_drive_quad.cpp @@ -22,10 +22,10 @@ Tank_Drive_Quad::~Tank_Drive_Quad() { void Tank_Drive_Quad::setVoltage(float* vals) { /* Less than some threshold */ float lt = vals[0]; float lb = vals[1];float rt = vals[2]; float rb = vals[3]; - if (fabs(lt) > motorPowerThreshold) lt = motorPowerThreshold * fabs(lt) / lt; - if (fabs(rt) > motorPowerThreshold) rt = motorPowerThreshold * fabs(rt) / rt; - if (fabs(lb) > motorPowerThreshold) lb = motorPowerThreshold * fabs(lb) / lb; - if (fabs(rb) > motorPowerThreshold) rb = motorPowerThreshold * fabs(rb) / rb; + if (fabs(lt) > this->maxSpeed) lt = this->maxSpeed * fabs(lt) / lt; + if (fabs(rt) > this->maxSpeed) rt = this->maxSpeed * fabs(rt) / rt; + if (fabs(lb) > this->maxSpeed) lb = this->maxSpeed * fabs(lb) / lb; + if (fabs(rb) > this->maxSpeed) rb = this->maxSpeed * fabs(rb) / rb; std::stringstream s3; s3 << std::fixed << ::std::setprecision(2); s3 << "Left: "<< lt << " Right: " << rt; @@ -36,11 +36,11 @@ void Tank_Drive_Quad::setVoltage(float* vals) { this->pMotorD->move(rb); } -int Tank_Drive_Quad::drive(TerriBull::Vector2 pos, float delta) { +int Tank_Drive_Quad::drive(TerriBull::Vector2 v_f, TerriBull::Vector2 v_i, float delta, bool reverse) { /* Theta of desired Modified By our current Look Angle */ float* vals = new float[6]; float pct = 0; - Vector2* dP = (pos - *(this->pCurrentPos)); + Vector2* dP = (v_f - *(this->pCurrentPos)); float difToZero = fabs(GetDTheta(RAD2DEG(dP->theta), *(this->pCurrentAngle))); float difToBack = fabs(GetDTheta(RAD2DEG(dP->theta), fmod(*(this->pCurrentAngle)+ 180, 360))); int errorMod = (difToZero < difToBack) ? 1 : -1; @@ -54,8 +54,8 @@ int Tank_Drive_Quad::drive(TerriBull::Vector2 pos, float delta) { /* Basic PID Equation */ pct = kP*currentError + kI*this->sumError + kD*this->dError() / delta; - if (fabs(pct) > 127) {/* Clmp Pwr to 127 */ - pct = 127 * fabs(pct) / pct; + if (fabs(pct) > this->maxSpeed) {/* Clmp Pwr to 127 */ + pct = this->maxSpeed * fabs(pct) / pct; } float pL = pct; float pR = pct; @@ -69,7 +69,7 @@ int Tank_Drive_Quad::drive(TerriBull::Vector2 pos, float delta) { pR -= MIN(fabs(this->kPThetaTranslation*offTrack), fabs(0.1* pct)) * dir * errorMod; } else if (fabs(dP->r) > 1.0 && offTrack > 90) { - this->change_orientation(fmod(RAD2DEG(dP->theta) + angleMod, 360), delta); + this->maneuverAngle(fmod(RAD2DEG(dP->theta) + angleMod, 360), delta); delete[] vals; delete dP; return 0; @@ -88,7 +88,7 @@ int Tank_Drive_Quad::change_orientation(float theta, float delta) { float* vals = new float[6]; this->currentError = GetDTheta(theta, *(this->pCurrentAngle)); this->sumError += this->currentError; - float pwr = this->kPTheta * this->currentError + this->kI * this->sumError + this->kDTheta * this->dError(); + float pwr = this->kPTheta * this->currentError + this->kITheta * this->sumError + this->kDTheta * this->dError() / delta; std::stringstream s3; s3 << std::fixed << ::std::setprecision(1); s3 << "Err: "<< this->currentError << " Pwr: " << pwr; @@ -104,6 +104,23 @@ int Tank_Drive_Quad::change_orientation(float theta, float delta) { return 0; } +void Tank_Drive_Quad::maneuverAngle(float theta, float delta) { + float* vals = new float[4]; + this->currentError = GetDTheta(theta, *(this->pCurrentAngle)); + this->sumError += this->currentError; + float pwr = this->kPTheta * this->currentError + this->kI * this->sumError + this->kDTheta * this->dError() / delta; + std::stringstream s3; + s3 << std::fixed << ::std::setprecision(1); + s3 << "Err: "<< this->currentError << " Pwr: " << pwr; + pros::lcd::set_text(4,s3.str()); + vals[0] = -pwr; + vals[1] = -pwr; + vals[2] = pwr; + vals[3] = pwr; + this->setVoltage(vals); + delete[] vals; +} + void Tank_Drive_Quad::reset() { this->pMotorA->move(0); this->pMotorB->move(0); @@ -119,8 +136,8 @@ Vector2* Tank_Drive_Quad::resultant_vector() { float l2 = this->pMotorB->get_position(); float r1 = this->pMotorC->get_position(); float r2 = this->pMotorD->get_position(); - float left = ((l1 + l2) / 2) * this->wheelRadius * this->conversionFactor / ENCODER_UNIT[this->gearSet]; /* Assuming Radius of wheel is 5 */ - float right = ((r1 + r2) / 2) * this->wheelRadius * this->conversionFactor / ENCODER_UNIT[this->gearSet]; /* Assuming Radius of wheel is 5 */ + float left = ((l1 + l2) / 2) * 2 * PI * this->wheelRadius * this->conversionFactor / ENCODER_UNIT[this->gearSet]; /* Assuming Radius of wheel is 5 */ + float right = ((r1 + r2) / 2) * 2 * PI * this->wheelRadius * this->conversionFactor / ENCODER_UNIT[this->gearSet]; /* Assuming Radius of wheel is 5 */ int leftDir = fabs(left) / left; int rightDir = fabs(right) / right; float leftAngle = (leftDir > 0) ? *(this->pCurrentAngle) : fmod((180 + *(this->pCurrentAngle)), 360.0); @@ -137,3 +154,11 @@ Vector2* Tank_Drive_Quad::resultant_vector() { delete vecUnNormalized; return resultantVector; } + +float Tank_Drive_Quad::getRPM() const { + float rpmI = fabs(this->pMotorA->get_actual_velocity()); + float rpmJ = fabs(this->pMotorB->get_actual_velocity()); + float rpmK = fabs(this->pMotorC->get_actual_velocity()); + float rpmL = fabs(this->pMotorD->get_actual_velocity()); + return 0.25 *(rpmI + rpmJ + rpmJ + rpmK); +} diff --git a/src/MechanicalComponents/tank_drive_std.cpp b/src/MechanicalComponents/tank_drive_std.cpp index 83095b8..0685d8b 100644 --- a/src/MechanicalComponents/tank_drive_std.cpp +++ b/src/MechanicalComponents/tank_drive_std.cpp @@ -23,17 +23,20 @@ Tank_Drive_Std::~Tank_Drive_Std() { void Tank_Drive_Std::setVoltage(float* vals) { /* Less than some threshold */ + /*TODO: Implement VoltageRegulator Class that Registers each of these voltages as a */ + if (fabs(vals[0]) > this->maxSpeed) vals[0] = this->maxSpeed * fabs(vals[0]) / vals[0]; + if (fabs(vals[3]) > this->maxSpeed) vals[3] = this->maxSpeed * fabs(vals[3]) / vals[3]; + if (fabs(vals[1]) > this->maxSpeed) vals[1] = this->maxSpeed * fabs(vals[1]) / vals[1]; + if (fabs(vals[4]) > this->maxSpeed) vals[4] = this->maxSpeed * fabs(vals[4]) / vals[4]; + if (fabs(vals[2]) > this->maxSpeed) vals[2] = this->maxSpeed * fabs(vals[2]) / vals[2]; + if (fabs(vals[5]) > this->maxSpeed) vals[5] = this->maxSpeed * fabs(vals[5]) / vals[5]; + if (this->pUseVoltageRegulator) vals = this->pVoltageRegulator->getRegulatedVoltages(vals); float lt = vals[0]; float lm = vals[1];float lb = vals[2]; float rt = vals[3]; float rm = vals[4]; float rb = vals[5]; - if (fabs(lt) > motorPowerThreshold) lt = motorPowerThreshold * fabs(lt) / lt; - if (fabs(rt) > motorPowerThreshold) rt = motorPowerThreshold * fabs(rt) / rt; - if (fabs(lm) > motorPowerThreshold) lm = motorPowerThreshold * fabs(lm) / lm; - if (fabs(rm) > motorPowerThreshold) rm = motorPowerThreshold * fabs(rm) / rm; - if (fabs(lb) > motorPowerThreshold) lb = motorPowerThreshold * fabs(lb) / lb; - if (fabs(rb) > motorPowerThreshold) rb = motorPowerThreshold * fabs(rb) / rb; + std::stringstream s3; s3 << std::fixed << ::std::setprecision(2); s3 << "Left: "<< lm << " Right: " << rm; - pros::lcd::set_text(5, s3.str()); + pros::lcd::set_text(6, s3.str()); this->pMotorA->move(lt); this->pMotorB->move(lm); this->pMotorC->move(lb); @@ -42,42 +45,57 @@ void Tank_Drive_Std::setVoltage(float* vals) { this->pMotorF->move(rb); } -int Tank_Drive_Std::drive(TerriBull::Vector2 pos, float delta) { - /* Theta of desired Modified By our current Look Angle */ +int Tank_Drive_Std::drive(TerriBull::Vector2 v_f, TerriBull::Vector2 v_i, float delta, bool reverse) { +/* Theta of desired Modified By our current Look Angle */ float* vals = new float[6]; float pct = 0; - Vector2* dP = (pos - *(this->pCurrentPos)); - float difToZero = fabs(GetDTheta(RAD2DEG(dP->theta), *(this->pCurrentAngle))); - float difToBack = fabs(GetDTheta(RAD2DEG(dP->theta), fmod(*(this->pCurrentAngle)+ 180, 360))); - int errorMod = (difToZero < difToBack) ? 1 : -1; - if (dP->r > errorMod) targetDirection = errorMod; - this->currentError = dP->r * errorMod; /* First Part: Absolute Displacement, Second Part: Positive or Negative */ - this->sumError+=currentError; + Vector2* v_to_goal = (v_f - *(this->pCurrentPos)); + Vector2* v_i_to_goal = (v_f - v_i); + Vector2* v_i_to_bot = (*(this->pCurrentPos) - v_i); + int rev = (reverse)? -1 : 1; + /* Overshooting: Bot traveled further than distance, and the Vectors of displacement are somewhat in the same direction */ + int errorMod = (v_i_to_goal->r < v_i_to_bot->r) ? 1 : -1; + // if (v_to_goal->r > errorMod) targetDirection = errorMod; + this->currentError = v_to_goal->r * errorMod; /* First Part: Absolute Displacement, Second Part: Positive or Negative */ + // this->sumError+=currentError; + // this->sumError = std::clamp(this->sumError, -200.0f, 200.0f); std::stringstream s3; s3 << std::fixed << ::std::setprecision(1); - s3 << "Err: "<< this->currentError << " Mod: " << errorMod << "|" << RAD2DEG(dP->theta) << " " << dP->r; + s3 << "Err: "<< this->currentError << " Mod: " << errorMod << "|" << RAD2DEG(v_to_goal->theta) << " " << v_to_goal->r; pros::lcd::set_text(4,s3.str()); /* Basic PID Equation */ - pct = kP*currentError + kI*this->sumError + kD*this->dError() / delta; - - if (fabs(pct) > 127) {/* Clmp Pwr to 127 */ - pct = 127 * fabs(pct) / pct; + pct = kP*currentError + kD*this->dError() / delta; + if (fabs(pct) > this->maxSpeed) {/* Clmp Pwr to this->maxSpeed */ + pct = this->maxSpeed * fabs(pct) / pct; } float pL = pct; float pR = pct; - int angleMod = (errorMod > 0 ) ? 0 : 180; - float offTrack = GetDTheta(RAD2DEG(dP->theta), fmod(*(this->pCurrentAngle) + angleMod, 360)); - if (fabs(dP->r) > 3) { + int angleMod = (rev > 0) ? 0 : 180; + float offTrack = GetDTheta(RAD2DEG(v_to_goal->theta), fmod(*(this->pCurrentAngle) + angleMod, 360)); + if (fabs(v_to_goal->r) > 6.0 && fabs(offTrack) > 30 ) { + pros::lcd::set_text(4,"+++ in wrong 1 case ++++"); + // this->reset(); + this->pNeedsAngleCorrection = true; + delete v_to_goal; + delete v_i_to_goal; + delete v_i_to_bot; + return 0; + } + else if (fabs(offTrack) < 15) { + // pros::lcd::set_text(4,"--- in target case ---"); int dir = fabs(offTrack)/offTrack; - pL *= 0.95; - pR *= 0.95; - pL += MIN(fabs(this->kPThetaTranslation*offTrack), fabs(0.1* pct)) * dir * errorMod; - pR -= MIN(fabs(this->kPThetaTranslation*offTrack), fabs(0.1* pct)) * dir * errorMod; + pL *= 0.5; + pR *= 0.5; + pL += MIN(fabs(this->kPThetaTranslation*offTrack), fabs(0.05* pct)) * dir * errorMod * rev; + pR -= MIN(fabs(this->kPThetaTranslation*offTrack), fabs(0.05* pct)) * dir * errorMod * rev; } - else if (fabs(dP->r) > 1.0 && offTrack > 90) { - this->change_orientation(fmod(RAD2DEG(dP->theta) + angleMod, 360), delta); - delete[] vals; - delete dP; + else if (fabs(offTrack) >= 15) { + pros::lcd::set_text(4,"+++ in wrong 2 case ++++"); + // this->reset(); + this->pNeedsAngleCorrection = true; + delete v_to_goal; + delete v_i_to_goal; + delete v_i_to_bot; return 0; } @@ -90,7 +108,9 @@ int Tank_Drive_Std::drive(TerriBull::Vector2 pos, float delta) { vals[5] = pR; this->setVoltage(vals); delete[] vals; - delete dP; + delete v_to_goal; + delete v_i_to_goal; + delete v_i_to_bot; return 0; } @@ -98,7 +118,7 @@ int Tank_Drive_Std::change_orientation(float theta, float delta) { float* vals = new float[6]; this->currentError = GetDTheta(theta, *(this->pCurrentAngle)); this->sumError += this->currentError; - float pwr = this->kPTheta * this->currentError + this->kI * this->sumError + this->kDTheta * this->dError() / delta; + float pwr = this->kPTheta * this->currentError + this->kITheta * this->sumError + this->kDTheta * this->dError() / delta; std::stringstream s3; s3 << std::fixed << ::std::setprecision(1); s3 << "Err: "<< this->currentError << " Pwr: " << pwr; @@ -114,6 +134,40 @@ int Tank_Drive_Std::change_orientation(float theta, float delta) { return 0; } +void Tank_Drive_Std::maneuverAngle(float theta, float delta, float r, int errorMod) { + float Kr = 0.028; + float* vals = new float[6]; + this->currentError = GetDTheta(theta, *(this->pCurrentAngle)); + this->sumError += this->currentError; + float pwr = r*Kr*(this->kPTheta * this->currentError + this->kDTheta * this->dError() / delta); + float thirdPwr = -pwr*0.3; + std::stringstream s3; + s3 << std::fixed << ::std::setprecision(1); + s3 << "Err: "<< this->currentError << " Pwr: " << pwr; + pros::lcd::set_text(4,s3.str()); + float lPwr = 0 , rPwr = 0; + if (errorMod > 0) { + lPwr = MAX(0, pwr); + rPwr = MIN(pwr, 0); + lPwr = (lPwr == 0) ? thirdPwr : lPwr; + rPwr = (rPwr == 0)? thirdPwr : rPwr; + } + else if (errorMod < 0) { + lPwr = MIN(pwr, 0); + rPwr = MAX(0, pwr); + lPwr = (lPwr == 0)? thirdPwr : lPwr; + rPwr = (rPwr == 0)? thirdPwr : rPwr; + } + vals[0] = lPwr; + vals[1] = lPwr; + vals[2] = lPwr; + vals[3] = rPwr; + vals[4] = rPwr; + vals[5] = rPwr; + this->setVoltage(vals); + delete[] vals; +} + void Tank_Drive_Std::reset() { this->pMotorA->move(0); this->pMotorB->move(0); @@ -124,6 +178,10 @@ void Tank_Drive_Std::reset() { this->currentError = 0; this->sumError = 0; this->previousError = 0; + this->pNeedsAngleCorrection = false; + if (this->pUseVoltageRegulator) { + this->pVoltageRegulator->ResetGradients(); + } } Vector2* Tank_Drive_Std::resultant_vector() { @@ -133,8 +191,8 @@ Vector2* Tank_Drive_Std::resultant_vector() { float r1 = this->pMotorD->get_position(); float r2 = this->pMotorE->get_position(); float r3 = this->pMotorF->get_position(); - float left = ((l1 + l2 + l3) / 3) * this->wheelRadius * this->conversionFactor / ENCODER_UNIT[this->gearSet]; /* Assuming Radius of wheel is 5 */ - float right = ((r1 + r2 + r3) / 3) * this->wheelRadius * this->conversionFactor / ENCODER_UNIT[this->gearSet]; /* Assuming Radius of wheel is 5 */ + float left = ((l1 + l2 + l3) / 3) * 2 * PI * this->wheelRadius * this->conversionFactor / ENCODER_UNIT[this->gearSet]; /* Assuming Radius of wheel is 5 */ + float right = ((r1 + r2 + r3) / 3) * 2 * PI * this->wheelRadius * this->conversionFactor / ENCODER_UNIT[this->gearSet]; /* Assuming Radius of wheel is 5 */ int leftDir = fabs(left) / left; int rightDir = fabs(right) / right; float leftAngle = (leftDir > 0) ? *(this->pCurrentAngle) : fmod((180 + *(this->pCurrentAngle)), 360.0); @@ -151,3 +209,149 @@ Vector2* Tank_Drive_Std::resultant_vector() { delete vecUnNormalized; return resultantVector; } + +float Tank_Drive_Std::getRPM() const { + float rpmI = fabs(this->pMotorA->get_actual_velocity()); + float rpmJ = fabs(this->pMotorB->get_actual_velocity()); + float rpmK = fabs(this->pMotorC->get_actual_velocity()); + float rpmL = fabs(this->pMotorD->get_actual_velocity()); + return 0.25 *(rpmI + rpmJ + rpmJ + rpmK); +} +/// Old Drive Code using PID +///* Theta of desired Modified By our current Look Angle */ +// float* vals = new float[6]; +// float pct = 0; +// Vector2* v_to_goal = (v_f - *(this->pCurrentPos)); +// Vector2* v_i_to_goal = (v_f - v_i); +// Vector2* v_i_to_bot = (*(this->pCurrentPos) - v_i); +// int8_t rev = (reverse)? -1 : 1; +// /* Overshooting: Bot traveled further than distance, and the Vectors of displacement are somewhat in the same direction */ +// int8_t errorMod = (v_i_to_goal->r < v_i_to_bot->r && *(v_i_to_goal) * *( v_i_to_bot) > 0.5) ? 1 : -1; +// // if (v_to_goal->r > errorMod) targetDirection = errorMod; +// this->currentError = v_to_goal->r * errorMod; /* First Part: Absolute Displacement, Second Part: Positive or Negative */ +// this->sumError+=currentError; +// this->sumError = std::clamp(this->sumError, -200.0f, 200.0f); +// std::stringstream s3; +// s3 << std::fixed << ::std::setprecision(1); +// s3 << "Err: "<< this->currentError << " Mod: " << errorMod << "|" << RAD2DEG(v_to_goal->theta) << " " << v_to_goal->r; +// pros::lcd::set_text(4,s3.str()); +// /* Basic PID Equation */ +// pct = kP*currentError + kI*this->sumError + kD*this->dError() / delta; +// if (fabs(pct) > this->maxSpeed) {/* Clmp Pwr to this->maxSpeed */ +// pct = this->maxSpeed * fabs(pct) / pct; +// } +// float pL = pct*rev; +// float pR = pct*rev; +// int angleMod = (rev > 0) ? 0 : 180; +// float offTrack = GetDTheta(RAD2DEG(v_to_goal->theta), fmod(*(this->pCurrentAngle) + angleMod, 360))*v_to_goal->r; +// if (fabs(v_to_goal->r) > 6.0 && offTrack > 65 ) { +// this->pNeedsAngleCorrection = true; +// float resetVoltages[] = {0, 0, 0, 0, 0, 0}; +// this->setVoltage(resetVoltages); +// delete v_to_goal; +// delete v_i_to_goal; +// delete v_i_to_bot; +// return 0; +// } +// else if (fabs(v_to_goal->r) > 3) { +// int dir = fabs(offTrack)/offTrack; +// pL *= 0.78; +// pR *= 0.78; +// pL += MIN(fabs(this->kPThetaTranslation*offTrack), fabs(0.1* pct)) * dir * errorMod * rev; +// pR -= MIN(fabs(this->kPThetaTranslation*offTrack), fabs(0.1* pct)) * dir * errorMod * rev; +// } +// // else if (fabs(v_to_goal->r) > 0.5 && fabs(offTrack) > 15) { +// // this->pNeedsAngleCorrection = true; +// // float resetVoltages[] = {0, 0, 0, 0, 0, 0}; +// // this->setVoltage(resetVoltages); +// // delete v_to_goal; +// // delete v_i_to_goal; +// // delete v_i_to_bot; +// // return 0; +// // } +// // else if (fabs(dP->r) > 0.5 && offTrack > 10) { +// // this->maneuverAngle(fmod(RAD2DEG(dP->theta) + angleMod, 360), delta, dP->r, errorMod); +// // delete[] vals; +// // delete dP; +// // return 0; +// // } +// vals[0] = pL; +// vals[1] = pL; +// vals[2] = pL; + +// vals[3] = pR; +// vals[4] = pR; +// vals[5] = pR; +// this->setVoltage(vals); +// delete[] vals; +// delete v_to_goal; +// delete v_i_to_goal; +// delete v_i_to_bot; +// return 0; +// } + +// /** +// * @brief Equations: Applied Kinematics +// * @cite Control of Mobile Robots https://drive.google.com/file/d/1Aakq3O7vaGR-X-FKcNERPgBE4h5oWohU/view?usp=share_link +// * @author https://www.usf.edu/engineering/cse/people/weitzenfeld-alfredo.aspx +// */ +// int angleMod = (reverse) ? 0 : 0; +// int rev = (reverse)? -1 : 1; +// Vector2* dPos = v_f - *(this->pCurrentPos); +// Vector2* dPos_Unit = dPos->unit(); +// Vector2* dPos_Scaled = (*dPos_Unit)*(3); /* 3 Inches ahead of Center*/ +// float currentAngle = DEG2RAD(fmod(*(this->pCurrentAngle) + angleMod, 360.0)); +// float deltaAngle = GetDTheta(RAD2DEG(dPos_Unit->theta), RAD2DEG(currentAngle)); +// float ICC_r = dPos_Scaled->r / (2 * sin(deltaAngle)); +// Vector2* ICC = Vector2::cartesianToVector2(dPos_Scaled->x-(0.5*this->wheelBase)*sin(deltaAngle), dPos_Scaled->y+(0.5*this->wheelBase)*cos(deltaAngle)); +// matrix ICC_Matrix = {{cos(deltaAngle), sin(deltaAngle), 0}, {-sin(deltaAngle), cos(deltaAngle), 0}, {0, 0, 1}}; +// matrix radial_Matrix = {{}}; +// float omega = deltaAngle / ICC_r; +// float vLeft = omega * (ICC_r + (this->wheelBase / 2)); +// float vRight = omega * (ICC_r - (this->wheelBase / 2)); +// /* +// Account for 0 division +// */ +// this->currentError = dPos->r; +// float leftProportional = vLeft / vRight; +// float rightProportional = vRight / vLeft; +// float y_t = this->kP * this->currentError + this->kD * this->dError() / delta; +// /* +// We need to Scale our vLeft and vRight such that the Proportion is maintained but the power is relative to y_t +// */ +// if (vLeft == 0 || vRight == 0 || fabs(fabs(vLeft) - fabs(vRight)) < 0.01) { +// vLeft = y_t; +// vRight = y_t; +// } +// else if (fabs(leftProportional) > fabs(rightProportional)) { /* Lets start the scale on the Left Side */ +// vLeft = std::clamp(vLeft*y_t, -this->maxSpeed, this->maxSpeed);// * rev; +// vRight = vLeft * rightProportional;// * rev; +// } +// else { /* Lets start the scale on the Right Side */ +// vRight = std::clamp(vRight*y_t, -this->maxSpeed, this->maxSpeed);// * rev; +// vLeft = vRight * leftProportional;// * rev; +// } +// if (reverse) { +// vLeft = -vRight; +// vRight = -vLeft; +// } +// std::stringstream s3; +// s3 << std::fixed << ::std::setprecision(1); +// s3 << "Err: "<< this->currentError << " / " << vLeft << " | " << vRight << " / r:" << dPos->r; +// pros::lcd::set_text(4,s3.str()); +// std::stringstream debugOutput; +// debugOutput << std::fixed << ::std::setprecision(1); +// debugOutput << "o: " << omega; +// debugOutput << " IC->r: " << ICC->r; +// debugOutput << "lP: " << leftProportional; +// debugOutput << "rP: " << rightProportional; +// pros::lcd::set_text(5, debugOutput.str()); +// float voltages[] = {vLeft, vLeft, vLeft, vRight, vRight, vRight}; +// this->setVoltage(voltages); +// /* Clean Memory */ +// delete dPos; +// delete dPos_Unit; +// delete dPos_Scaled; +// delete ICC; +// return 0; +// } \ No newline at end of file diff --git a/src/MechanicalComponents/x_drive.cpp b/src/MechanicalComponents/x_drive.cpp index 097904f..782c4b9 100644 --- a/src/MechanicalComponents/x_drive.cpp +++ b/src/MechanicalComponents/x_drive.cpp @@ -10,7 +10,8 @@ * */ #include "../../include/MechanicalComponents/Drive/configurations/x_drive.hpp" - +#include +#include X_Drive::~X_Drive() { delete this->pMotorA; delete this->pMotorB; @@ -21,16 +22,14 @@ X_Drive::~X_Drive() { void X_Drive::setVoltage(float* vals) { /* Less than some threshold */ float lt = vals[0];float lb = vals[1];float rt = vals[2];float rb = vals[3]; - if (fabs(lt) < motorPowerThreshold && fabs(rt) < motorPowerThreshold && - fabs(lb) < motorPowerThreshold && fabs(rb) < motorPowerThreshold) { - lt = lb = rt = rb = 0; - } - else { - } - if (fabs(lt) > motorPowerThreshold) lt = motorPowerThreshold * fabs(lt) / lt; - if (fabs(rt) > motorPowerThreshold) rt = motorPowerThreshold * fabs(rt) / rt; - if (fabs(lb) > motorPowerThreshold) lb = motorPowerThreshold * fabs(lb) / lb; - if (fabs(rb) > motorPowerThreshold) rb = motorPowerThreshold * fabs(rb) / rb; + if (fabs(lt) > this->maxSpeed) lt = this->maxSpeed * fabs(lt) / lt; + if (fabs(rt) > this->maxSpeed) rt = this->maxSpeed * fabs(rt) / rt; + if (fabs(lb) > this->maxSpeed) lb = this->maxSpeed * fabs(lb) / lb; + if (fabs(rb) > this->maxSpeed) rb = this->maxSpeed * fabs(rb) / rb; + std::stringstream s3; + s3 << std::fixed << ::std::setprecision(1); + s3 << "pWRS: " << lt << " " << lb << " " << rt << " " << rb; + pros::lcd::set_text(6,s3.str()); this->pMotorA->move(lt); this->pMotorB->move(lb); this->pMotorC->move(rt); @@ -38,22 +37,25 @@ void X_Drive::setVoltage(float* vals) { } -int X_Drive::drive(TerriBull::Vector2 pos, float delta) { +int X_Drive::drive(TerriBull::Vector2 v_f, TerriBull::Vector2 v_i, float delta, bool reverse) { /* Theta of desired Modified By our current Look Angle */ float* vals = new float[4]; - float angle = pos.theta - *(this->pCurrentAngle); - angle = (angle<0) ? 360.0 + angle : angle; + Vector2* dP = (v_f - *(this->pCurrentPos)); + double angle = fmod((RAD2DEG(dP->theta) + (*(this->pCurrentAngle)- 90)) , 360.0); + angle = (angle < 0)? angle + 360.0 : angle; int x = int(round(angle/45)) % 8; int dir = 0; float pct = 0; /* Basic PID Equation */ - Vector2* dP = pos - *(this->pCurrentPos); this->currentError = dP->r; // ::pros::lcd::set_text(6, "Drive Error: " + std::to_string(this->pCurrentPos->theta)); this->sumError+=currentError; - pct = kP*currentError + kI*this->sumError + kD*this->dError(); - // ::pros::lcd::set_text(7, "Drive Pct: " + std::to_string(pct)); + pct = kP*this->currentError + kI*this->sumError + kD*this->dError() / delta; + std::stringstream s3; + s3 << std::fixed << ::std::setprecision(1); + s3 << "Err: "<< this->currentError << "|" << x <<" " << angle; + pros::lcd::set_text(4,s3.str()); /* X Drive Drives in all 8 directions, pick the direction an apply the right direction modifier */ switch(x) { @@ -78,6 +80,7 @@ int X_Drive::drive(TerriBull::Vector2 pos, float delta) { vals[0] = 0; vals[1] = -pct*dir; vals[2] = -pct*dir; vals[3] = 0; break; default: + // pros::lcd::set_text(4,"DEFAULT"); delete[] vals; return -1; } @@ -96,8 +99,8 @@ int X_Drive::change_orientation(float theta, float delta) { ::pros::lcd::set_text(7, "Drive Pct: " + std::to_string(pwr)); vals[0] = -pwr * fabs(this->currentError)/this->currentError; vals[1] = 0; - vals[2] = pwr * fabs(this->currentError)/this->currentError; - vals[3] = 0; + vals[2] = 0; + vals[3] = pwr * fabs(this->currentError)/this->currentError; this->setVoltage(vals); delete[] vals; @@ -113,3 +116,37 @@ void X_Drive::reset() { this->sumError = 0; this->previousError = 0; } + +float X_Drive::getRPM() const { + float rpmI = fabs(this->pMotorA->get_actual_velocity()); + float rpmJ = fabs(this->pMotorB->get_actual_velocity()); + float rpmK = fabs(this->pMotorC->get_actual_velocity()); + float rpmL = fabs(this->pMotorD->get_actual_velocity()); + return 0.25 *(rpmI + rpmJ + rpmJ + rpmK); +} + +Vector2* X_Drive::resultant_vector() { + // return Vector2::cartesianToVector2(0, 0); + float l1 = this->pMotorA->get_position(); + float l2 = this->pMotorB->get_position(); + float r1 = this->pMotorC->get_position(); + float r2 = this->pMotorD->get_position(); + float left = ((l1 + r2) / 2) * 2 * PI * this->wheelRadius * this->conversionFactor / ENCODER_UNIT[this->gearSet]; /* Assuming Radius of wheel is 5 */ + float right = ((l2 + r1) / 2) * 2 * PI * this->wheelRadius * this->conversionFactor / ENCODER_UNIT[this->gearSet]; /* Assuming Radius of wheel is 5 */ + + int leftDir = fabs(left) / left; + int rightDir = fabs(right) / right; + float leftAngle = (leftDir > 0) ? fmod((-45 + *(this->pCurrentAngle)), 360.0) : fmod((135 + *(this->pCurrentAngle)), 360.0); + float rightAngle = (rightDir > 0)? fmod((45 + *(this->pCurrentAngle)), 360.0) : fmod((225 + *(this->pCurrentAngle)), 360.0); + Vector2* vecLeft = Vector2::polarToVector2(fabs(left), DEG2RAD(leftAngle)); + Vector2* vecRight = Vector2::polarToVector2(fabs(right), DEG2RAD(rightAngle)); + Vector2* vecUnNormalized = *vecLeft + *vecRight; + // Vector2* resultantVector = *vecUnNormalized * 0.5; + + /* Cleanup */ + this->tare_encoders(); + delete vecLeft; + delete vecRight; + return vecUnNormalized; +} + diff --git a/src/Serial/RegisteredCallbacks.cpp b/src/Serial/RegisteredCallbacks.cpp new file mode 100644 index 0000000..dd94ea8 --- /dev/null +++ b/src/Serial/RegisteredCallbacks.cpp @@ -0,0 +1,56 @@ +// #include "pros/apix.h" +// #include "../../include/TerriBull/TerriBull.hpp" +// #include +// #include +// #include +// #include +// #include +// #include +// #include + + +// /** +// * Runs initialization code. This occurs as soon as the program is started. +// * +// * All other competition modes are blocked by initialize; it is recommended +// * to keep execution time for this mode under a few seconds. +// */ + +// // EXAMPLE CALLBACK +// void Callback(char * array, int start_index, int length) +// { +// int* i = new int(start_index); +// std::string param1 = SerialController::DeserializeString(array, i); +// double param2 = SerialController::DeserializeNumber(array, i); +// double param3 = SerialController::DeserializeNumber(array, i); + +// //function(param1, param2, param3); +// } + +// void initialize() +// { +// pros::c::serctl(SERCTL_ENABLE_COBS, nullptr); +// pros::delay(2000); +// SerialController Serial = SerialController(); + +// Serial.RegisterCallback("test", (SerialController::PacketCallback)Callback); + +// while(true) +// { +// std::cout << (unsigned char) Serial.GetCallbackIndex("test"); +// std::cout << SerialController::SerializeNumber(45); +// std::cout << SerialController::SerializeNumber(115.411); +// std::cout << SerialController::SerializeNumber(678676766.676786828); +// std::cout << SerialController::SerializeString("Hello World!"); +// std::cout << (char)11 << (char)11 << std::endl; +// Serial.readBuffer(); +// } +// /* Registering Callbacks 1-1*/ +// std::stringstream s3; +// s3 << (unsigned char)1; +// s3 << SerialController::SerializeNumber(45); +// s3 << SerialController::SerializeNumber(115.411); +// s3 << SerialController::SerializeNumber(678676766.676786828); +// s3 << SerialController::SerializeString("Hello World!") << (char)11 << (char)11 << std::endl; +// std::cout << s3.str(); +// } diff --git a/src/TerriBull/TerriBull.cpp b/src/TerriBull/TerriBull.cpp index b81d479..8cc5cfe 100644 --- a/src/TerriBull/TerriBull.cpp +++ b/src/TerriBull/TerriBull.cpp @@ -40,7 +40,6 @@ namespace TerriBull { Logger logger("/usd/logfile.log"); /* Global Logger */ ::pros::Controller controller(::pros::E_CONTROLLER_MASTER); /* Global Controller */ pros::Imu mu(11); - }; #endif #ifndef __TERRIBULL_FUNCTIONS__ @@ -56,13 +55,26 @@ namespace TerriBull { } float GetDTheta(float tf, float ti) { - float positiveDTheta = fmod((tf+360)-ti, 360.0); - float negativeDTheta = -360 + positiveDTheta; - - if (fabs(positiveDTheta) <= fabs(negativeDTheta)) - return positiveDTheta; - else return negativeDTheta; + float positiveDTheta = fmod((tf+360)-ti, 360.0); + float negativeDTheta = -360 + positiveDTheta; + if (fabs(positiveDTheta) <= fabs(negativeDTheta)) + return positiveDTheta; + else return negativeDTheta; + } + matrix matrix_mult(matrix &M1, int M1_rows, int M1_cols, matrix &M2, int M2_rows, int M2_cols) { + if (M1_cols != M2_rows) { + throw runtime_error("The number of columns in M1 must match the number of rows in M2 for matrix multiplication"); + } + matrix result(M1_rows, std::vector(M2_cols, 0)); + for (int i = 0; i < M1_rows; i++) { + for (int j = 0; j < M2_cols; j++) { + for (int k = 0; k < M1_cols; k++) { + result[i][j] += M1[i][k] * M2[k][j]; + } + } + } + return result; } }; #endif \ No newline at end of file diff --git a/src/TerriBull/lib/ConfigurationParser.cpp b/src/TerriBull/lib/ConfigurationParser.cpp index f87eb3a..e5d72a3 100644 --- a/src/TerriBull/lib/ConfigurationParser.cpp +++ b/src/TerriBull/lib/ConfigurationParser.cpp @@ -28,9 +28,15 @@ /* Shooter */ #include "../../../include/MechanicalComponents/Shooters/Configurations/Catapult/CatapultZolt.hpp" +#include "../../../include/MechanicalComponents/Shooters/Configurations/FlyWheel/FlyWheelSB.hpp" +#include "../../../include/MechanicalComponents/Shooters/Magazines/Magazine.hpp" /* Ctrls*/ #include "../../../include/Controllers/InputController/Configurations/AidanJoeShmo.hpp" +#include "../../../include/Controllers/InputController/Configurations/DanTheMan.hpp" +#include "../../../include/Controllers/InputController/Configurations/NateIsTank.hpp" +#include "../../../include/Controllers/InputController/Configurations/RyanIsTank.hpp" + /*END INCLUDE*/ ConfigurationParser::~ConfigurationParser () { @@ -39,21 +45,22 @@ ConfigurationParser::~ConfigurationParser () { } TerriBull::Drive* ConfigurationParser::getDriveConfig() { - if (this->pConfigVariables.DriveConfig.isNull() || this->pConfigVariables.DriveMotorPorts.isNull() || this->pConfigVariables.DriveMotorGearset.isNull() || this->pConfigVariables.DriveWheelRadius.isNull() || this->pConfigVariables.DriveConversionFactor.isNull()) { + if (this->pConfigVariables.DriveConfig.Config.isNull() || this->pConfigVariables.DriveConfig.MotorPorts.isNull() || this->pConfigVariables.DriveConfig.MotorGearset.isNull() || this->pConfigVariables.DriveConfig.WheelRadius.isNull() || this->pConfigVariables.DriveConfig.WheelRadius.isNull() || this->pConfigVariables.DriveConfig.ConversionFactor.isNull()) { this->errCode = VARIABLE_PARSE_ERROR; return nullptr; } - Json::String DriveType = this->pConfigVariables.DriveConfig.asString(); - Json::String gearSet = this->pConfigVariables.DriveMotorGearset.asString(); + Json::String DriveType = this->pConfigVariables.DriveConfig.Config.asString(); + Json::String gearSet = this->pConfigVariables.DriveConfig.MotorGearset.asString(); + /* Drive Configurations that are Currently Supported */ if (DriveType == "x_drive") { - return new X_Drive(this->pConfigVariables.DriveMotorPorts[0].asInt(), this->pConfigVariables.DriveMotorPorts[1].asInt(), this->pConfigVariables.DriveMotorPorts[2].asInt(), this->pConfigVariables.DriveMotorPorts[3].asInt(), GEAR_ENCODER[gearSet], this->pConfigVariables.DriveConversionFactor.asFloat(), this->pConfigVariables.DriveWheelRadius.asFloat()); + return new X_Drive(this->pConfigVariables.DriveConfig.MotorPorts[0].asInt(), this->pConfigVariables.DriveConfig.MotorPorts[1].asInt(), this->pConfigVariables.DriveConfig.MotorPorts[2].asInt(), this->pConfigVariables.DriveConfig.MotorPorts[3].asInt(), GEAR_ENCODER[gearSet], this->pConfigVariables.DriveConfig.ConversionFactor.asFloat(), this->pConfigVariables.DriveConfig.WheelRadius.asFloat(), this->pConfigVariables.DriveConfig.WheelBase.asFloat(), this->pConfigVariables.DriveConfig.KPos[0].asFloat(), this->pConfigVariables.DriveConfig.KPos[1].asFloat(), this->pConfigVariables.DriveConfig.KPos[2].asFloat(), this->pConfigVariables.DriveConfig.KTheta[0].asFloat(), this->pConfigVariables.DriveConfig.KTheta[1].asFloat(), this->pConfigVariables.DriveConfig.KTheta[2].asFloat()); } if (DriveType == "tank_drive_std") { - return new Tank_Drive_Std(this->pConfigVariables.DriveMotorPorts[0].asInt(), this->pConfigVariables.DriveMotorReverse[0].asBool(), this->pConfigVariables.DriveMotorPorts[1].asInt(), this->pConfigVariables.DriveMotorReverse[1].asBool(), this->pConfigVariables.DriveMotorPorts[2].asInt(), this->pConfigVariables.DriveMotorReverse[2].asBool(), this->pConfigVariables.DriveMotorPorts[3].asInt(), this->pConfigVariables.DriveMotorReverse[3].asBool(), this->pConfigVariables.DriveMotorPorts[4].asInt(), this->pConfigVariables.DriveMotorReverse[4].asBool(), this->pConfigVariables.DriveMotorPorts[5].asInt(), this->pConfigVariables.DriveMotorReverse[5].asBool(), GEAR_ENCODER[gearSet], this->pConfigVariables.DriveConversionFactor.asFloat(), this->pConfigVariables.DriveWheelRadius.asFloat()); + return new Tank_Drive_Std(this->pConfigVariables.DriveConfig.MotorPorts[0].asInt(), this->pConfigVariables.DriveConfig.MotorReverse[0].asBool(), this->pConfigVariables.DriveConfig.MotorPorts[1].asInt(), this->pConfigVariables.DriveConfig.MotorReverse[1].asBool(), this->pConfigVariables.DriveConfig.MotorPorts[2].asInt(), this->pConfigVariables.DriveConfig.MotorReverse[2].asBool(), this->pConfigVariables.DriveConfig.MotorPorts[3].asInt(), this->pConfigVariables.DriveConfig.MotorReverse[3].asBool(), this->pConfigVariables.DriveConfig.MotorPorts[4].asInt(), this->pConfigVariables.DriveConfig.MotorReverse[4].asBool(), this->pConfigVariables.DriveConfig.MotorPorts[5].asInt(), this->pConfigVariables.DriveConfig.MotorReverse[5].asBool(), GEAR_ENCODER[gearSet], this->pConfigVariables.DriveConfig.ConversionFactor.asFloat(), this->pConfigVariables.DriveConfig.WheelRadius.asFloat(), this->pConfigVariables.DriveConfig.WheelBase.asFloat(), this->pConfigVariables.DriveConfig.KPos[0].asFloat(), this->pConfigVariables.DriveConfig.KPos[2].asFloat(), this->pConfigVariables.DriveConfig.KPos[2].asFloat(), this->pConfigVariables.DriveConfig.KTheta[0].asFloat(), this->pConfigVariables.DriveConfig.KTheta[1].asFloat(), this->pConfigVariables.DriveConfig.KTheta[2].asFloat()); } if (DriveType == "tank_drive_quad") { - return new Tank_Drive_Quad(this->pConfigVariables.DriveMotorPorts[0].asInt(), this->pConfigVariables.DriveMotorReverse[0].asBool(), this->pConfigVariables.DriveMotorPorts[1].asInt(), this->pConfigVariables.DriveMotorReverse[1].asBool(), this->pConfigVariables.DriveMotorPorts[2].asInt(), this->pConfigVariables.DriveMotorReverse[2].asBool(), this->pConfigVariables.DriveMotorPorts[3].asInt(), this->pConfigVariables.DriveMotorReverse[3].asBool(), GEAR_ENCODER[gearSet], this->pConfigVariables.DriveConversionFactor.asFloat(), this->pConfigVariables.DriveWheelRadius.asFloat()); + return new Tank_Drive_Quad(this->pConfigVariables.DriveConfig.MotorPorts[0].asInt(), this->pConfigVariables.DriveConfig.MotorReverse[0].asBool(), this->pConfigVariables.DriveConfig.MotorPorts[1].asInt(), this->pConfigVariables.DriveConfig.MotorReverse[1].asBool(), this->pConfigVariables.DriveConfig.MotorPorts[2].asInt(), this->pConfigVariables.DriveConfig.MotorReverse[2].asBool(), this->pConfigVariables.DriveConfig.MotorPorts[3].asInt(), this->pConfigVariables.DriveConfig.MotorReverse[3].asBool(), GEAR_ENCODER[gearSet], this->pConfigVariables.DriveConfig.ConversionFactor.asFloat(), this->pConfigVariables.DriveConfig.WheelRadius.asFloat(), this->pConfigVariables.DriveConfig.WheelBase.asFloat(), this->pConfigVariables.DriveConfig.KPos[0].asFloat(), this->pConfigVariables.DriveConfig.KPos[2].asFloat(), this->pConfigVariables.DriveConfig.KPos[2].asFloat(), this->pConfigVariables.DriveConfig.KTheta[0].asFloat(), this->pConfigVariables.DriveConfig.KTheta[1].asFloat(), this->pConfigVariables.DriveConfig.KTheta[2].asFloat()); } else return nullptr; } @@ -71,7 +78,15 @@ TerriBull::InputController* ConfigurationParser::getInputControllerConfig(RoboCo if (ConfigType == "AidanJoeShmo") { return new AidanJoeShmo(roboController, deadzone); } - + if (ConfigType == "DanTheMan") { + return new DanTheMan(roboController, deadzone); + } + if (ConfigType == "NateIsTank") { + return new NateIsTank(roboController, deadzone); + } + if (ConfigType == "RyanIsTank") { + return new RyanIsTank(roboController, deadzone); + } return nullptr; } @@ -112,7 +127,7 @@ TerriBull::Roller* ConfigurationParser::getRollerConfig() { return nullptr; } -TerriBull::Shooter* ConfigurationParser::getShooterConfig() { +TerriBull::Shooter* ConfigurationParser::getShooterConfig(TerriBull::MechanicalSystem* _system) { Json::Value ShooterConfig = this->pConfigVariables.Config["mechanical_system"]["shooter"]; if (ShooterConfig.isNull()) { this->errCode = VARIABLE_PARSE_ERROR; @@ -124,6 +139,22 @@ TerriBull::Shooter* ConfigurationParser::getShooterConfig() { if (ConfigType == "CatapultZolt") { return new CatapultZolt(ShooterConfig["motor_ports"][0].asInt(), ShooterConfig["reverse_motors"][0].asBool(), ShooterConfig["motor_ports"][1].asInt(), ShooterConfig["reverse_motors"][1].asBool(), ShooterConfig["limit_port"].asString()[0], GEAR_ENCODER[gearSet]); } + else if (ConfigType == "ShooterFlyWheelSB") { + /* Parse for the Magazine as well*/ + Json::Value MagazineConfig = this->pConfigVariables.Config["mechanical_system"]["magazine"]; + int cnt_inc = MagazineConfig["inc_threshold"].size(); + int cnt_dec = MagazineConfig["dec_threshold"].size(); + float* increment_thresholds = new float[cnt_inc]; + float* decrement_thresholds = new float[cnt_dec]; + for (int i = 0; i < cnt_inc; i++) { + increment_thresholds[i] = MagazineConfig["inc_threshold"][i].asFloat(); + } + for (int i = 0; i < cnt_dec; i++) { + decrement_thresholds[i] = MagazineConfig["dec_threshold"][i].asFloat(); + } + Magazine* mag = new Magazine(MagazineConfig["incPorts"].asString(), increment_thresholds, MagazineConfig["decPorts"].asString(), decrement_thresholds); + return new FlyWheelSB(ShooterConfig["motor_ports"][0].asInt(), ShooterConfig["reverse_motors"][0].asBool(), ShooterConfig["motor_ports"][1].asInt(), ShooterConfig["reverse_motors"][1].asBool(), mag, GEAR_ENCODER[gearSet], _system); + } return nullptr; } @@ -135,12 +166,12 @@ TerriBull::MechanicalSystem* ConfigurationParser::getMechanicalSystemConfig() { } TerriBull::Drive* drive = this->getDriveConfig(); if (drive == nullptr) { - pros::lcd::set_text(3, "Null Drivebase : " + this->pConfigVariables.DriveConfig.asString()); + pros::lcd::set_text(3, "Null Drivebase : " + this->pConfigVariables.DriveConfig.Config.asString()); this->errCode = VARIABLE_PARSE_ERROR; return nullptr; } if (this->pConfigVariables.StartingAngle.isNull() || this->pConfigVariables.StartingPos.isNull()) { - pros::lcd::set_text(3, "Null Start Info : " + this->pConfigVariables.DriveConfig.asString()); + pros::lcd::set_text(3, "Null Start Info : " + this->pConfigVariables.DriveConfig.Config.asString()); this->errCode = VARIABLE_PARSE_ERROR; return nullptr; } @@ -168,7 +199,7 @@ TerriBull::MechanicalSystem* ConfigurationParser::getMechanicalSystemConfig() { } else if (ref == "shooter") { - TerriBull::Shooter* shooter = this->getShooterConfig(); + TerriBull::Shooter* shooter = this->getShooterConfig(system); if (shooter!= nullptr) { system->setShooter(shooter); } diff --git a/src/TerriBull/lib/Tasking/DriveTask.cpp b/src/TerriBull/lib/Tasking/DriveTask.cpp index 35c04b7..c358b2c 100644 --- a/src/TerriBull/lib/Tasking/DriveTask.cpp +++ b/src/TerriBull/lib/Tasking/DriveTask.cpp @@ -10,11 +10,16 @@ * */ #include "../../../../include/TerriBull/lib/Tasking/Types/DriveTask.hpp" + +#include +#include +DriveTask::DriveTask(TerriBull::MechanicalSystem* _system) : Task(DRIVE, _system), needsInitialize(false) {} + /* Make Orientation 420 to Have it auto Calculate. */ -DriveTask::DriveTask(TerriBull::Vector2* pos, float _orientation, bool reversed, DriveType _driveType, TerriBull::MechanicalSystem* _system) : Task(DRIVE, _system), approachOrientation(_orientation), driveType(_driveType), calculateOnInit(false), reversed(reversed) { - this->pos = pos; +DriveTask::DriveTask(TerriBull::Vector2* pos, float _orientation, bool reversed, DriveType _driveType, TerriBull::MechanicalSystem* _system) : Task(DRIVE, _system), targetTheta(_orientation), driveType(_driveType), calculateOnInit(false), reversed(reversed), needsInitialize(false) { + this->v_f = pos; this->deleteOnCleanup = false; - this->approachOrientation = _orientation; + this->targetTheta = _orientation; this->deleteOnCleanup = true; if (_orientation == 420) { calculateOnInit = true; @@ -22,8 +27,8 @@ DriveTask::DriveTask(TerriBull::Vector2* pos, float _orientation, bool reversed, } /* Make Orientation 420 to Have it auto Calculate based on the position provided. */ -DriveTask::DriveTask(TerriBull::Vector2 pos, float _orientation, bool reversed, DriveType _driveType, TerriBull::MechanicalSystem* _system) : Task(DRIVE, _system), approachOrientation(_orientation), driveType(_driveType), calculateOnInit(false), reversed(reversed) { - this->pos = new TerriBull::Vector2(pos); +DriveTask::DriveTask(TerriBull::Vector2 pos, float _orientation, bool reversed, DriveType _driveType, TerriBull::MechanicalSystem* _system) : Task(DRIVE, _system), targetTheta(_orientation), driveType(_driveType), calculateOnInit(false), reversed(reversed), needsInitialize(false) { + this->v_f = new TerriBull::Vector2(pos); this->deleteOnCleanup = true; if (_orientation == 420) { calculateOnInit = true; @@ -32,36 +37,102 @@ DriveTask::DriveTask(TerriBull::Vector2 pos, float _orientation, bool reversed, DriveTask::~DriveTask() { if (this->deleteOnCleanup) { - delete this->pos; + delete this->v_f; + delete this->v_i; + delete this->offset; } } +DriveTask* DriveTask::DynamicInitialize(Vector2* offset, bool reversed, DriveType driveType, TerriBull::MechanicalSystem* system) { + DriveTask* task = new DriveTask(system); + task->reversed = reversed; + task->deleteOnCleanup = true; + task->needsInitialize = true; + task->offset = offset; + task->driveType = driveType; + return task; +} + +DriveTask* DriveTask::GoToObject(TerriBull::GameObject* object, bool reversed,TerriBull::MechanicalSystem* system) { + return nullptr; +} + void DriveTask::init() { this->finishedFlag = false; - this->system->resetDrive(); + this->hitTarget = false; + this->system->ResetDrive(); + this->v_i = new TerriBull::Vector2(*(this->system->getPosition())); if (this->calculateOnInit) { + this->v_f = *(v_i) + *(this->offset); float angleMod = (this->reversed) ? 180 : 0; - Vector2* dPos = *(this->pos) - *(this->system->getPosition()); - this->approachOrientation = fmod(RAD2DEG(dPos->theta) + angleMod, 360); + Vector2* dPos = *(this->v_f) - *(v_i); + this->targetTheta = fmod(RAD2DEG(dPos->theta) + angleMod, 360); delete dPos; } + if (this->needsInitialize) { + this->v_f = *(v_i) + *(this->offset); + Vector2* dPos = *(this->v_f) - *(v_i); + this->deleteOnCleanup = true; + float angleMod = (this->reversed) ? 180 : 0; + this->targetTheta = fmod(RAD2DEG(dPos->theta) + angleMod, 360); + switch(this->driveType) { + case TRANSLATION: + break; + case ORIENTATION: + break; + } delete dPos; + } } void DriveTask::update(float delta) { + pros::lcd::set_text(4,"Inside drivetask"); if (!this->finishedFlag) { + std::stringstream s3, s4; + bool currentAngleCurrection; switch(driveType) { case TRANSLATION: - this->system->GoToPosition(*(this->pos)); + currentAngleCurrection = this->system->DriveNeedsAngleCorrection(); + if (!this->hitTarget) { + if (currentAngleCurrection) { + Vector2 * currentPos = this->system->getPosition(); + Vector2* v_to_goal = (*v_f - *(currentPos)); + Vector2* v_i_to_goal = (*v_f - *v_i); + Vector2* v_i_to_bot = (*(currentPos) - v_i); + int errorMod = (v_i_to_goal->r < v_i_to_bot->r) ? 1 : -1; + float angleMod = (this->reversed ^ errorMod > 0)? 180 : 0; + float angleCorrection = fmod(RAD2DEG(v_to_goal->theta) + angleMod, 360); + this->system->TurnToAngle(angleCorrection); + this->system->getDrive()->updateAngleCorrection(fabs(this->system->getDriveError()) < 1 && (fabs(this->system->getDriveDError()) / delta) < 0.01); + delete v_to_goal; + delete v_i_to_goal; + delete v_i_to_bot; + } + else { + this->system->GoToPosition(*(this->v_f), *(this->v_i), this->reversed); /*TODO: Test Delta Value */ + if (currentAngleCurrection == this->lastNeedsCorrection) this->hitTarget = fabs(this->system->getDriveError()) < 1.6 && (fabs(this->system->getDriveDError()) / delta) < 0.1; + } + } + else { + this->system->TurnToAngle(this->targetTheta); + if (currentAngleCurrection == this->lastNeedsCorrection) this->finishedFlag = fabs(this->system->getDriveError()) < 1.5 && (fabs(this->system->getDriveDError()) / delta) < 0.01; + } + s3 << std::fixed << ::std::setprecision(1) << "x: "<< this->v_f->x << " y: " << this->v_f->y; + pros::lcd::set_text(5, s3.str()); + s4 << std::fixed << ::std::setprecision(1) << "| Off x: "<< this->offset->x << " y: " << this->offset->y; + pros::lcd::set_text(6, s4.str()); + this->lastNeedsCorrection = currentAngleCurrection; break; case ORIENTATION: - this->system->TurnToAngle(this->approachOrientation); + this->system->TurnToAngle(this->targetTheta); + this->finishedFlag = fabs(this->system->getDriveError()) < 1.5 && (fabs(this->system->getDriveDError()) / delta) < 0.01; break; + } if (this->finishedFlag) { + this->system->ResetDrive(); } - this->finishedFlag = fabs(this->system->getDriveError()) < 0.8 && fabs(this->system->getDriveDError()) < 0.25; /* Some Threshold */ + } } void DriveTask::terminate() { this->terminated = true; - this->system->resetDrive(); } \ No newline at end of file diff --git a/src/TerriBull/lib/Tasking/IntakeTask.cpp b/src/TerriBull/lib/Tasking/IntakeTask.cpp index 5940675..bbdd330 100644 --- a/src/TerriBull/lib/Tasking/IntakeTask.cpp +++ b/src/TerriBull/lib/Tasking/IntakeTask.cpp @@ -18,11 +18,11 @@ void IntakeTask::update(float delta) { if (!this->finishedFlag) { switch (intakeType) { case ON: - this->system->turnOnIntake(this->dir); + this->system->TurnOnIntake(this->dir); this->finishedFlag = (this->system->getIntake()->isToggled()) && (this->system->getIntake()->getDirection() == this->dir); break; case OFF: - this->system->turnOffIntake(); + this->system->TurnOffIntake(); this->finishedFlag = !(this->system->getIntake()->isToggled()); break; } diff --git a/src/TerriBull/lib/Tasking/RollerTask.cpp b/src/TerriBull/lib/Tasking/RollerTask.cpp index 8ce1832..ae06328 100644 --- a/src/TerriBull/lib/Tasking/RollerTask.cpp +++ b/src/TerriBull/lib/Tasking/RollerTask.cpp @@ -35,7 +35,7 @@ void RollerTask::init() { if(this->needsInitialize) { this->targetPos = *(this->initialPos) + this->offset; } - this->system->resetRoller(); + this->system->ResetRoller(); } void RollerTask::update(float delta) { @@ -43,12 +43,12 @@ void RollerTask::update(float delta) { if (!this->finishedFlag) { switch(rollerType) { case TIME: - this->system->spinRollerFor(this->direction, this->time); + this->system->SpinRollerFor(this->direction, this->time); this->finishedFlag = this->system->isRollerCompleted(); break; case POS: - this->system->spinRollerTo(this->targetPos); - this->finishedFlag = fabs(this->system->getRollerError()) < 0.2 && fabs(this->system->getRollerDError()) < 0.5; + this->system->SpinRollerTo(this->targetPos); + this->finishedFlag = fabs(this->system->getRollerError()) < 2 && fabs(this->system->getRollerDError()) < 0.5; break; } } if(this->finishedFlag) ; @@ -56,5 +56,5 @@ void RollerTask::update(float delta) { void RollerTask::terminate() { this->terminated = true; - this->system->resetRoller(); + this->system->ResetRoller(); } \ No newline at end of file diff --git a/src/TerriBull/lib/Tasking/ShooterTask.cpp b/src/TerriBull/lib/Tasking/ShooterTask.cpp index fe8d419..cddaa2f 100644 --- a/src/TerriBull/lib/Tasking/ShooterTask.cpp +++ b/src/TerriBull/lib/Tasking/ShooterTask.cpp @@ -10,24 +10,30 @@ * */ #include "../../../../include/TerriBull/lib/Tasking/Types/ShooterTask.hpp" -ShooterTask::ShooterTask(TerriBull::MechanicalSystem* _system) : Task(SHOOTER, _system) {} +ShooterTask::ShooterTask(ShooterType _shooterType, TerriBull::MechanicalSystem* _system) : Task(SHOOTER, _system), shooterType(_shooterType) {} ShooterTask::~ShooterTask() {} void ShooterTask::init() { this->finishedFlag = false; - this->system->resetShooter(); + this->system->ResetShooter(); } void ShooterTask::update(float delta) { if (!this->finishedFlag) { - this->system->ShootDisk(); - this->finishedFlag = this->system->isShotCompleted(); - } - if(this->finishedFlag) ; + switch (this->shooterType) { + case LOAD: + this->system->LoadShooter(); + this->finishedFlag = this->system->isShooterLoaded(); + break; + case SHOOT: + this->system->ShootDisk(); + this->finishedFlag = this->system->isShotCompleted(); + } + } } void ShooterTask::terminate() { this->terminated = true; - this->system->resetRoller(); + this->system->ResetShooter(); } \ No newline at end of file diff --git a/src/TerriBull/lib/Tasking/TimeTask.cpp b/src/TerriBull/lib/Tasking/TimeTask.cpp index 30f7fb5..4fbba63 100644 --- a/src/TerriBull/lib/Tasking/TimeTask.cpp +++ b/src/TerriBull/lib/Tasking/TimeTask.cpp @@ -19,12 +19,15 @@ void TimeTask::init() { } void TimeTask::update(float delta) { + // pros::lcd::set_text(4,); if (!this->finishedFlag) { this->sumTime+=delta; this->finishedFlag = this->sumTime > this->goalTime; + } } void TimeTask::terminate() { + pros::lcd::set_text(4,"LEAVING TIME TASK"); this->terminated = true; } \ No newline at end of file diff --git a/src/TerriBull/lib/Tasking/VariableTask.cpp b/src/TerriBull/lib/Tasking/VariableTask.cpp new file mode 100644 index 0000000..041eaec --- /dev/null +++ b/src/TerriBull/lib/Tasking/VariableTask.cpp @@ -0,0 +1,72 @@ +/** + * @file VariableTask.vpp + * @author John Koch jkoch21@usf.edu + * @brief Task Targetting the drive system of the BullBot For modifying program variables. + * + * @version 0.1 + * @date 2023-03-09 + * + * @copyright Copyright (c) 2022 + * + */ +#include "../../../../include/TerriBull/lib/Tasking/Types/VariableTask.hpp" +VariableTask::VariableTask(void* targetVariable, void* targetValue, VarType _varType, TerriBull::MechanicalSystem* _system) : Task(VARIABLE, _system), varType(_varType) { + this->targetVariable = targetVariable; + this->targetValue = targetValue; +} + +VariableTask::~VariableTask() {} + +void VariableTask::init() { + this->finishedFlag = false; +} + +void VariableTask::update(float delta) { + if (!this->finishedFlag) { + switch (varType) { + case INT: + *(int*)targetVariable = *(int*)targetValue; + break; + case FLOAT: + *(float*)targetVariable = *(float*)targetValue; + break; + case CHAR: + *(char*)targetVariable = *(char*)targetValue; + break; + case DOUBLE: + *(double*)targetVariable = *(double*)targetValue; + break; + case BOOL: + *(bool*)targetVariable = *(bool*)targetValue; + break; + default: + break; + } + this->finishedFlag = true; + } +} + +void VariableTask::terminate() { + if (!this->finishedFlag) { + switch (varType) { + case INT: + delete (int*)targetValue; + break; + case FLOAT: + delete (float*)targetValue; + break; + case CHAR: + delete (char*)targetValue; + break; + case DOUBLE: + delete (double*)targetValue; + break; + case BOOL: + delete (bool*)targetValue; + break; + default: + break; + } + } + this->terminated = true; +} \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 2941f46..3435a55 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -38,28 +38,105 @@ TerriBull::RoboController controlSys; void on_center_button() { - controlSys.ClearTasks(); + //controlSys.ClearTasks(); // pros::delay(100); // controlSys.getSystem()->resetDrive(); } +// trim from start (in place) +static inline void ltrim(std::string &s) { + s.erase(s.begin(), std::find_if(s.begin(), s.end(), [](unsigned char ch) { + return !std::isspace(ch); + })); +} + +// trim from end (in place) +static inline void rtrim(std::string &s) { + s.erase(std::find_if(s.rbegin(), s.rend(), [](unsigned char ch) { + return !std::isspace(ch); + }).base(), s.end()); +} + +// trim from both ends (in place) +static inline void trim(std::string &s) { + rtrim(s); + ltrim(s); +} + +int kghkh = 1; +void TestFunction( RoboController* c, char *array, int start_index, int length ) +{ + int i = start_index + 1; + + double number0 = SerialController::DeserializeNumber(array, &i); + i++; + double number1 = SerialController::DeserializeNumber(array, &i); + i++; + std::string string0 = SerialController::DeserializeString(array, &i); + + pros::lcd::set_text(0, to_string(kghkh++).c_str()); + pros::lcd::set_text(1, to_string(number0).c_str()); + pros::lcd::set_text(2, to_string(number1).c_str()); + + int u = 3; + std::string fmt; + for (int p = 0; p < string0.length(); p++) + { + //if (u > 7) u = 2; + fmt += string0[p]; + if (fmt.length() > 26) + { + trim(fmt); + if (fmt.length() > 0) + pros::lcd::set_text(u++, fmt.c_str()); + fmt.clear(); + } + } + trim(fmt); + if (fmt.length() > 0) + pros::lcd::set_text(u++, fmt.c_str()); +} + + void initialize() { pros::lcd::initialize(); - controlSys.Init(); - pros::lcd::register_btn1_cb(on_center_button); + // controlSys.Init(); + SerialController* serialController = new SerialController(nullptr); + serialController->RegisterCallback("TestFunction", (SerialController::PacketCallback)TestFunction); + serialController->ExchangeTags(); + + int sent = 1; + while(true) + { + serialController->ReadBuffer(); + + stringstream stream; + stream << (unsigned char) (serialController->GetCallbackIndex("test_function")); + stream << SerialController::SerializeNumber(654); + stream << SerialController::SerializeString("this is a super long string that we are sending over serial that i hope will just be over 255 characters to ensure strings are working properly"); + stream << SerialController::SerializeNumber(1); + stream << SerialController::SerializeNumber(715.4); + stream << SerialController::SerializeNumber(789.6786); + serialController->SendData(stream.str()); + pros::lcd::set_text(7, to_string(sent++).c_str()); + pros::delay(5); + } + //pros::lcd::register_btn1_cb(on_center_button); } void autonomous() { - while (true) { - controlSys.Run(); - } + //while (true) { + // controlSys.Run(); + //} } void opcontrol() { - while (true) { - controlSys.Run(); - } + // controlSys.ClearTasks(); + //controlSys.getSystem()->setStartingPosition(0, 0); + //while (true) { + // controlSys.Run(); + //} }