Merge branch 'master' of https://twinetfs.visualstudio.com/_git/Tango

2 files changed, 104 insertions, 176 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
new file mode 100644
index 000000000..05fb0a934
--- /dev/null
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -0,0 +1,92 @@
+/*
+ * Thread_Winder.c
+ *
+ *  Created on: 25 במרץ 2018
+ *      Author: shlomo
+ */
+#include"include.h"
+#include "thread.h"
+#include "Drivers/Peripheral_GPIO/GPIO.h"
+#include "drivers/Motors/Motor.h"
+#include "StateMachines/Printing/PrintingSTM.h"
+bool Winder_ScrewHoming = false;
+uint32_t Winder_ScrewAtOffsetCallback(uint32_t NumberOfSteps);
+uint32_t Winder_PrepareStage2(void);
+
+uint32_t Winder_Init(void)
+{
+
+}
+uint32_t Winder_Prepare(void)
+{
+    uint32_t status;
+    /*
+     * 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
+     */
+    if (PollGPIO(GPI113_LS_SCREW_RIGHT))
+    {
+        //go to prepare stage 2
+        Winder_PrepareStage2();
+    }
+    else
+    {
+        Winder_ScrewHoming = true;
+        status = MotorSetDirection(MOTOR_SCREW,1);//make sur to move the cart home
+        status =  MotorSetSpeed(MOTOR_SCREW, MotorsCfg[SCREW_MOTOR].maxfreq, 8);
+
+    }
+}
+uint32_t Winder_PrepareStage2(void)
+{
+    uint32_t status;
+    uint32_t numOfSteps = InternalWinderCfg.segmentoffsetpulses;
+    /*
+     * 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(MOTOR_SCREW,0);//make sur to move the cart out
+        status |= MotorMoveSteps (MOTOR_SCREW, numOfSteps, Winder_ScrewAtOffsetCallback);
+        status |=  MotorSetSpeed(MOTOR_SCREW, MotorsCfg[SCREW_MOTOR].maxfreq, 8);
+
+        return status;
+
+}
+uint32_t Winder_Print(void)
+{
+    /*
+     * speed is set by the winding parameters and by winder rotational speed (read POSITION every 10msec)
+     * calculate
+     */
+}
+uint32_t Winder_End(void)
+{
+    //stop screw
+}
+void Winder_ScrewHomeLimitSwitchInterrupt(void)
+{
+    uint32_t status;
+    if  (Winder_ScrewHoming)
+    {
+        StopMotor(MOTOR_SCREW,3); //stop ASAP
+    }
+    status = MotorSetDirection(MOTOR_SCREW,0);//make sur to move the cart out
+}
+uint32_t Winder_ScrewAtOffsetCallback(uint32_t NumberOfSteps)
+{
+    if (NumberOfSteps == InternalWinderCfg.segmentoffsetpulses)
+    {
+        JobPrepareReady(Module_Winder, OK);
+        return OK;
+    }
+    else
+    {
+        //do we want to do something?
+        JobPrepareReady(Module_Winder, ERROR);
+        return ERROR;
+    }
+
+}
+
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 6e4624723..5f14a17a7 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -10,8 +10,9 @@
 #include "../control/pidalgo.h"
 #include "PMR/Hardware/HardwareMotor.pb-c.h"
 #include "PMR/Hardware/HardwareMotorType.pb-c.h"
+
 #include "drivers/Motors/Motor.h"
-#include "Drivers/Danser_SSI/ssi_comm.h"
+#include "drivers/Danser_SSI/ssi_comm.h"
 #include "drivers/Heater/TemperatureSensor.h"
 #include "drivers/Heater/Heater.h"
 ////////////////////////////////State machine operation////////////////////////////////////
@@ -31,70 +32,6 @@ typedef enum
 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};
 
