remove microstep from motor speed set and from speed calculation. connect position get to millisecond task.

1 files changed, 52 insertions, 22 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 6ffbf75ae..b0bdfea4d 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -107,8 +107,9 @@ void ThreadUpdateProcessLength (double length, void *Funcptr)
 {
     CurrentRequestedLength = length*100;//Centimetres
     CurrentProcessedLength = 0;
-    PreviousPosition = 0;
-    CurrentPosition = 0;
+    //PreviousPosition = 0;
+    //CurrentPosition = 0;
+    totalLength = 0;
     ProcessedLengthFuncPtr = (ProcessedLengthFunc)Funcptr;
 }
 uint32_t MotorSentData[1000] = {0};
@@ -139,15 +140,21 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     if (initialpos == 0xFFFF)
         initialpos = CurrentPosition;
     positionDiff = Control_Delta_Position_Pass(CurrentPosition,PreviousPosition);
-    positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep;
+    //positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep;
     PreviousPosition = CurrentPosition;
 
     // total length = (position diff / full cycle) * pulley perimeter
     //(positionDiff/pulseperround)*((2*PI*motor_Radius)
 
     length = (positionDiff/MotorsCfg[ThreadMotorIdToMotorId[index]].pulseperround)*(2*PI*MotorsCfg[ThreadMotorIdToMotorId[index]].pulleyradius);
-    totalLength+=length;
-    CurrentProcessedLength+=length;
+    if (length > 0.1)
+    {
+        totalLength+=length;
+    }
+
+#warning control disabled
+        CurrentProcessedLength+=length;
+#warning control disabled
     PosDif[MotorDataIndex] = positionDiff;
     MotorSentData[MotorDataIndex] = length;
     tick[MotorDataIndex] = UsersysTickGet();
@@ -280,7 +287,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         if (abs(calculated_speed-CurrentControlledSpeed[index])>5)
         {
             CurrentControlledSpeed[index] = calculated_speed;
-            MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed,  MotorsCfg[ThreadMotorIdToMotorId[index]].microstep);
+            MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed);
         }
     }
     if (index == FEEDER_MOTOR)
@@ -289,8 +296,21 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         if (pooler_counter>=1000)
         {
             float error_integered = MotorControlConfig[index].m_calculatedError*1000;
+            /*{
+  "HeaterGroupId": 0,
+  "Zone1Temp": 80,
+  "Zone2Temp": 2641,
+  "Heater1Active": false,
+  "Heater2Active": false,
+  "Heater1Percentage": 3,
+  "Heater2Percentage": 4000,
+  "InfoMessage": "Standard DC"
+}  void HeatingTestSendResonse(uint32_t status, bool last,bool heater1Active,bool heater2Active, int temperature1, int temperature2,int Heater1Percentage,int Heater2Percentage, char* Message)
+
+}*/
             //HeatingTestSendResonse(0, false,true,true, MotorDriverRequest[22].Speed,MotorDriverRequest[18].Speed,MotorDriverRequest[15].Speed,MotorDriverRequest[3].Speed, "MotorSpeed");
-            HeatingTestSendResonse(0, false,true,true, /*OriginalMotorSpd_2PPS[index]*/_speed,(int)error_integered,calculated_speed,ReadValue, "FeederSpeed");
+
+            HeatingTestSendResonse(0, false,true,true, /*OriginalMotorSpd_2PPS[index]*/_speed,OriginalMotorSpd_2PPS[index]/*(int)error_integered*/,calculated_speed,ReadValue, "FeederSpeed");
                 pooler_counter = 0;
         }
     }
@@ -353,7 +373,7 @@ bool InitialProcess = false;
                     RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction);
                 }
                 ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
-                //AddControlCallback(ThreadControlSpeedReadFunction, eHundredMillisecond,MotorGetSpeedFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i);
+                AddControlCallback(ThreadControlSpeedReadFunction, eHundredMillisecond,MotorGetSpeedFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i);
             }
             if (Motor_i == POOLER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will n//ot be controlled
             {
@@ -389,33 +409,43 @@ bool InitialProcess = false;
     return OK;
 }
 
+void SetOriginMotorSpeed(float process_speed)
+{
+    int Motor_i, HW_Motor_Id;
+    for (Motor_i = 0; Motor_i <= WINDER_MOTOR; Motor_i++)
+    {
+        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);
+        double motor_speed = (process_speed
+                * MotorsCfg[HW_Motor_Id].pulseperround)
+                / (2 * PI * MotorsCfg[HW_Motor_Id].pulleyradius);
+        //MotorControlConfig[Motor_i].m_SetParam = motor_speed;
+        OriginalMotorSpd_2PPS[Motor_i] = (int) motor_speed;
+    }
+}
+
 //********************************************************************************************************************
 uint32_t ThreadPreSegmentState(void *JobDetails)
 {
 //set the speed only before the first segment, speed is constant accros job
     JobTicket* JobTicket = JobDetails;
-    int Motor_i, HW_Motor_Id;
 
     float process_speed = JobTicket->processparameters->dyeingspeed;
 
-    for (Motor_i = 0;Motor_i <= WINDER_MOTOR;Motor_i++)
-    {
-        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;
-    }
+    SetOriginMotorSpeed(process_speed);
     //ControlStart();
     // set the new speed in the dryer motor to the speed of the new segment
-    MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING, OriginalMotorSpd_2PPS[DRYER_MOTOR],  MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING].microstep);
+    MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING, OriginalMotorSpd_2PPS[DRYER_MOTOR]);
  //only for testing - when control works, these motors will take their speed from the dryer
-    MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_LDRIVING, OriginalMotorSpd_2PPS[POOLER_MOTOR],  MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDRIVING].microstep);
+    //MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_LDRIVING, OriginalMotorSpd_2PPS[POOLER_MOTOR]);
  //only for testing - when control works, these motors will take their speed from the dryer
-    MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_RDRIVING, OriginalMotorSpd_2PPS[FEEDER_MOTOR],  MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].microstep);
+    //MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_RDRIVING, OriginalMotorSpd_2PPS[FEEDER_MOTOR]);
 
-    MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 20, 1);
-    MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 20,1);
+#warning rocker disabled
+//    MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 50);
+//    MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 50);
+#warning rocker disabled
 
 
     // activate control fr all motors