Update Motor Configuration interface

2 files changed, 30 insertions, 2 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
index 5b0f034f4..7119238de 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
@@ -45,7 +45,7 @@ uint32_t MotorsConfigMessage(HardwareMotor * request)
 {
     uint32_t status = PASSED;
     int Motor_i;
-
+    MotorDriverConfigStruc MotorDriverConfig;
     Motor_i = request->hardwaremotortype;
     if (Motor_i< MAX_THREAD_MOTORS_NUM)
     {
@@ -63,7 +63,30 @@ uint32_t MotorsConfigMessage(HardwareMotor * request)
         MotorsCfg[Motor_i].kd               = request->kd;
         MotorsCfg[Motor_i].changeslope      = request->changeslope;
         MotorsCfg[Motor_i].hightimeoutusec  = request->highlengthmicrosecond;
-        ThreadInitialTestStub(request);
+        if (Motor_i == MOTOR_LDRIVING)
+            ThreadInitialTestStub(request);
+        /*
+        typedef struct
+        {
+              uint32_t ACC; //set
+              uint32_t DEC;  //set
+              uint32_t MaxSpeed; //set
+              bool HasMicroStep;
+              uint32_t Microstep; //set
+              bool HasConfigWord;
+              uint16_t ConfigWord; //set
+        }MotorDriverConfigStruc;
+        */
+        MotorDriverConfig.ACC = MotorsCfg[Motor_i].changeslope;
+        MotorDriverConfig.DEC = MotorsCfg[Motor_i].changeslope;
+        MotorDriverConfig.MaxSpeed = MotorsCfg[Motor_i].maxfreq;
+        MotorDriverConfig.HasMicroStep = true;
+        MotorDriverConfig.Microstep = MotorsCfg[Motor_i].minmicrostep;
+        MotorDriverConfig.HasConfigWord = false;
+        MotorDriverConfig.ConfigWord = 0;
+
+        status = MotorConfig( Motor_i,  &MotorDriverConfig);
+
         return status;
     }
     else return Motor_i;
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 250e055dc..7bc38c936 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -111,6 +111,7 @@ uint32_t ThreadControlCBFunction(uint32_t deviceID, uint32_t ReadValue)
             MotorControlConfig[index].m_calculatedError = MotorControlConfig[index].m_params.MIN;
         }
 
+        MotorSetSpeed(ThreadMotorIdToMotorId[index], MotorControlConfig[index].m_calculatedError,  MotorsCfg[index].minmicrostep);
         //SetMotorFreq (index, MotorControlConfig[index].m_calculatedError);
     }
 
@@ -190,6 +191,8 @@ uint32_t ThreadPreSegmentState(void *JobDetails)
 
     // set the new speed in the dryer motor to the speed of the new segment
     MotorSetSpeed(MOTOR_DRYER_DRIVING, process_speed,  MotorsCfg[MOTOR_DRYER_DRIVING].minmicrostep);
+    MotorSetSpeed(MOTOR_RLOADING, 1, 1);
+    MotorSetSpeed(MOTOR_LLOADING, 1,1);
 
     // activate control fr all motors
     //set speed for both rocker motors
@@ -214,6 +217,8 @@ uint32_t ThreadPreSegmentState(void *JobDetails)
      {
         StopMotor(ThreadMotorIdToMotorId[Motor_i],Hard_Hiz);
      }
+     StopMotor(MOTOR_RLOADING,Hard_Hiz);
+     StopMotor(MOTOR_LLOADING,Hard_Hiz);
 
     return OK;
 }