path: root/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c

/*
 * Motor.c
 *
 *  Created on: 15 ���� 2018
 *      Author: shlomo
 */
/************************************************************************************************
 * TimerMotor.h
 *  this module is used to operate and control over dispensers and winding, using clock object
 *  the motors which are not connected to the PWM pins and needs microstepping
 *
 *************************************************************************************************/

#include <DataDef.h>
#include <Drivers/FPGA/Motors_Driver/L6470.h>
#include "include.h"
#include "motor.h"
#include "Modules/thread/thread.h"
#include "Modules/Control/control.h"

#include <stdint.h>
#include <stdbool.h>


#include "drivers/SPI/SPI_Comm.h"
#include "drivers/FPGA/FPGA_SPI_Comm.h"
#include "drivers/FPGA/FPGA_Comm.h"
#include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
#include "driverlib/ssi.h"
#include "StateMachines/Printing/PrintingSTM.h"

extern unsigned long Run_Value ;
extern unsigned long Pos_Value ;
extern bool Direction ;
extern unsigned char Stop_Command;

uint32_t Global_Speed;

uint32_t  Global_Read_MicroSteps;
///////////////////////////
extern SPI Fpga_Spi[NUM_OF_MOTORS];

SPI MotFPGA_Spi;

MotorDriverResponseStruct MotorDriverResponse[NUM_OF_MOTORS];
MotorDriverRequestStruct MotorDriverRequest[NUM_OF_MOTORS];
bool MotorConfigState[NUM_OF_MOTORS];
//MotorDriverConfigStruc  MotorsCfg[NUM_OF_MOTORS];
TimerMotors_t FastMotorToMotorId[4] = {HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,HARDWARE_MOTOR_TYPE__MOTO_LDRIVING,HARDWARE_MOTOR_TYPE__MOTO_WINDER};


//initialize general motors driver database
uint32_t MotorsInit(void)
{
    int i = 0;
    #ifdef EVALUATION_BOARD
        //MOTOR Driver L6470 SPI
        SPI2_Init();
        init_BUSY_Pin();

        MotorDriverConfigStruc MotConfigRequest;

        memset(&MotConfigRequest,0,sizeof(MotConfigRequest));

        setup(&MotConfigRequest);
    #else
        FPGA_SetMotorsInit();
    #endif
        MotorActionsInit();
        for (i=0;i<NUM_OF_MOTORS;i++)
        {
            MotorConfigState[i] = false;
        }

#ifndef EVALUATION_BOARD
    //option to change itafter initialization  (to solve EMC issues) in the function Control_motors_clock_source
    #ifdef EMC_FORCE_MOT_CLOCK_INTERNAL
        F3_Moto_Clk_Src_Sel.Motor.LDRIVING = INTERNAL;
        F3_Moto_Clk_Src_Sel.Motor.RDRIVING = INTERNAL;
        F3_Moto_Clk_Src_Sel.Motor.SCREW    = INTERNAL;
        F3_Moto_Clk_Src_Sel.Motor.WINDER   = INTERNAL;
    #else
        F3_Moto_Clk_Src_Sel.Motor.LDRIVING = EXTERNAL;
        F3_Moto_Clk_Src_Sel.Motor.RDRIVING = EXTERNAL;
        F3_Moto_Clk_Src_Sel.Motor.SCREW    = EXTERNAL;
        F3_Moto_Clk_Src_Sel.Motor.WINDER   = EXTERNAL;
    #endif

    F3_MOTO_CLK_SRC_SEL = F3_Moto_Clk_Src_Sel.ushort;
#endif

    return OK;
}
//set the motor configuration
uint32_t MotorConfig(TimerMotors_t _motorId, MotorDriverConfigStruc *MotorConfig)//?
{
    #ifdef EVALUATION_BOARD
        setup(MotorConfig);
        return OK;
    #else
        MotorConfigState[_motorId] = true;
        return FPGA_MotorConfig( _motorId, MotorConfig);
    #endif
}
uint32_t Motor_ReconfigAllMotors(void)
{
    TimerMotors_t Motor_i;
    uint32_t status = PASSED;
    ReportWithPackageFilter(GeneralFilter,"Motor_ReconfigAllMotors",__FILE__,__LINE__,NUM_OF_MOTORS,RpMessage,0,0);
    for (Motor_i = 0; Motor_i < NUM_OF_MOTORS ; Motor_i++)
    {
        if (isMotorConfigured(Motor_i))
        {
            status |= MotorConfig( Motor_i,  &MotorsCfg[Motor_i]);
        }
    }
    return status;
}
bool isMotorConfigured(TimerMotors_t _motorId)
{
    return MotorConfigState[_motorId];
}


