Tgo serial

include/Controllers/SerialController/SerialController.hpp

@@ -17,6 +17,9 @@
 #include "pros/apix.h" //added this in
 #include <vector>
 #include <iostream>
+#include <string>
+#include <mutex>
+#include <cstring>
 
 class TerriBull::SerialController {
     public:
@@ -25,7 +28,7 @@ class TerriBull::SerialController {
     typedef struct {
         PacketCallback callback;
         std::string friendly_name;
-        int jetson_id;
+        int jetson_id = -1;
     } CallbackItem;
 
     typedef struct {
@@ -36,32 +39,52 @@ class TerriBull::SerialController {
 
     private:
     vector<char> __next_packet;
-    char __packet_header[4] { (char)115, (char)111, (char)117, (char)116 };
-    char __end_of_transmission[4] { (char)11, (char)11, (char)10, (char)0 };
-    int __header_length = sizeof(__packet_header) + 1;
-    int __footer_length = sizeof(__end_of_transmission);
-    int __packet_index_offset = 15;
+    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<int, CallbackItem*> Callbacks;
+    map<int, CallbackItem*> tmpCallbacks;
     vector<ScheduledCallback*> ScheduledCallbacks;
     TerriBull::RoboController* motherSys;
 
-    bool compareBuffer(vector<char> buffer1, int start, int end, char* buffer2);
-
+    bool CompareBuffer(vector<char> 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<pros::Mutex> lock(TerriBull::SerialController::input_mutex);
+            TerriBull::SerialController::input_buffer += c;
+            lock.unlock();
+        }
+    }
     SerialController(TerriBull::RoboController* _motherSys);
 
     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 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);
-    void readBuffer();
+    void Update(float delta);
+    bool ReadBuffer();
     void processDataFromBuffer();
     void SendData(::std::string data);
     void updateExchangeTags();

include/MechanicalComponents/MechanicalComponent.hpp

@@ -33,7 +33,7 @@ class TerriBull::MechanicalComponent  {
 
     float kP, kD, kI;
 
-    private:
+    public:
     MotorRefs *motorRefs;
 
     public:

src/Controllers/RoboController.cpp

@@ -43,7 +43,7 @@ SerialController* RoboController::getSerialController() {
 }
 
 void RoboController::runSerialTask(void* args) {
-    this->serialController->update(this->delta());
+    this->serialController->Update(this->delta());
 }
 void RoboController::runMainTask(void* args) {
     this->Run();
@@ -73,8 +73,8 @@ void TerriBull::RoboController::Init() {
         this->taskManager = new TaskManager();
         /* Init Serial Controller */
         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("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);
@@ -202,7 +202,7 @@ void TerriBull::RoboController::Init() {
 void TerriBull::RoboController::Run() {
     this->updateTime();
     this->system->update(this->delta());
-    this->serialController->update(this->delta());
+    this->serialController->Update(this->delta());
     if (pros::competition::is_autonomous()) { /*TODO: Or engaged Autonomous Control */
         this->taskManager->run(this->delta());
     } else {
@@ -554,6 +554,7 @@ void TagExchangeCallback(TerriBull::RoboController* robot, char * array, int sta
  * @param start 
  * @param length 
  */
+/*
 void SerialTestV5ToJetsonCallback(TerriBull::RoboController* robot, char * array, int start, int length) {
     std::stringstream s3;
     float theta = robot->getSystem()->getAngle();
@@ -563,6 +564,7 @@ void SerialTestV5ToJetsonCallback(TerriBull::RoboController* robot, char * array
     s3 << SerialController::SerializeNumber(3.14159);
     robot->getSerialController()->SendData(s3.str());
 }
+*/
 /**
  * @brief Recieves Test Data from the Jetson
  * 
@@ -571,6 +573,7 @@ void SerialTestV5ToJetsonCallback(TerriBull::RoboController* robot, char * array
  * @param start 
  * @param length 
  */
+/*
 void SerialTestJetsonToV5Callback(TerriBull::RoboController* robot, char * array, int start, int length) {
 
-}
+}*/