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

2 files changed, 58 insertions, 26 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
index 5e81e4543..b61247fe4 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -22,7 +22,8 @@ uint32_t Winder_Init(void)
 uint32_t Winder_Prepare(void)
 {
     uint32_t status = 0;
-    double ScrewSpeed = (50 *  MotorsCfg[SCREW_MOTOR].pulseperround *  MotorsCfg[SCREW_MOTOR].microstep)/(2*PI* MotorsCfg[SCREW_MOTOR].pulleyradius);
+//    double ScrewSpeed = (50 *  MotorsCfg[SCREW_MOTOR].pulseperround *  MotorsCfg[SCREW_MOTOR].microstep)/(2*PI* MotorsCfg[SCREW_MOTOR].pulleyradius);
+    double ScrewSpeed = (50 *  MotorsCfg[SCREW_MOTOR].pulseperround)/(2*PI* MotorsCfg[SCREW_MOTOR].pulleyradius);
 
     /*
      * 1. move home to the limit switch (check that the cart is clear from the limit switch, start moving, with acceleration to maximal speed. enable interrupt on the limit switch, upon interrupt stop.
@@ -38,7 +39,7 @@ uint32_t Winder_Prepare(void)
     {
         Winder_ScrewHoming = true;
         status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,1);//make sur to move the cart home
-        status |=  MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_SCREW, ScrewSpeed, MotorsCfg[SCREW_MOTOR].microstep);
+        status |=  MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_SCREW, ScrewSpeed);
     }
     return status;
 }
@@ -69,7 +70,8 @@ uint32_t Winder_Presegment(void *JobDetails)
     float screw_speed = 0;
     float RotationsPerSecond;
 
-    double ScrewSpeed = (process_speed *  MotorsCfg[SCREW_MOTOR].pulseperround *  MotorsCfg[SCREW_MOTOR].microstep)/(2*PI* MotorsCfg[SCREW_MOTOR].pulleyradius);
+//    double ScrewSpeed = (process_speed *  MotorsCfg[SCREW_MOTOR].pulseperround *  MotorsCfg[SCREW_MOTOR].microstep)/(2*PI* MotorsCfg[SCREW_MOTOR].pulleyradius);
+    double ScrewSpeed = (process_speed *  MotorsCfg[SCREW_MOTOR].pulseperround)/(2*PI* MotorsCfg[SCREW_MOTOR].pulleyradius);
 
 /*typedef struct
 {
@@ -97,7 +99,7 @@ uint32_t Winder_Presegment(void *JobDetails)
 //     * 5. register motor nBusy callback. this callback will flip between move(traverse length, hardstop) and goto(0), with handline og the coneshape and adjusting maxspeed
 
     //MotorMove (InternalWinderCfg.segmentoffsetpulses,screw_speed); process: set point 0, set max speed, move to the specified length, return back.
-     MotorSetSpeedWithCallback (HARDWARE_MOTOR_TYPE__MOTO_SCREW, screw_speed, MotorsCfg[SCREW_MOTOR].microstep,WinderPresegmentReady);
+     MotorSetSpeedWithCallback (HARDWARE_MOTOR_TYPE__MOTO_SCREW, screw_speed,WinderPresegmentReady);
     //in a callback: calculate backing rate for top and bottom, update point 0, update passing length, call the appropriate move to 0 / move;
 
     return OK;
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