TimerMotors_t getMotorId(int FastMotorId)
{
    return FastMotorToMotorId[FastMotorId];
}

uint32_t MotorGetDirection(TimerMotors_t _motorId)
{
    return ((MotorDriverResponse[_motorId].Status & x_STATUS_DIR)>>4);
}

uint32_t MotorSetDirection(TimerMotors_t _motorId,bool _direction)
{

    //ReportWithPackageFilter(GeneralFilter,"MotorSetDirection",__FILE__,_direction,_motorId,RpMessage,0,0);

    #ifdef EVALUATION_BOARD
        Direction = _direction;
        //Mot_Run();
    #else
        MotorDriverRequest[_motorId].Direction = _direction;
        //FPGA_SetMotSpeed(_motorId);
    #endif

    return OK;
}

uint32_t MotorSetSpeed(TimerMotors_t _motorId, float _freq)//TODO MicroSteps?
{
    uint32_t RunSpeed = SpdCalc(_freq);
    //ReportWithPackageFilter(GeneralFilter,"MotorSetSpeed",__FILE__,(int)_freq,_motorId,RpMessage,RunSpeed,0);
    #ifdef EVALUATION_BOARD
            Run_Value = RunSpeed;
            Mot_Run();
    #else
            MotorDriverRequest[_motorId].Speed = RunSpeed;
            FPGA_SetMotSpeed(_motorId);
    #endif

    return OK;
}
uint32_t MotorSetSpeedDirect(TimerMotors_t _motorId, float _freq)
{
    uint32_t RunSpeed = SpdCalc(_freq);
    MotorDriverRequest[_motorId].Speed = RunSpeed;
    FPGA_SetMotSpeedDirect(_motorId);

    return OK;
}


float MotorGetSpeed(TimerMotors_t _motorId)
{
    return MotorDriverResponse[_motorId].Speed;
}

uint32_t MotorGetADC(TimerMotors_t _motorId)
{
    return MotorDriverResponse[_motorId].ADC;
}

uint32_t MotorGetPosition(TimerMotors_t _motorId)
{
    return MotorDriverResponse[_motorId].Position;
}

uint32_t MotorGetPositionFromFPGA(TimerMotors_t _motorId)
{
    #ifdef EVALUATION_BOARD
            MotorDriverResponse[_motorId].Position = Get_Param(x_ABS_POS);
         return MotorDriverResponse[_motorId].Position;
    #else
            FPGA_GetMotPosition_Cmd(_motorId);
         return OK;
    #endif
}
uint32_t MotorGetPositionFromFPGA_Callback(TimerMotors_t _motorId,uint32_t Data)
{
    #ifndef EVALUATION_BOARD
      //  FPGA_Get_Res(_motorId);
        MotorDriverResponse[_motorId].Position = Data;
    #endif
    return MotorDriverResponse[_motorId].Position;
}

uint32_t MotorGetSpeedFromFPGA1(TimerMotors_t _motorId)
{
    #ifdef EVALUATION_BOARD
            MotorDriverResponse[_motorId].Speed = (CurrentSpdCalc(Get_Param(x_SPEED)));
            return (uint32_t)(MotorDriverResponse[_motorId].Speed);
    #else
            FPGA_GetFPGAMotSpeed_Cmd(_motorId);
         return OK;
    #endif
}
float MotorGetSpeedFromFPGA_Callback(TimerMotors_t _motorId,uint32_t Data)
{
    #ifndef EVALUATION_BOARD
      //  FPGA_Get_Res(_motorId);
        MotorDriverResponse[_motorId].Speed = CurrentSpdCalc(Data);
    #endif
    return MotorDriverResponse[_motorId].Speed;
}

//uint32_t MotorGetPositionFromFPGA_Res(TimerMotors_t _motorId)
//{
//    #ifndef EVALUATION_BOARD
//       // FPGA_Get_Res(_motorId);
//       // MotorDriverResponse[_motorId].Position = Fpga_Spi[_motorId].RX_MISO;
//    #endif
//    return MotorDriverResponse[_motorId].Position;
//}

