Control fixes (Encoders, Motors and more) and FPGA preparations

4 files changed, 39 insertions, 31 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
index c737b9ea8..b31c74830 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
@@ -50,7 +50,7 @@ extern InternalWinderConfigStruc    InternalWinderCfg;
 extern HardwareDancer               DancersCfg[MAX_SYSTEM_DANCERS];
 
 #define MAX_CONTROL_SAMPLES 6
-extern uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES];
+extern int32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES];
 extern int MotorSamplePointer[MAX_THREAD_MOTORS_NUM];
 extern double NormalizedErrorCoEfficient[MAX_THREAD_MOTORS_NUM];
 
@@ -64,7 +64,7 @@ uint32_t MotorPidRequestMessage(HardwarePidControl* request);
 uint32_t DancerConfigMessage(HardwareDancer * request);
 
 uint32_t ThreadPrepareState(void *JobDetails);
-
+uint32_t Winder_Prepare(void);
 
 #endif //MODULES_THREAD_THREAD_H_
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
index da0844c3c..6a05b83ce 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -105,16 +105,21 @@ uint32_t Winder_Presegment(void *JobDetails)
 uint32_t Winder_End(void)
 {
     //stop screw
-    return StopMotor (MOTOR_SCREW,1);
+    return StopMotor (MOTOR_SCREW,Hard_Hiz);
 }
 void Winder_ScrewHomeLimitSwitchInterrupt(void)
 {
     uint32_t status;
     if  (Winder_ScrewHoming)
     {
-        StopMotor(MOTOR_SCREW,3); //stop ASAP
+        StopMotor(MOTOR_SCREW,Hard_Stop); //stop ASAP
     }
-    status = MotorSetDirection(MOTOR_SCREW,0);//make sur to move the cart out
+    status = MotorSetDirection(MOTOR_SCREW,MotorsCfg[SCREW_MOTOR].directionthreadwize);//make sure to move the cart out
+}
+void Winder_ScrewOutLimitSwitchInterrupt(void)
+{
+    uint32_t status;
+    status = MotorSetDirection(MOTOR_SCREW,1-MotorsCfg[SCREW_MOTOR].directionthreadwize);//make sure to move the cart out
 }
 uint32_t Winder_ScrewAtOffsetCallback(uint32_t NumberOfSteps)
 {
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
index 8c4b83ec6..1727c650a 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
@@ -14,7 +14,7 @@
 HardwareMotor  MotorsCfg[NUM_OF_MOTORS]={0};
 HardwarePidControl MotorsControl[MAX_THREAD_MOTORS_NUM] = {0};
 
-uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES] = {0};
+int32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES] = {0};
 int MotorSamplePointer[MAX_THREAD_MOTORS_NUM] = {0};
 double NormalizedErrorCoEfficient[MAX_THREAD_MOTORS_NUM] = {0};
 InternalWinderConfigStruc InternalWinderCfg = {0};
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 07fecf313..fb9740dec 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -27,14 +27,14 @@
 
 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};
