update motor driver

3 files changed, 39 insertions, 8 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
index 913b38df8..d2675452c 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
@@ -11,6 +11,9 @@
 #include "PMR/Printing/JobSpool.pb-c.h"
 #include "PMR/Printing/JobSpoolType.pb-c.h"
 
+#include "drivers/Motors/Motor.h"
+#include "drivers/Danser_SSI/ssi_comm.h"
+
 
 typedef struct
 {
@@ -59,8 +62,8 @@ typedef enum threadMotorsEnum
     MAX_THREAD_MOTORS_NUM
 }threadMotorsEnum;
 #define MAX_THREAD_FEED_MOTORS WINDER_MOTOR+1
-extern uint32_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM];
-
+extern TimerMotors_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM];
+extern DANCER_ENUM ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM];
 
 extern MotorConfigStruc  MotorsCfg[MAX_THREAD_MOTORS_NUM];
 extern InternalWinderConfigStruc InternalWinderCfg;
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 366ce827b..7bc38c936 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -25,8 +25,8 @@
 ///////////////////////////////////////////////////////////////////////////////////////////
 
 
-uint32_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM] = {MOTOR_RDRIVING,MOTOR_DRYER_DRIVING,MOTOR_LDRIVING,MOTOR_WINDER,MOTOR_SCREW};
-uint32_t ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM] = {FEEDER_DANCER,NUM_OF_DANCERS,POOLER_DANCER,WINDER_DANCER,NUM_OF_DANCERS};
+TimerMotors_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM] = {MOTOR_RDRIVING,MOTOR_DRYER_DRIVING,MOTOR_LDRIVING,MOTOR_WINDER,MOTOR_SCREW};
+DANCER_ENUM ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM] = {FEEDER_DANCER,NUM_OF_DANCERS,POOLER_DANCER,WINDER_DANCER,NUM_OF_DANCERS};
 
 typedef struct
 {
@@ -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);
     }
 
@@ -120,13 +121,13 @@ uint32_t ThreadControlCBFunction(uint32_t deviceID, uint32_t ReadValue)
 //********************************************************************************************************************
 
 //********************************************************************************************************************
-#ifdef DEBUG_TEST_FUNCTIONS
+/*#ifdef DEBUG_TEST_FUNCTIONS
 uint32_t Debug_Get_Dancer_Read(uint32_t DancerId, uint32_t Parameter1, uint32_t Parameter2)
 {
 
     return (rand() % (103 + 1 + 103) - 103);
 }
-#endif
+#endif*/
 uint32_t ThreadInitialTestStub(HardwareMotor * request)
 {
 
@@ -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;
 }