uint32_t MotorGetSpeedFromFPGA(TimerMotors_t _motorId)
{
//    MotFPGA_Spi.MotID = _motorId;
//    FPGA_GetMotSpeed(MotFPGA_Spi);
//    MotorDriverResponse[_motorId].Speed = (float)(CurrentSpdCalc(MotFPGA_Spi.RX_MISO));
//    return MotorDriverResponse[_motorId].Speed;
       #ifdef EVALUATION_BOARD
            MotorDriverResponse[_motorId].Speed = (CurrentSpdCalc(Get_Param(x_SPEED)));
            return (uint32_t)(MotorDriverResponse[_motorId].Speed);
       #else
            FPGA_GetMotSpeed_Cmd(_motorId);
            return OK;
       #endif
}

float MotorGetSpeedFromFPGA_Res(TimerMotors_t _motorId)
{
    #ifndef EVALUATION_BOARD
        FPGA_Get_Res(_motorId);
        MotorDriverResponse[_motorId].Speed = CurrentSpdCalc(Fpga_Spi[_motorId].RX_MISO);
    #endif
    return MotorDriverResponse[_motorId].Speed;
}

uint32_t MotorGetADCFromFPGA(TimerMotors_t _motorId)
{
#ifndef EVALUATION_BOARD
            FPGA_Get_ADC_OUT_Cmd(_motorId);
            return OK;
#else
            return 0;
#endif
}

bool MotorParseStatus(TimerMotors_t _motorId)
{
    if (MotorDriverResponse[_motorId].DriverType == CombinrdMotDriver)
    {
        PStep01Status[_motorId].Reg = MotorDriverResponse[_motorId].Status;
    }
    else
    if (MotorDriverResponse[_motorId].DriverType == VoltageMotDriver)
    {
        VolMotDriverStatus[_motorId].Reg = MotorDriverResponse[_motorId].Status;
    }
    else
    if (MotorDriverResponse[_motorId].DriverType == CurrentMotDriver)
    {
        CurMotDriverStatus[_motorId].Reg = MotorDriverResponse[_motorId].Status;
    }
    else // UnKnownMotDriver
    {
        return ERROR;
    }
    return OK;
}
uint32_t MotorGetOverCurrentStatus(TimerMotors_t _motorId)
{
    if (MotorDriverResponse[_motorId].DriverType == CombinrdMotDriver)
    {
        return (PStep01Status[_motorId].bits.OCD_ActiveLow==0);
    }
    else
    if (MotorDriverResponse[_motorId].DriverType == VoltageMotDriver)
    {
        return (VolMotDriverStatus[_motorId].bits.OCD_ActiveLow==0);
    }
    else
    if (MotorDriverResponse[_motorId].DriverType == CurrentMotDriver)
    {
        return (CurMotDriverStatus[_motorId].bits.OCD_ActiveLow==0);
    }

    return ERROR;
}
uint32_t MotorGetUnderVoltageStatus(TimerMotors_t _motorId)
{
    if (MotorDriverResponse[_motorId].DriverType == CombinrdMotDriver)
    {
        return (PStep01Status[_motorId].bits.UVLO_ActiveLow==0);
    }
    else
    if (MotorDriverResponse[_motorId].DriverType == VoltageMotDriver)
    {
        return (VolMotDriverStatus[_motorId].bits.UVLO_ActiveLow==0);
    }
    else
    if (MotorDriverResponse[_motorId].DriverType == CurrentMotDriver)
    {
        return (CurMotDriverStatus[_motorId].bits.UVLO_ActiveLow==0);
    }

    return ERROR;
}
uint32_t MotorGetStallStatus(TimerMotors_t _motorId)
{
    if (MotorDriverResponse[_motorId].DriverType == CombinrdMotDriver)
    {
        return (PStep01Status[_motorId].bits.STALL_A_ActiveLow==0);
    }
    else
    if (MotorDriverResponse[_motorId].DriverType == VoltageMotDriver)
    {
        return (VolMotDriverStatus[_motorId].bits.STEP_LOSS_A_ActiveLow && VolMotDriverStatus[_motorId].bits.STEP_LOSS_B_ActiveLow);
    }
    else
    if (MotorDriverResponse[_motorId].DriverType == CurrentMotDriver)
    {
        return false;
    }

    return ERROR;
}
uint32_t MotorGetOverHeatStatus(TimerMotors_t _motorId)
{
    if (MotorDriverResponse[_motorId].DriverType == CombinrdMotDriver)
    {
        return (PStep01Status[_motorId].bits.TH_STATUS>1);
    }
    else
    if (MotorDriverResponse[_motorId].DriverType == VoltageMotDriver)
    {
        return (VolMotDriverStatus[_motorId].bits.TH_SD_ActiveLow==0);
    }
    else
    if (MotorDriverResponse[_motorId].DriverType == CurrentMotDriver)
    {
        return (CurMotDriverStatus[_motorId].bits.TH_SD_ActiveLow==0);
    }

    return ERROR;
}