-
+uint32_t    ControlIdtoMotorId [MAX_THREAD_MOTORS_NUM] = {0xFF};
 int OriginalMotorSpd_2PPS[MAX_THREAD_MOTORS_NUM] = {0};
 
 typedef struct
 {
     bool                m_isEnabled;
-    uint32_t            m_SetParam;
-    uint32_t            m_mesuredParam;
+    int32_t             m_SetParam;
+    float               m_mesuredParam;
     float               m_preError;
     float               m_integral;
     float               m_calculatedError;
@@ -54,7 +54,7 @@ uint32_t ThreadSpeedControlCBFunction(uint32_t deviceID, uint32_t ReadValue)
     //read value is the dancer angle
     int i,index=MAX_THREAD_MOTORS_NUM;
     for (i=0;i<MAX_THREAD_MOTORS_NUM;i++)
-        if (ThreadMotorIdToDancerId[i] == deviceID)
+        if (ControlIdtoMotorId[i] == deviceID)
         {
             index = i;
             break;
@@ -83,7 +83,7 @@ uint32_t ThreadSpeedControlCBFunction(uint32_t deviceID, uint32_t ReadValue)
 
  return OK;
 }
-uint32_t ThreadControlCBFunction(uint32_t DancerId, uint32_t ReadValue)
+uint32_t ThreadControlCBFunction(uint32_t MotorId, uint32_t ReadValue)
 {
 //#define MAX_CONTROL_SAMPLES 6
 //extern uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES];
@@ -91,46 +91,47 @@ uint32_t ThreadControlCBFunction(uint32_t DancerId, uint32_t ReadValue)
 
     //read value is the dancer angle
     int i,index=MAX_THREAD_MOTORS_NUM;
-    int Pid_Id;
+    int Pid_Id,DancerId;
     int32_t TranslatedReadValue, avreageSampleValue = 0;
     double NormalizedError;
     for (i=0;i<MAX_THREAD_MOTORS_NUM;i++)
-        if (ThreadMotorIdToDancerId[i] == DancerId)
+        if (ControlIdtoMotorId[i] == MotorId)
         {
             index = i;
             break;
         }
     if (index==MAX_THREAD_MOTORS_NUM)
     {
-        LOG_ERROR (DancerId, "No motor  for device");
+        LOG_ERROR (MotorId, "No motor  for device");
         return 0xFFFFFFFF;
     }
 
     if(MotorControlConfig[index].m_isEnabled )
     {
         Pid_Id = ThreadMotorIdToControlId[index];
+        DancerId = ThreadMotorIdToDancerId[index];
         TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint;
-        MotorSamples[index][MotorSamplePointer[index]] = TranslatedReadValue;
+        MotorSamples[index][MotorSamplePointer[index]] = TranslatedReadValue;//(-1 * TranslatedReadValue);
         MotorSamplePointer[index]++;
         if (MotorSamplePointer[index] >= MotorsControl[index].pvinputfilterfactormode) MotorSamplePointer[index] = 0;
         for (i=0;i<MotorsControl[index].pvinputfilterfactormode;i++)
             avreageSampleValue += MotorSamples[index][i];
         avreageSampleValue = avreageSampleValue / MotorsControl[index].pvinputfilterfactormode;
         NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index];
-
-        MotorControlConfig[index].m_mesuredParam = avreageSampleValue;
-        MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation(MotorControlConfig[index].m_SetParam , MotorControlConfig[index].m_mesuredParam,
+if (index != 0) return OK;
+        MotorControlConfig[index].m_mesuredParam = NormalizedError;
+        MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation((float)MotorControlConfig[index].m_SetParam , (float)MotorControlConfig[index].m_mesuredParam,
                                                                               &MotorControlConfig[index].m_params,   &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral);
-        if (MotorControlConfig[index].m_calculatedError >= MotorControlConfig[index].m_params.MAX)
+        /*if (MotorControlConfig[index].m_calculatedError >= MotorControlConfig[index].m_params.MAX)
         {
             MotorControlConfig[index].m_calculatedError = MotorControlConfig[index].m_params.MAX;
         }
         if (MotorControlConfig[index].m_calculatedError < MotorControlConfig[index].m_params.MIN)
         {
             MotorControlConfig[index].m_calculatedError = MotorControlConfig[index].m_params.MIN;
-        }
+        }*/
 
-        MotorSetSpeed(ThreadMotorIdToMotorId[index], MotorControlConfig[index].m_calculatedError,  MotorsCfg[index].microstep);
+        MotorSetSpeed(ThreadMotorIdToMotorId[index], (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index],  MotorsCfg[ThreadMotorIdToMotorId[index]].microstep);
         //SetMotorFreq (index, MotorControlConfig[index].m_calculatedError);
     }
 
@@ -165,11 +166,12 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request)
     {
         HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i];
         Pid_Id = Motor_i;/*ThreadMotorIdToControlId[Motor_i];*/
-            MotorControlConfig[Motor_i].m_params.MAX = MotorsControl[Pid_Id].outputproportionalpowerlimit*OriginalMotorSpd_2PPS[Motor_i];
-            MotorControlConfig[Motor_i].m_params.MIN = 0;
+            MotorControlConfig[Motor_i].m_params.MAX = 1;
+            MotorControlConfig[Motor_i].m_params.MIN = MotorsControl[Pid_Id].outputproportionalpowerlimit*-1;
             MotorControlConfig[Motor_i].m_params.Kd = MotorsControl[Pid_Id].derivativetime;
             MotorControlConfig[Motor_i].m_params.Kp = MotorsControl[Pid_Id].proportionalgain;
             MotorControlConfig[Motor_i].m_params.Ki = MotorsControl[Pid_Id].integraltime;
+            MotorControlConfig[Motor_i].m_params.epsilon = 0.01;
             MotorControlConfig[Motor_i].m_params.dt = eOneMillisecond;
             MotorControlConfig[Motor_i].m_calculatedError = 0;
             MotorControlConfig[Motor_i].m_integral = 0;
@@ -181,23 +183,25 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request)
 
             MotorSetDirection(HW_Motor_Id,MotorsCfg[HW_Motor_Id].directionthreadwize);
     #ifdef DEBUG_TEST_FUNCTIONS
-            if (HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
+            if (Motor_i == FEEDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
+                ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i);
+            /*if (HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
                 //AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,MotorGetSpeed,ThreadMotorIdToMotorId[Motor_i],0);
     //            continue;
-                AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i);
+                AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i);
             else if ((HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||(HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING)||(HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING))
-                AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i);
+                AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i);*/
     #else
             if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
                 continue;
                 //AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,MotorGetSpeed,ThreadMotorIdToMotorId[Motor_i],Motor_i);
             else if ((Motor_i == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||(Motor_i == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING)||(Motor_i == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING))
-                AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i);
+                AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i);
     #endif
     }
     Winder_Prepare();
     //set 3 dancers to the profile positions
-    ControlStart();
+
     return OK;
 }
 
@@ -217,11 +221,10 @@ uint32_t ThreadPreSegmentState(void *JobDetails)
         HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i];
         //(Speed*uStep*PPR)/((2*PI*motor_Radius)
         double motor_speed = (process_speed *  MotorsCfg[HW_Motor_Id].pulseperround *  MotorsCfg[HW_Motor_Id].microstep)/(2*PI* MotorsCfg[HW_Motor_Id].pulleyradius);
-
+        //MotorControlConfig[Motor_i].m_SetParam = motor_speed;
         OriginalMotorSpd_2PPS[Motor_i] = (int)motor_speed;
-        MotorControlConfig[Motor_i].m_SetParam = motor_speed;
     }
-
+    ControlStart();
     // set the new speed in the dryer motor to the speed of the new segment
     MotorSetSpeed(MOTOR_DRYER_DRIVING, OriginalMotorSpd_2PPS[DRYER_MOTOR],  MotorsCfg[MOTOR_DRYER_DRIVING].microstep);
     MotorSetSpeed(MOTOR_RLOADING, 1, 1);