Merge pull request #172 from luisarellano09/171-improve-robot-control

171 improve robot control
luisarellano09 · Feb 23, 2024 · 0e0f812 · 0e0f812
2 parents 11360f9 + 5cc552d
commit 0e0f812
Show file tree

Hide file tree

Showing 12 changed files with 72 additions and 38 deletions.
diff --git a/SBR_ESP32/SBR_ESP32_Manager/src/Application/System/Tasks/Tasks.h b/SBR_ESP32/SBR_ESP32_Manager/src/Application/System/Tasks/Tasks.h
@@ -74,11 +74,9 @@ void TaskCLI(void *parameter){
 
     F_CLI_Hello();
     F_CLI_Info();
-
-    TickType_t xLastWakeTime = xTaskGetTickCount();
     while(true) {
-        vTaskDelayUntil(&xLastWakeTime, TimerTaskCLI);
         RunCLI();
+        vTaskDelay(TimerTaskCLI);
     }
 }
 
@@ -87,21 +85,20 @@ void TaskCLI(void *parameter){
 
 void TaskGetValueCLI(void *parameter){
 
-    TickType_t xLastWakeTime = xTaskGetTickCount();
     while(true) {
-        vTaskDelayUntil(&xLastWakeTime, TimerTaskCLI);
         GetValueCLI();
+        vTaskDelay(TimerTaskCLI);
     }
 }
 
 
 //=====================================================================================================
 
 void TaskOTA(void *parameter){
-    TickType_t xLastWakeTime = xTaskGetTickCount();
+
     while(true) {
-        vTaskDelayUntil(&xLastWakeTime, TimerTaskOTA);
         manager->m_wifiManager->RunOTA();
+        vTaskDelay(TimerTaskOTA);
     }
 }
 

diff --git a/SBR_ESP32/SBR_ESP32_Node01/src/Application/Manager/Manager.cpp b/SBR_ESP32/SBR_ESP32_Node01/src/Application/Manager/Manager.cpp
@@ -51,14 +51,20 @@ Manager::Manager(){
     this->m_motionControl->m_PIDPitch->SetParameters(5.0, 25.0, 0.2);
     this->m_motionControl->m_PIDPitch->SetMVRange(-100.0, 100.0);
 
-    this->m_motionControl->m_PIDPosition->SetCycleTime(0.1);
+    this->m_motionControl->m_PIDPosition->SetCycleTime(0.04);
     this->m_motionControl->m_PIDPosition->SetParameters(5.0, 0.5, 8);
     this->m_motionControl->m_PIDPosition->SetMVRange(-8.0, 10.0);
     this->m_IMU->SetPitchOffset(-4.0);
 
     this->m_motionControl->m_PIDAngle->SetCycleTime(0.1);
     this->m_motionControl->m_PIDAngle->SetParameters(1.0, 0.2, 0.2);
     this->m_motionControl->m_PIDAngle->SetMVRange(-100.0, 100.0);
+
+    this->m_motionControl->m_PIDPitch->SetHysteresis(0);
+    this->m_motionControl->m_PIDPosition->SetHysteresis(0);
+    this->m_motionControl->m_PIDAngle->SetHysteresis(0);
+
+
     Log.traceln("[Manager::Manager] Motion Control instanced");
 
     // Start Node 

diff --git a/SBR_ESP32/SBR_ESP32_Node01/src/Application/MotionControl/MotionControl.cpp b/SBR_ESP32/SBR_ESP32_Node01/src/Application/MotionControl/MotionControl.cpp
@@ -65,17 +65,20 @@ RC_e MotionControl::Run(){
         vTaskDelay(2000);
     } else {
 
+        // Reduce the call cycle 
+        if (m_CycleCounter %4 == 0){
+            // PID Position
+            this->m_PIDPosition->SetSP(this->m_SPPos);
+            this->m_PIDPosition->SetPV(this->m_odometry->GetDistance());
+            this->m_PIDPosition->Run();
+        }
+
         // Reduce the call cycle 
         if (m_CycleCounter %10 == 0){
             // PID Angle
             this->m_PIDAngle->SetSP(this->m_SPAngle);
             this->m_PIDAngle->SetPV(this->m_odometry->GetAngle());
             this->m_PIDAngle->Run();
-
-            // PID Position
-            this->m_PIDPosition->SetSP(this->m_SPPos);
-            this->m_PIDPosition->SetPV(this->m_odometry->GetDistance());
-            this->m_PIDPosition->Run();
         }
 
         // PID Pitch
@@ -84,8 +87,11 @@ RC_e MotionControl::Run(){
         this->m_PIDPitch->Run();
 
         // Assign speed to Motors
-        this->m_motorLeft->SetSpeed(this->m_PIDPitch->GetMV()*0.8 + this->m_PIDAngle->GetMV()*0.2) ;
+        this->m_motorLeft->SetSpeed(this->m_PIDPitch->GetMV()*0.8 + this->m_PIDAngle->GetMV()*0.2);
         this->m_motorRight->SetSpeed(this->m_PIDPitch->GetMV()*0.8 - this->m_PIDAngle->GetMV()*0.2);
+
+        // this->m_motorLeft->SetSpeed(this->m_PIDPitch->GetMV()*0.8 + this->steer*0.2);
+        // this->m_motorRight->SetSpeed(this->m_PIDPitch->GetMV()*0.8 - this->steer*0.2);
     }
 
     return retCode;

diff --git a/SBR_ESP32/SBR_ESP32_Node01/src/Application/MotionControl/MotionControl.h b/SBR_ESP32/SBR_ESP32_Node01/src/Application/MotionControl/MotionControl.h
@@ -33,6 +33,7 @@ class MotionControl {
     PID* m_PIDPitch = NULL;         /**@brief Instance for PID Pitch. */
     PID* m_PIDPosition = NULL;      /**@brief Instance for PID Position. */
     PID* m_PIDAngle = NULL;         /**@brief Instance for PID Angle. */
+    double steer = 0.0;
 
     /**
      * @brief Construct a new Motion Control object

diff --git a/SBR_ESP32/SBR_ESP32_Node01/src/Application/System/CLI/CLI.h b/SBR_ESP32/SBR_ESP32_Node01/src/Application/System/CLI/CLI.h
@@ -1494,21 +1494,24 @@ void F_CLI_Test(){
 //=====================================================================================================
 
 void F_CLI_Test_Test1(){
-
+    manager->m_motionControl->steer++;
+    Serial.println("Steer: " + String(manager->m_motionControl->steer));
 }
 
 
 //=====================================================================================================
 
 void F_CLI_Test_Test2(){
-
+    manager->m_motionControl->steer--;
+    Serial.println("Steer: " + String(manager->m_motionControl->steer));
 }
 
 
 //=====================================================================================================
 
 void F_CLI_Test_Test3(){
-
+    manager->m_motionControl->steer=0.0;
+    Serial.println("Steer: " + String(manager->m_motionControl->steer));
 }
 
 

diff --git a/SBR_ESP32/SBR_ESP32_Node01/src/Application/System/Tasks/Tasks.h b/SBR_ESP32/SBR_ESP32_Node01/src/Application/System/Tasks/Tasks.h
@@ -92,11 +92,9 @@ void TaskCLI(void *parameter){
     vTaskDelay(1000);
     F_CLI_Hello();
     F_CLI_Info();
-
-    TickType_t xLastWakeTime = xTaskGetTickCount();
     while(true) {
-        vTaskDelayUntil(&xLastWakeTime, TimerTaskCLI);
         RunCLI();
+        vTaskDelay(TimerTaskCLI);
     }
 }
 
@@ -105,10 +103,9 @@ void TaskCLI(void *parameter){
 
 void TaskGetValueCLI(void *parameter){
     vTaskDelay(1000);
-    TickType_t xLastWakeTime = xTaskGetTickCount();
     while(true) {
-        vTaskDelayUntil(&xLastWakeTime, TimerTaskCLI);
         GetValueCLI();
+        vTaskDelay(TimerTaskCLI);
     }
 }
 
@@ -117,10 +114,9 @@ void TaskGetValueCLI(void *parameter){
 
 void TaskOTA(void *parameter){
     vTaskDelay(1000);
-    TickType_t xLastWakeTime = xTaskGetTickCount();
     while(true) {
-        vTaskDelayUntil(&xLastWakeTime, TimerTaskOTA);
         manager->m_wifiManager->RunOTA();
+        vTaskDelay(TimerTaskOTA);
     }
 }
 
@@ -139,22 +135,20 @@ void TaskNodeESP32(void *parameter){
 //=====================================================================================================
 
 void TaskIMU(void *parameter){
-    TickType_t xLastWakeTime = xTaskGetTickCount();
     while(true) {
-        vTaskDelayUntil(&xLastWakeTime, TimerTaskIMU);
         manager->m_IMU->Run();
         manager->m_motionControl->CalculateFalldown();
+        vTaskDelay(TimerTaskIMU);
     }
 }
 
 
 //=====================================================================================================
 
 void TaskOdometry(void *parameter){
-    TickType_t xLastWakeTime = xTaskGetTickCount();
     while(true) {
-        vTaskDelayUntil(&xLastWakeTime, TimerTaskOdometry);
         manager->m_odometry->Run();
+        vTaskDelay(TimerTaskOdometry);
     }
 }
 
@@ -163,10 +157,9 @@ void TaskOdometry(void *parameter){
 
 void TaskMotionControl(void *parameter){
     vTaskDelay(1000);
-    TickType_t xLastWakeTime = xTaskGetTickCount();
     while(true) {
-        vTaskDelayUntil(&xLastWakeTime, TimerTaskMotionControl);
         manager->m_motionControl->Run();
+        vTaskDelay(TimerTaskMotionControl);
     }
 }
 
@@ -185,10 +178,9 @@ void TaskModes(void *parameter){
 
 void TaskDatalog(void *parameter){
     vTaskDelay(1000);
-    TickType_t xLastWakeTime = xTaskGetTickCount();
     while(true) {
-        vTaskDelayUntil(&xLastWakeTime, TimerTaskDatalog);
         Datalog();
+        vTaskDelay(TimerTaskDatalog);
     }
 }
 
@@ -197,10 +189,9 @@ void TaskDatalog(void *parameter){
 
 void TaskRegistersUpdateRT(void *parameter){
     vTaskDelay(1000);
-    TickType_t xLastWakeTime = xTaskGetTickCount();
     while(true) {
-        vTaskDelayUntil(&xLastWakeTime, TimerTaskRegistersUpdateRT);
         UpdateRegistersRT();
+        vTaskDelay(TimerTaskRegistersUpdateRT);
     }
 }
 

diff --git a/SBR_ESP32/SBR_ESP32_Node01/src/Application/System/Tasks/TasksConfig.h b/SBR_ESP32/SBR_ESP32_Node01/src/Application/System/Tasks/TasksConfig.h
@@ -34,7 +34,7 @@ TickType_t TimerTaskCLI = 500 / portTICK_PERIOD_MS;                     /**@brie
 TickType_t TimerTaskOTA = 2000 / portTICK_PERIOD_MS;                    /**@brief Timer of Task OTA */
 TickType_t TimerTaskNodeESP32 = 1 / portTICK_PERIOD_MS;                 /**@brief Timer of Task Node ESP32 */
 TickType_t TimerTaskModes = 500 / portTICK_PERIOD_MS;                   /**@brief Timer of Task Modes */
-TickType_t TimerTaskIMU = 7 / portTICK_PERIOD_MS;                       /**@brief Timer of Task IMU */
+TickType_t TimerTaskIMU = 5 / portTICK_PERIOD_MS;                       /**@brief Timer of Task IMU */
 TickType_t TimerTaskOdometry = 10 / portTICK_PERIOD_MS;                 /**@brief Timer of Task Odometry */
 TickType_t TimerTaskMotionControl = 10 / portTICK_PERIOD_MS;            /**@brief Timer of Task Motion */
 TickType_t TimerTaskDatalog = 100 / portTICK_PERIOD_MS;                 /**@brief Timer of Task Datalog */

diff --git a/SBR_ESP32/SBR_ESP32_Node01/src/Middleware/PID/PID.cpp b/SBR_ESP32/SBR_ESP32_Node01/src/Middleware/PID/PID.cpp
@@ -46,6 +46,11 @@ RC_e PID::Run(){
     if (this->m_mode == PIDMode::PID_START){
         // Error
         this->m_error = this->m_SP - this->m_PV;
+
+        // Hysteresis
+        if (abs(this->m_error) < this->m_hysteresis){
+            this->m_error = 0.0;
+        }
 
         // Proportional
         mvP = this->m_Kp * this->m_error;
@@ -114,6 +119,7 @@ RC_e PID::Stop(){
     this->m_mode = PIDMode::PID_STOP;
     this->m_prevError = 0.0;
     this->m_prevMVIntegral = 0.0;
+    this->m_MV = 0.0;
     Log.traceln("[PID::Stop] PID stopped");
 
     return retCode;
@@ -361,6 +367,20 @@ double PID::GetMVRangeMax(){
 }
 
 
+//=====================================================================================================
+
+RC_e PID::SetHysteresis(double hysteresis){
+    // Result code
+    RC_e retCode = RC_e::SUCCESS;
+
+    this->m_hysteresis = hysteresis;
+
+    Log.traceln("[PID::SetHysteresis] Hysteresis setted: %D", this->m_hysteresis);
+
+    return retCode;
+}
+
+
 //=====================================================================================================
 
 RC_e PID::Print(){

diff --git a/SBR_ESP32/SBR_ESP32_Node01/src/Middleware/PID/PID.h b/SBR_ESP32/SBR_ESP32_Node01/src/Middleware/PID/PID.h
@@ -231,6 +231,14 @@ class PID {
      * @return double MV max range
      */
     double GetMVRangeMax();
+
+    /**
+     * @brief Set the Hysteres
+     * 
+     * @param hysteresis Hysteresis
+     * @return RC_e Result code
+     */
+    RC_e SetHysteresis(double hysteresis);
 
     /**
      * @brief Print
@@ -263,6 +271,7 @@ class PID {
     double m_mvRangeMax = 100.0;    /**@brief Manipulated Variable Range Max */ 
     double m_prevMVIntegral = 0.0;  /**@brief Previous MV Integral part */
     double m_prevError = 0.0;       /**@brief Previous Error */
+    double m_hysteresis = 0.0;      /**@brief Hysteresis */
 };
 
 #endif // PID_H
diff --git a/SBR_NX/DevOps/docker-compose.yml b/SBR_NX/DevOps/docker-compose.yml
@@ -53,7 +53,7 @@ services:
             - /home/sbrnx/SBR/jetson-inference/python/www/recognizer:/jetson-inference/python/www/recognizer
             - /home/sbrnx/SBR/data/face_detector/known_faces:/face_detector/known_faces
         environment:
-            RABBITMQ_HOST: "sbrpi"
+            RABBITMQ_HOST: "172.168.10.10"
             RABBITMQ_USER: ${RABBITMQ_DEFAULT_USER}
             RABBITMQ_PASS: ${RABBITMQ_DEFAULT_PASS}
         build:

diff --git a/SBR_PI/Scripts/sbr.sh b/SBR_PI/Scripts/sbr.sh
@@ -2,7 +2,7 @@
 
 # Define menu options
 OPTIONS=(
-    "Update" "Update/Upgrade System"
+    "Update" "Update/Upgrade System Now"
     "Host Monitor" "Monitor the service: sbr_host_monitor.service"
     "Start Runner" "Start the GitHub Actions Runner"
     "Serial Node Manager" "Serial connection with Manager (exit: Ctrl-A k)"

diff --git a/SBR_SERV/nextjs/src/components/NavLayout/SidebarItems.jsx b/SBR_SERV/nextjs/src/components/NavLayout/SidebarItems.jsx
@@ -61,7 +61,8 @@ export default function SidebarItems({ collapsed }) {
                     <Link href={"http://sbrnx.local:9000"} target="_blank"> <p className=" px-2 rounded-lg hover:bg-[#697083]">Portainer NX</p> </Link>
                     <Link href={"http://" + hostName + ":15672"} target="_blank"> <p className=" px-2 rounded-lg hover:bg-[#697083]">Rabbitmq</p> </Link>
                     <Link href={"https://" + hostName + "/sbr_serv_graphql_playground"} target="_blank"> <p className=" px-2 rounded-lg hover:bg-[#697083]">GraphQL</p> </Link>
-                    <Link href={"http://sbrnx:8554"} target="_blank"> <p className=" px-2 rounded-lg hover:bg-[#697083]">ComputerVision</p> </Link>
+                    <Link href={"http://sbrnx.local:8554"} target="_blank"> <p className=" px-2 rounded-lg hover:bg-[#697083]">ComputerVision</p> </Link>
+                    <Link href={"https://" + hostName + ":8182"} target="_blank"> <p className=" px-2 rounded-lg hover:bg-[#697083]">SSH</p> </Link>
 				</AccordionItem>
 
 			</Accordion>