-/********************************************************************************************
-* functions describes motor operation flow and movement state during profile execution
-* used to operate in runtime correct profileflow execution
-*********************************************************************************************/
-static ReturnCode EntryState(void *JobDetails);
-static ReturnCode PrepareState(void *JobDetails);
-static ReturnCode PreSegmentState(void *JobDetails);
-static ReturnCode SegmentState(void *JobDetails);
-static ReturnCode EndState(void *JobDetails);
-static ReturnCode ExitState(void *JobDetails);
-;
-
-/**********************************************************************
-* the array and enum of PrintingState_t below must be in sync order
-***********************************************************************/
-static ReturnCode (* state[])(void *JobDetails) = { EntryState, PrepareState, PreSegmentState,  SegmentState, EndState, ExitState};
-
-typedef enum
-{
-    Entry= 0,
-    Prepare,
-    PreSegment,
-    Segment,
-    End,
-    Exit
-} PrintingState_t;
-
-typedef struct
-{
-    PrintingState_t m_sourceState;
-    ReturnCode          m_returnCode;
-    PrintingState_t     m_destinationState;
-} Transition_t;
-
-//*************************************************************
-/* transitions from end state aren't needed */
-//*************************************************************
-#define NUM_OF_TRANSITION   17
-#define EXIT_STATE          Exit
-#define ENTRY_STATE         Entry
-/*************************************************************
- * table which describes fast motors transitions states
- * during p_profile / segments execution
- *************************************************************/
-static Transition_t stateTransitionTable[NUM_OF_TRANSITION] =
-{};
-/*    {Entry,         NextState,  HomingStart},
-    {Entry,         Repeat,     Entry},         //for homing of dispensers
-    {HomingStart,   NextState,  Start},
-    {HomingStart,   Repeat,     HomingStart},
-    {Start,         NextState,  Segment},
-    {Start,         Repeat,     Start},
-    {Segment,       Inter,      Intersegment},
-    {Segment,       Repeat,     Segment},
-    {Segment,       Home,       HomingEnd},
-    {Intersegment,  NextState,  Segment},
-    {Intersegment,  Repeat,     Intersegment},
-    {Intersegment,  Home,       HomingEnd},
-    {HomingEnd,     NextState,  End},
-    {HomingEnd,     Repeat,     HomingEnd},
-    {End,           NextState,  Entry},
-    {End,           Repeat,     Entry},
-    {Exit,          Stop,       Exit}           //for stoping the machine iteration in case of error
-};*/
 typedef struct
 {
     bool                m_isEnabled;
@@ -106,28 +43,11 @@ typedef struct
     bool                m_isReady;
     PID_Config_Params   m_params;
 }MotorControlConfig_t;
-/*typedef struct
-{
-    float epsilon;
-    float dt;
-    float MAX;
-    float MIN;
-    float Kp;
-    float Kd;
-    float Ki;
-}PID_Config_Params;
-#define epsilon 0.01
-#define dt 0.01 //100ms loop time
-#define MAX 4 //For Current Saturation
-#define MIN -4
-#define Kp 0.1
-#define Kd 0.01
-#define Ki 0.005
-*/
+
 MotorControlConfig_t MotorControlConfig[MAX_THREAD_MOTORS_NUM];
 uint32_t DeviceId2Motor[MAX_THREAD_MOTORS_NUM];
 ////////////////////////Slow Motor State////////////////////////////////////
-static PrintingState_t gPrintingState;
+
 ////////////////////////////////////////////////////////////////////////////
 
 uint32_t ThreadSpeedControlCBFunction(uint32_t deviceID, uint32_t ReadValue)
@@ -200,14 +120,6 @@ uint32_t ThreadControlCBFunction(uint32_t deviceID, uint32_t ReadValue)
 }
 
 //********************************************************************************************************************
-/********************************************************************************************************************
-*function describes entry point of motor in profile execution - accelerate from stop position
-*function described above used to operate motor operation flow and movement state during profile execution
-*********************************************************************************************************************/
-static ReturnCode EntryState(void *JobDetails)
-{
-    return NextState;
-}
 
 //********************************************************************************************************************
 #ifdef DEBUG_TEST_FUNCTIONS
@@ -217,7 +129,7 @@ uint32_t Debug_Get_Dancer_Read(uint32_t DancerId, uint32_t Parameter1, uint32_t
 }
 #endif
 //********************************************************************************************************************
-static ReturnCode PrepareState(void *JobDetails)
+ ReturnCode PrepareState(void *JobDetails)
 {
     int Motor_i;
     //start thread control for all motors
@@ -251,11 +163,11 @@ static ReturnCode PrepareState(void *JobDetails)
     }
 
     //set 3 dancers to the profile positions
