screw handling initial works

1 files changed, 41 insertions, 24 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
index 164e5eb23..37f60e8cf 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -9,6 +9,7 @@
 #include "Drivers/Peripheral_GPIO/GPIO.h"
 #include "drivers/Motors/Motor.h"
 #include "StateMachines/Printing/PrintingSTM.h"
+#include "Modules/Control/Control.h"
 
 #include "drivers/FPGA/FPGA.h"
 
@@ -16,10 +17,12 @@
 #include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
 
 bool Winder_ScrewHoming = false;
-uint32_t Winder_ScrewAtOffsetCallback(uint32_t NumberOfSteps);
+uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag);
 uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue);
 
+bool ScrewCurrentDirection = false;
 double ScrewSpeed = 0;
+uint32_t ScrewControlId = 0xFF;
 
 uint32_t Winder_Init(void)
 {
@@ -75,7 +78,7 @@ uint32_t Winder_Prepare(void *JobDetails)
 
     float process_speed = JobTicket->processparameters->dyeingspeed;
 //    double ScrewSpeed = (50 *  MotorsCfg[SCREW_MOTOR].pulseperround *  MotorsCfg[SCREW_MOTOR].microstep)/(2*PI* MotorsCfg[SCREW_MOTOR].pulleyradius);
-    double ScrewSpeed = (process_speed *  MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulseperround)/(2*PI* MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulleyradius);
+    double ScrewSpeed = (process_speed*20 );//*  MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulseperround)/(2*PI* MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].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.
@@ -90,22 +93,23 @@ uint32_t Winder_Prepare(void *JobDetails)
     else
     {
         Winder_ScrewHoming = true;
-//        status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed, GPI_LS_SCREW_RIGHT, Winder_PrepareStage2);
-        status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed/10, GPI_LS_SCREW_RIGHT, Winder_Test1);
+        status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed, GPI_LS_SCREW_LEFT, Winder_PrepareStage2);
+//        status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed/10, GPI_LS_SCREW_RIGHT, Winder_Test1);
     }
     return status;
 }
 uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue)
 {
-    uint32_t status;
-    uint32_t numOfSteps = InternalWinderCfg.segmentoffsetpulses;
+    uint32_t status=OK;
+    uint32_t numOfSteps = InternalWinderCfg.startoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep;
     /*
      * 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.
      * 2. move back x steps - according to thehw specifications and bobine definitions in the job. move for a predefined number of steps. get a callback when done
      * report ready to the job STM
      */
-        status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,(1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize));//make sur to move the cart out
-//        status |= MotorMove(HARDWARE_MOTOR_TYPE__MOTO_SCREW, numOfSteps, Winder_ScrewAtOffsetCallback);
+        //status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,(1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize));//make sur to move the cart out
+        //status |= StopMotor(HARDWARE_MOTOR_TYPE__MOTO_SCREW,0);
+        status |= MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_SCREW, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize),numOfSteps, Winder_ScrewAtOffsetCallback);
         //status |= MotorMoveSteps (HARDWARE_MOTOR_TYPE__MOTO_SCREW, numOfSteps, Winder_ScrewAtOffsetCallback);
         //set motor location 0 here
         return status;
@@ -116,6 +120,24 @@ uint32_t WinderPresegmentReady(uint32_t deviceID, uint32_t ReadValue)
 {
     return PreSegmentReady(Module_Winder,ModuleDone);
 }
+uint32_t ScrewDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
+{
+    //ScrewCurrentDirection: false moves out, true moves home
+    if (BusyFlag == NOTBUSY)
+    {
+        if (ScrewCurrentDirection == false)
+        {
+            ScrewCurrentDirection = true;
+        }
+        else
+        {
+            ScrewCurrentDirection = false;
+        }
+        MotorMove (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewCurrentDirection,InternalWinderCfg.segmentoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep); //process: set point 0, set max speed, move to the specified length, return back.
+    }
+    return OK;
+
+}
 uint32_t Winder_Presegment(void *JobDetails)
 {
     JobTicket* JobTicket = JobDetails;
@@ -123,8 +145,9 @@ uint32_t Winder_Presegment(void *JobDetails)
     float screw_speed = 0;
     float RotationsPerSecond;
 
+    ScrewCurrentDirection = (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);
 //    double ScrewSpeed = (process_speed *  MotorsCfg[SCREW_MOTOR].pulseperround *  MotorsCfg[SCREW_MOTOR].microstep)/(2*PI* MotorsCfg[SCREW_MOTOR].pulleyradius);
-    double ScrewSpeed = (process_speed *  MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulseperround)/(2*PI* MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulleyradius);
+    //double ScrewSpeed = (process_speed *  MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulseperround)/(2*PI* MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulleyradius);
 
 /*typedef struct
 {
@@ -150,16 +173,19 @@ uint32_t Winder_Presegment(void *JobDetails)
 //     * 3. calculate cart travel length from winding parameters
 //     * 4. start move of travel length
 //     * 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 (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewCurrentDirection,InternalWinderCfg.segmentoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep); //process: set point 0, set max speed, move to the specified length, return back.
+    ScrewControlId = AddControlCallback(ScrewDirectionChange, eTenMilliSecond,MotorControlGetnBusyState,(IfTypeMotors*0x100+HARDWARE_MOTOR_TYPE__MOTO_SCREW), HARDWARE_MOTOR_TYPE__MOTO_SCREW, 0);
 
-    //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,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;
+    PreSegmentReady(Module_Winder,ModuleDone);
 
     return OK;
 }
 uint32_t Winder_End(void)
 {
     //stop screw
+    RemoveControlCallback(ScrewControlId,ScrewDirectionChange);
     return StopMotor (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz);
 }
 void Winder_ScrewHomeLimitSwitchInterrupt(void)
@@ -176,19 +202,10 @@ void Winder_ScrewOutLimitSwitchInterrupt(void)
     uint32_t status;
     status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);//make sure to move the cart out
 }
-uint32_t Winder_ScrewAtOffsetCallback(uint32_t NumberOfSteps)
+uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag)
 {
-    if (NumberOfSteps == InternalWinderCfg.segmentoffsetpulses)
-    {
-        PrepareReady(Module_Winder, ModuleDone);
-        return OK;
-    }
-    else
-    {
-        //do we want to do something?
-        PrepareReady(Module_Winder, ModuleFail);
-        return ERROR;
-    }
-
+    SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_SCREW);  //set this point as the spool home
+    PrepareReady(Module_Winder, ModuleDone);
+    return OK;
 }