uint32_t MotorGetStatus(TimerMotors_t _motorId)
{
    return MotorDriverResponse[_motorId].Status;
}

uint32_t MotorGetStatusFromFPGA(TimerMotors_t _motorId)
{
    #ifdef EVALUATION_BOARD
        uint32_t temp  = Get_and_Clear_Status();

        MotorDriverResponse[_motorId].Status = temp;

        return temp;
    #else
        FPGA_GetClrMotStat_Cmd(_motorId);
        MotorParseStatus(_motorId);
        return OK;
    #endif
}

uint32_t MotorGetStatusFromFPGA_Callback(TimerMotors_t _motorId,uint32_t Data)
{
    #ifndef EVALUATION_BOARD
        MotorDriverResponse[_motorId].Status = Data >> 8;
    #endif
    return (MotorDriverResponse[_motorId].Status & 0xFFFF);
}

//uint32_t MotorGetStatusFromFPGA_Res(TimerMotors_t _motorId)
//{
//    #ifndef EVALUATION_BOARD
//        FPGA_Get_Res(_motorId);
//        MotorDriverResponse[_motorId].Status = Fpga_Spi[_motorId].RX_MISO>>8;
//    #endif
//    return MotorDriverResponse[_motorId].Status;
//}

uint8_t MotorGetMicroSteps(TimerMotors_t _motorId)
{
    return  MotorDriverResponse[_motorId].MicroSteps;
}

uint8_t MotorGetMicroStepsFromFPGA(TimerMotors_t _motorId)
{
    #ifdef EVALUATION_BOARD
        uint32_t temp = (Get_Param(x_STEP_MODE)) & 0x03;
        MotorDriverResponse[_motorId].MicroSteps = temp;
        return  temp;
     #else
        FPGA_GetMotMicroSteps_Cmd(_motorId);
         return OK;
     #endif
}

uint32_t MotorGetMicroStepsFromFPGA_Res(TimerMotors_t _motorId)
{
    #ifndef EVALUATION_BOARD
        FPGA_Get_Res(_motorId);
        MotorDriverResponse[_motorId].MicroSteps = Fpga_Spi[_motorId].RX_MISO & 0x03;
    #endif
    return MotorDriverResponse[_motorId].MicroSteps;
}

uint32_t MotorGetnBusyFromFPGA(void) // get all motors nBusy bit status from the FPGAs
{
    #ifdef EVALUATION_BOARD

        uint8_t i;

        for(i=0;i<NUM_OF_MOTORS;i++)
        {
            uint32_t temp = (uint32_t)(Check_SPI_Busy() || SSIBusy(SSI2_BASE));
            MotorDriverResponse[i].Busy = temp;
        }

     #else
        FPGA_GetBusy();
     #endif

        return OK;
}
bool     MotorGetnBusyState(TimerMotors_t _motorId)
{
    return MotorDriverResponse[_motorId].Busy;
}

bool     MotorControlGetnBusyState(uint32_t _motorId, uint32_t parameter)
{

    return MotorDriverResponse[_motorId].Busy;
}

/*
 * Stop Command
  */
uint32_t MotorStop(TimerMotors_t _motorId, STOP_TYPE_ENUM StopType)
{
    #ifdef EVALUATION_BOARD
        Stop_Command = StopType;
        Mot_Stop();
    #else
        //ReportWithPackageFilter(GeneralFilter,"MotorStop",__FILE__,__LINE__,_motorId,RpMessage,StopType,0);
        MotorDriverRequest[_motorId].Stop = StopType;
        FPGA_SetMotStop(_motorId);
    #endif

    return OK;
}

uint32_t SetMotHome(TimerMotors_t _motorId)
{
    #ifdef EVALUATION_BOARD
        ResetPos();
    #else
        FPGA_SetMotHome(_motorId);
    #endif

    return OK;
}

uint32_t MotorSetMaxSpeed(TimerMotors_t _motorId, uint32_t MaxSpeed)
{
    if(MotorsCfg[_motorId].maxfrequency != MaxSpeed)
        ReportWithPackageFilter(GeneralFilter,"MotorSetMaxSpeed",__FILE__,__LINE__,_motorId,RpMessage,MaxSpeed,0);
    MotorsCfg[_motorId].maxfrequency = MaxSpeed;
    FPGA_SetMotMaxSpeed(_motorId);

    return OK;
}

