Merge branch 'master' of https://twinetfs.visualstudio.com/_git/Tango

1 files changed, 25 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;