-    return NextState;
+    return OK;
 }
 
 //********************************************************************************************************************
-static ReturnCode PreSegmentState(void *JobDetails)
+ReturnCode PreSegmentState(void *JobDetails)
 {
 
     TimerMotors_t Motor_i;
@@ -268,102 +180,27 @@ static ReturnCode PreSegmentState(void *JobDetails)
     //set speed for both rocker motors
     //wait for all motors to get to the required speed (set the target speed for the control to check)
     //call the job state machine when the thread system is ready
-    return NextState;
+    return OK;
 }
 
 //********************************************************************************************************************
-static ReturnCode SegmentState(void *JobDetails)
+ ReturnCode SegmentState(void *JobDetails)
 {
-    return Repeat;
+    return OK;
 }
 
 //********************************************************************************************************************
-static ReturnCode EndState(void *JobDetails)
+ ReturnCode EndState(void *JobDetails)
 {
-    return NextState;
-}
-//********************************************************************************************************************
-static ReturnCode ExitState(void *JobDetails)
-{
-    return Stop;
+    return OK;
 }
 
 
-//***********************************************************************************************************************
-//this function is responsible for operating and transitioning between the diffrent motor state executions of the profile
-//the lower managment level
-//***********************************************************************************************************************
-static PrintingState_t LookupTransitions(PrintingState_t state,ReturnCode returnCode)
-{
-    char str[80];
-    uint8_t len = 0;
-    uint8_t indexInTransitionTable;
-    for (indexInTransitionTable = 0; indexInTransitionTable < NUM_OF_TRANSITION; ++indexInTransitionTable)
-    {
-        if ((stateTransitionTable[indexInTransitionTable].m_sourceState == state) && (stateTransitionTable[indexInTransitionTable].m_returnCode == returnCode))
-        {
-            //len = usnprintf(str, 60, "\r\n  tick %d state %d return code %d",tick,state, returnCode );
-            //cb_push_back (str, len);
-
-            //in normal execution flow function should not arrive here
-            //in case it did the meaning is that the entery point was wrong and a bug should be corrected
-            return stateTransitionTable[indexInTransitionTable].m_destinationState;
-        }
-    }
-    //int tick = UsersysTickGet();
-    //len = usnprintf(str, 60, "\r\n  tick %d state %d return code %d",tick,state, returnCode );
-    //cb_push_back (str, len);
-
-    //in normal execution flow function should not arrive here
-    //in case it did the meaning is that the entery point was wrong and a bug should be corrected
-    len = usnprintf(str, 80, "Internal: invalid slow motor transition state %d return code %d",state, returnCode );
-
-    return EXIT_STATE;
-}
-//********************************************************************************
-//this function is used to manage and operate the motor managmant state mashine
-//the highest managment level
-//********************************************************************************
-bool ThreadPrintingIterate(void *JobDetails)
-{
-    uint32_t tick = 0;
-    char str[60];
-    uint8_t len = 0;
-    PrintingState_t keepstate  =  gPrintingState;
-    //
-    // Disable all interrupts.
-    //
-    ROM_IntMasterDisable();
-
-    ReturnCode (* state_fun)(void *JobDetails) = state[gPrintingState];
-    //if (_motorId == SCREW_MOTOR)
-    //    screw_movement[gPrintingState[_motorId]]++;
-    ReturnCode returnCode = state_fun(JobDetails);
-
-    gPrintingState = LookupTransitions(gPrintingState, returnCode);
-//    if (keepstate != gPrintingState){
-//    }
-
-    //
-    // Enable all interrupts.
-    //
-    ROM_IntMasterEnable();
-    return (gPrintingState != EXIT_STATE);
-}
-
-
-//********************************************************************************************************************
-
-void ThreadPrintingsInit(void)
-{
-//        gPrintingState = Start;
-}
 
 //********************************************************************************************************************
 
 void ThreadStartPrinting(void)
 {
-    gPrintingState = ENTRY_STATE;
     //PrintingIterate();
 }
 
@@ -372,6 +209,5 @@ void ThreadStartPrinting(void)
 
 void ThreadStopPrinting(void)
 {
-    gPrintingState = EXIT_STATE;
     //PrintingIterate();
 }