uint32_t MotorSetMicroStep(TimerMotors_t _motorId, uint32_t microstep)
{
    if (MotorDriverRequest[_motorId].microstep != microstep)
        ReportWithPackageFilter(GeneralFilter,"MotorSetMicroStep",__FILE__,__LINE__,_motorId,RpMessage,microstep,0);
    MotorDriverRequest[_motorId].microstep = microstep;
    FPGA_SetMotMicroStep(_motorId);

    return OK;
}

uint32_t MotorSetKvalHold(TimerMotors_t _motorId, uint8_t Value)
{
    if(MotorsCfg[_motorId].kvalhold != Value)
        ReportWithPackageFilter(GeneralFilter,"MotorSetKvalHold",__FILE__,__LINE__,_motorId,RpMessage,Value,0);
    MotorsCfg[_motorId].kvalhold = Value;
    MotorsCfg[_motorId].tvalhold = Value;
    FPGA_SetMotKvalHold(_motorId);

    return OK;
}
uint32_t MotorSetKvalRun(TimerMotors_t _motorId, uint8_t Value)
{
    if(MotorsCfg[_motorId].kvalrun != Value)
        ReportWithPackageFilter(GeneralFilter,"MotorSetKvalRun",__FILE__,__LINE__,_motorId,RpMessage,Value,0);
    MotorsCfg[_motorId].kvalrun = Value;
    MotorsCfg[_motorId].tvalrun = Value;
    FPGA_SetMotKvalRun(_motorId);

    return OK;
}

uint32_t MotorSetKvalAcc(TimerMotors_t _motorId, uint8_t Value)
{
    MotorDriverRequest[_motorId].KvalAcc = Value;
    MotorDriverRequest[_motorId].TvalAcc = Value;
    FPGA_SetMotKvalAcc(_motorId);

    return OK;
}

uint32_t MotorSetKvalDec(TimerMotors_t _motorId, uint8_t Value)
{
    MotorDriverRequest[_motorId].KvalDec = Value;
    MotorDriverRequest[_motorId].TvalDec = Value;
    FPGA_SetMotKvalDec(_motorId);

    return OK;
}

uint32_t MotorMove(TimerMotors_t _motorId,bool direction, uint32_t Steps)
{
    #ifdef EVALUATION_BOARD
            Move(direction,Steps );
    #else
            MotorDriverRequest[_motorId].Position = Steps;
            MotorDriverRequest[_motorId].Direction = direction;
            FPGA_SetMotPosition(_motorId);
    #endif

    return OK;
}

uint32_t MotorGoTo(TimerMotors_t _motorId, uint32_t Steps)
{
    #ifdef EVALUATION_BOARD
    GoTo(Steps );
    #else
            MotorDriverRequest[_motorId].Position = Steps;
            FPGA_SetGoToPosition(_motorId);
    #endif
    Report("MotorGoTo",__FILE__,__LINE__, _motorId,RpMessage,Steps,0);
    return OK;
}

uint32_t MotorGoToDir(TimerMotors_t _motorId,bool direction, uint32_t Steps)
{
    #ifdef EVALUATION_BOARD
    GoTo_DIR(direction , Steps );
    #else
            MotorDriverRequest[_motorId].Position = Steps;
            MotorDriverRequest[_motorId].Direction = direction;
            FPGA_SetGoToDirPosition(_motorId);
    #endif
    Report("MotorGoToDir",__FILE__,__LINE__, _motorId,RpMessage,Steps,0);
    return OK;
}

///////////////// temporary for testing /////////////
/*
void loop_Run_Read_Speed()//every miliSec
{
    static uint32_t counter = 0;

    static bool Odd_Even = 0;

    static uint32_t SpeedCmd = 700;
    static uint32_t Falg = 0;

    double response_speed;

    if(counter == 0)
    {
        MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_LDRIVING,false);
        MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_LDRIVING, 700.0);
    }

    if(counter > 4000) // wait 4 Sec
    {
        if(Odd_Even == 0)
        {
            MotorGetSpeedFromFPGA(HARDWARE_MOTOR_TYPE__MOTO_LDRIVING); //Ask for Speeed
            Odd_Even += 1;
        }
        else
        if(Odd_Even == 1)
        {
            Odd_Even += 1;
        }
        else
        if(Odd_Even == 2)
        {
            response_speed = MotorGetSpeedFromFPGA_Res(HARDWARE_MOTOR_TYPE__MOTO_LDRIVING);

            if(((response_speed > SpeedCmd+1) || (response_speed < SpeedCmd-1)) && (counter > 4010) )// check for errors
            {
                //while(true);// ERROR!
                MotorStop(HARDWARE_MOTOR_TYPE__MOTO_LDRIVING, 0x03);
            }

            // change speed
            if(SpeedCmd == (700 + 250))
                Falg = 1;//Down
            else
            if(SpeedCmd == 700)
                Falg = 0;//up

            if(Falg == 0)
                SpeedCmd = SpeedCmd + 5;
            else
                SpeedCmd = SpeedCmd - 5;

            MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_LDRIVING,false);
            MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_LDRIVING, SpeedCmd);

            Odd_Even = 0;
        }

    }

    if(counter < 5000)
    {
            counter += 1;
    }
}

*/


void Combined_Motor_Driver_Mode(TimerMotors_t _motorId, MOTDRIVER_MODE New_Mode)// New_mode: Current or Voltage
{
    /*
    if(MotorDriverResponse[_motorId].DriverType == CombinrdMotDriver)
    {
        MotorDriverResponse[_motorId].DriverMode = New_Mode;
        //need to do all the configuration acording to the new mode
        //The STEP_MODE register includes the CM_VM bit (sets the method between voltage and current mode)
       FPGA_SetMotMicroStep(_motorId);
    }
    */
}
void Combined_dispenser_Driver_Mode(uint8_t Dispenser_ID, MOTDRIVER_MODE New_Mode)// New_mode: Current or Voltage
{
    assert(Dispenser_ID < MAX_DISPENSER_NUM);

    //Combined_Motor_Driver_Mode(Dispenser_ID + HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1, New_Mode);//dispenser ID -> motor id
}

void test_Home_Pos()
{
    SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_SCREW);
    delayms(10);
    char i = 0;
    uint32_t Steps = 100;
    bool dir = true;

    for(i=0;i<5;i++)
    {
        MotorGoToDir(HARDWARE_MOTOR_TYPE__MOTO_SCREW,dir, Steps);
        //MotorMove(HARDWARE_MOTOR_TYPE__MOTO_SCREW,dir, Steps);
        Steps+=50;
        delayms(200);
    }


    MotorStop(HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz);
    delayms(1000);
    dir = false;

    for(i=0;i<5;i++)
    {
        Steps-=50;
        MotorGoToDir(HARDWARE_MOTOR_TYPE__MOTO_SCREW,dir, Steps);
       // MotorMove(HARDWARE_MOTOR_TYPE__MOTO_SCREW,dir, Steps);
        delayms(200);
    }

/*
    dir = true;
    MotorStop(HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz);
    delayms(10);

    for(i=0;i<5;i++)
    {
        //MotorGoToDir(HARDWARE_MOTOR_TYPE__MOTO_SCREW,0, Steps);
        MotorMove(HARDWARE_MOTOR_TYPE__MOTO_SCREW,dir, Steps);
        //Steps+=50;
        delayms(10);
    }
*/
}

/* no need can be done with stub directly to the fpga address
uint32_t Control_motors_clock_source(TimerMotors_t motorId, bool Inetrnal_Or_External)// INTERNAL / EXTERNAL
{
    uint32_t Status = OK;
    switch(motorId)
    {
        case HARDWARE_MOTOR_TYPE__MOTO_RDRIVING:
                F3_Moto_Clk_Src_Sel.Motor.RDRIVING = Inetrnal_Or_External;
            break;
        case HARDWARE_MOTOR_TYPE__MOTO_LDRIVING:
                F3_Moto_Clk_Src_Sel.Motor.LDRIVING = Inetrnal_Or_External;
            break;
        case HARDWARE_MOTOR_TYPE__MOTO_SCREW:
                F3_Moto_Clk_Src_Sel.Motor.SCREW = Inetrnal_Or_External;
            break;
        case HARDWARE_MOTOR_TYPE__MOTO_WINDER:
                F3_Moto_Clk_Src_Sel.Motor.WINDER = Inetrnal_Or_External;
            break;
        default:
            Status =  ERROR;
            break;
    }

    F3_MOTO_CLK_SRC_SEL = F3_Moto_Clk_Src_Sel.ushort;

    return Status;
}
*/