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

/************************************************************************************************
 * 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
 *
 *************************************************************************************************/

#ifndef DRIVERS_MOTOR_H_
#define DRIVERS_MOTOR_H_
#include "include.h"
//#include "Modules/thread/thread.h"
#include "PMR/Hardware/HardwareMotorType.pb-c.h"
#include "PMR/Hardware/HardwareMotor.pb-c.h"

#include <stdint.h>
#include <stdbool.h>
#include "drivers/FPGA/FPGA.h"
#include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"

#define TimerMotors_t HardwareMotorType
    static char* MotorStr[30] = {
    "Dyeing Head Clean Head",
    "Dyeing Head Clean Mech",
    "Dyeing Head Lid ",
    "Dryer Driving",
    "Dryer Lid ",
    "Dryer Loadarm",
    "Dispenser 1",
    "Dispenser 2",
    "Dispenser 3",
    "Dispenser 4",
    "Dispenser 5",
    "Dispenser 6",
    "Dispenser 7",
    "Dispenser 8",
    "Screw ",
    "Winder ",
    "Left Dancer ",
    "Middle Dancer",
    "Left Driving ",
    "Left Rockers ",
    "Left pivot ",
    "Right Dancer ",
    "Right Driving",
    "Right Loadarm",
    "Right Rockers",
    "Spare1 1",
    "Spare1 2",
    "Spare2 1",
    "Spare2 2",
    "Spare3 1"};


//#define NUM_OF_MOTORS  (HARDWARE_MOTOR_TYPE__MOTO_RLOADING + 1) // TODO After the integration with mulbery change to:
#define NUM_OF_MOTORS  HARDWARE_MOTOR_TYPE__MOTO_SPARE2_2+1
#define MINIMAL_MOTOR_SPEED 0.2

//typedef enum
//{
//    SetSpeed,
//    SetPosition,
//    SetHardStop,
//    SetSoftStop,
//    SetHardStopHiz,
//    SetSoftStopHiz,
//    GetSpeed,
//    GetPosition,
//    GetStatus,
//
//}MotorCommand_t;

//typedef struct
//{
//  TimerMotors_t id;
//  uint32_t minfreq;
//  uint32_t maxfreq;
//  uint32_t minmicrostep;
//  uint32_t maxmicrostep;
//  double changeslope;
//  double hightimeoutusec;
//}MotorDriverConfigStruc;

typedef struct
{
  HardwareMotorType hardwaremotortype;
  int32_t minfrequency;
  int32_t maxfrequency;
  int32_t setmicrostep;
  int32_t microstep;
  double maxchangeslope;
  double highlengthmicrosecond;
  bool speedmaster;
  int32_t pulseperround;
  double pulleyradius;
  int32_t configword;
  bool directionthreadwize;
  int32_t kvalhold;
  int32_t kvalrun;
  int32_t kvalacc;
  int32_t kvaldec;
  int32_t overcurrentthreshold;
  int32_t stallthreshold;
  int32_t thermalcompensationfactor;
  bool lowspeedoptimization;
  int32_t stslp;
  int32_t intspd;
  int32_t fnslpacc;
  int32_t fnslpdec;
  int32_t fsspd;
  uint32_t gatecfg1;
  uint32_t gatecfg2;
  uint32_t tvalhold;
  uint32_t tvalrun;
  uint32_t tvalacc;
  uint32_t tvaldec;
  uint32_t tfast;
  uint32_t tonmin;
  uint32_t toffmin;
  bool has_p01configword;//needed for backward compatibility
  uint32_t p01configword;

}MotorDriverConfigStruc;

//#define MotorDriverConfigStruc HardwareMotor
typedef struct
{
  bool Direction;
  //uint32_t Command_Mode;
  uint32_t Speed;
  uint32_t Position;
  uint8_t Stop;
  uint8_t KvalHold;
  uint8_t KvalRun;
  uint8_t KvalAcc;
  uint8_t KvalDec;
  uint8_t TvalHold;
  uint8_t TvalRun;
  uint8_t TvalAcc;
  uint8_t TvalDec;
  int32_t microstep;
  uint8_t  ocd_th;
}MotorDriverRequestStruct;

typedef struct
{
  bool Direction;
  float Speed;
  uint32_t Position;
  uint32_t Status;
  uint8_t MicroSteps;
  uint32_t ADC; //read the Adc in order to know the motor driver type L6470/ L6472 / powerSTEP01
  MOTDRIVER_TYPE DriverType; //current or voltage mode
  MOTDRIVER_MODE DriverMode; //current or voltage mode in powerSTEP01
  bool Busy;

}MotorDriverResponseStruct;


// PowerStep Status register
typedef union
{
    struct
    {
        bool HiZ                    : 1; //0
        bool Busy                   : 1; //1
        bool SW_F                   : 1; //2
        bool SW_EVN                 : 1; //3
        bool DIR                    : 1; //4
        unsigned short MOT_STATUS   : 2; //5-6
        bool CMD_ERROR              : 1; //7
        bool STCK_MOD               : 1; //8
        bool UVLO_ActiveLow         : 1; //9
        bool UVLO_ADC_ActiveLow     : 1; //10
        unsigned short  TH_STATUS   : 2; //11-12
        bool OCD_ActiveLow          : 1; //13
        bool STALL_B_ActiveLow      : 1; //14
        bool STALL_A_ActiveLow      : 1; //15
    }bits;
    unsigned short Reg;
}POWERSTEP01_STATUS;

POWERSTEP01_STATUS PStep01Status[NUM_OF_MOTORS];

// L6470 Status register
typedef union
{
    struct
    {
        bool HiZ                    : 1; //0
        bool Busy                   : 1; //1
        bool SW_F                   : 1; //2
        bool SW_EVN                 : 1; //3
        bool DIR                    : 1; //4
        unsigned short MOT_STATUS   : 2; //5-6
        bool NOTPERF_CMD            : 1; //7
        bool WRONG_CMD              : 1; //8
        bool UVLO_ActiveLow         : 1; //9
        bool TH_WRN_ActiveLow       : 1; //10
        bool TH_SD_ActiveLow        : 1; //11
        bool OCD_ActiveLow          : 1; //12
        bool STEP_LOSS_A_ActiveLow  : 1; //13
        bool STEP_LOSS_B_ActiveLow  : 1; //14
        bool SCK_MOD                : 1; //15
    }bits;
    unsigned short Reg;
}L6470_STATUS;

L6470_STATUS VolMotDriverStatus[NUM_OF_MOTORS];

// L6472 Status register
typedef union
{
    struct
    {
        bool HiZ                    : 1; //0
        bool Busy                   : 1; //1
        bool SW_F                   : 1; //2
        bool SW_EVN                 : 1; //3
        bool DIR                    : 1; //4
        unsigned short MOT_STATUS   : 2; //5-6
        bool NOTPERF_CMD            : 1; //7
        bool WRONG_CMD              : 1; //8
        bool UVLO_ActiveLow         : 1; //9
        bool TH_WRN                 : 1; //10
        bool TH_SD_ActiveLow        : 1; //11
        bool OCD_ActiveLow          : 1; //12
        bool X_Bit_13               : 1; //13
        bool X_Bit_14               : 1; //14
        bool SCK_MOD                : 1; //15
    }bits;
    unsigned short Reg;
}L6472_STATUS;

L6472_STATUS CurMotDriverStatus[NUM_OF_MOTORS];

extern MotorDriverRequestStruct MotorDriverRequest[NUM_OF_MOTORS];

extern MotorDriverResponseStruct MotorDriverResponse[NUM_OF_MOTORS];

extern MotorDriverConfigStruc  MotorsCfg[NUM_OF_MOTORS];

//initialize general motors driver database
uint32_t MotorsInit(void);
//set the motor configuration
uint32_t MotorConfig(TimerMotors_t _motorId, MotorDriverConfigStruc *MotorConfig);
uint32_t Motor_ReconfigAllMotors(void);

bool isMotorConfigured(TimerMotors_t _motorId);

TimerMotors_t getMotorId(int FastMotorId);

uint32_t MotorGetDirection(TimerMotors_t _motorId);

uint32_t MotorSetDirection(TimerMotors_t _motorId,bool _direction);

uint32_t MotorSetSpeed(TimerMotors_t _motorId, float _freq);
uint32_t MotorSetSpeedDirect(TimerMotors_t _motorId, float _freq);  // call directly and not from millisec task

uint32_t MotorSetMaxSpeed(TimerMotors_t _motorId, uint32_t MaxSpeed);

uint32_t MotorSetMicroStep(TimerMotors_t _motorId, uint32_t microstep);

float MotorGetSpeed(TimerMotors_t _motorId);
uint32_t MotorGetADC(TimerMotors_t _motorId);

uint32_t MotorGetPosition(TimerMotors_t _motorId);
uint32_t MotorGetPositionFromFPGA(TimerMotors_t _motorId);
uint32_t MotorGetPositionFromFPGA_Callback(TimerMotors_t _motorId,uint32_t Data);

//uint32_t MotorGetPositionFromFPGA_Res(TimerMotors_t _motorId);

uint32_t MotorGetSpeedFromFPGA(TimerMotors_t _motorId);
float MotorGetSpeedFromFPGA_Res(TimerMotors_t _motorId);
uint32_t MotorGetSpeedFromFPGA1(TimerMotors_t _motorId);
float MotorGetSpeedFromFPGA_Callback(TimerMotors_t _motorId,uint32_t Data);


uint32_t MotorGetADCFromFPGA(TimerMotors_t _motorId);
uint32_t MotorGetADCFromFPGA_Res(TimerMotors_t _motorId);

uint32_t MotorGetStatusFromFPGA(TimerMotors_t _motorId);
uint32_t MotorGetStatusFromFPGA_Callback(TimerMotors_t _motorId,uint32_t Data);
uint32_t MotorGetStatus(TimerMotors_t _motorId);

uint8_t MotorGetMicroSteps(TimerMotors_t _motorId);

uint8_t MotorGetMicroStepsFromFPGA(TimerMotors_t _motorId);

uint32_t MotorGetnBusyFromFPGA(void); // get all motors nBusy bit status from the FPGAs

bool     MotorGetnBusyState(TimerMotors_t _motorId);

int MotorGetFPGAResponse(TimerMotors_t MotorId,uint32_t *Data);  //returns -1 on busy or other error
int MotorSendFPGARequest(TimerMotors_t MotorId,uint32_t Data,uint32_t Length);  //returns -1 on busy or other error

/*
 * Stop Command
 * 1 - Hard_Stop/ 2 - Soft_Stop/ 3 - Hard_Hiz/ 4 - Soft_Hiz
 */
typedef enum
{
    Hard_Stop,
    Soft_Stop,
    Hard_Hiz,
    Soft_Hiz,
}STOP_TYPE_ENUM;

extern int32_t AccumulatedArmMovement;

uint32_t MotorStop(TimerMotors_t _motorId, STOP_TYPE_ENUM StopType);

uint32_t MotorMove(TimerMotors_t _motorId,bool direction, uint32_t Steps);

uint32_t MotorMoveWithCallback (TimerMotors_t _motorId,bool direction, uint32_t Steps, callback_fptr callback,uint32_t timeout);
uint32_t MotorRunWithCallback (TimerMotors_t MotorId,bool direction, uint32_t Freq, callback_fptr callback,uint32_t timeout);
uint32_t MotorGotoWithCallback (TimerMotors_t MotorId, uint32_t Steps,  uint32_t LimitSwitchId,callback_fptr callback,uint32_t timeout); //TODO
uint32_t MotorGotoWithBusyCallback (TimerMotors_t MotorId,bool direction, uint32_t Steps, callback_fptr callback,uint32_t timeout); //TODO

uint32_t MotorGoToDir(TimerMotors_t _motorId,bool direction, uint32_t Steps);

bool     MotorControlGetnBusyState(uint32_t _motorId, uint32_t parameter);

uint32_t MotorSetSpeedWithCallback (TimerMotors_t _motorId, uint32_t _freq, callback_fptr callback);

uint32_t SetMotHome(TimerMotors_t _motorId);

uint32_t MotorSetKvalHold(TimerMotors_t _motorId, uint8_t Value);
uint32_t MotorSetKvalRun(TimerMotors_t _motorId, uint8_t Value);
uint32_t MotorSetKvalAcc(TimerMotors_t _motorId, uint8_t Value);
uint32_t MotorSetKvalDec(TimerMotors_t _motorId, uint8_t Value);

void MotorActionsInit(void);

uint32_t MotorGetOverHeatStatus(TimerMotors_t _motorId);
uint32_t MotorGetStallStatus(TimerMotors_t _motorId);
uint32_t MotorGetUnderVoltageStatus(TimerMotors_t _motorId);
uint32_t MotorGetOverCurrentStatus(TimerMotors_t _motorId);

uint32_t MotorGetStatus(TimerMotors_t _motorId);
uint32_t MotorGetStatusFromFPGA_Callback(TimerMotors_t _motorId,uint32_t Data);
typedef enum {
    MotorHomingDirectionUp,
    MotorHomingDirectionDown,
    MotorHomingDirectionNoHoming
}MotorHomingDirectionEnum;

extern FPGA_GPI_ENUM     Motor_Id_to_LS_IdDown[NUM_OF_MOTORS];
extern FPGA_GPI_ENUM     Motor_Id_to_LS_IdUp[NUM_OF_MOTORS];
extern MotorHomingDirectionEnum     Motor_Id_to_LS_Direction[NUM_OF_MOTORS];

uint32_t MotorMovetoLimitSwitch (TimerMotors_t _motorId,bool direction, uint32_t Freq, uint32_t LimitSwitchId, callback_fptr callback,uint32_t timeout);
uint32_t MotorMovetoBreakSensor (TimerMotors_t _motorId,bool direction, uint32_t Freq,  callback_fptr callback,uint32_t timeout);
uint32_t MotorMovetoDancerPosition (TimerMotors_t _motorId,bool direction, uint32_t Freq,uint32_t DancerId,bool dancervaluedirection,  callback_fptr callback,uint32_t timeout);
uint32_t MotorAbortMovetoLimitSwitch (TimerMotors_t _motorId);
uint32_t MotorMoveToStopper (TimerMotors_t _motorId,bool direction, uint32_t Speed, callback_fptr callback,uint32_t backlash,uint32_t timeout);
uint32_t MotorMovetoEncoderPosition (TimerMotors_t MotorId,  callback_fptr callback,uint32_t timeout, bool direction,uint32_t speed);
uint32_t LoadingArmReset (callback_fptr callback,uint32_t timeout);
uint32_t LoadingStopArmReset (void);

float Calculate_Arm_Angle(uint32_t Drier_Center,uint32_t Current_Angle);
int Calculate_Arm_Distance(uint32_t Drier_Center,uint32_t Current_Angle);

uint32_t MotorStopAction (TimerMotors_t MotorId); //Stop the controlled action of a motor

uint32_t MotorGoTo(TimerMotors_t _motorIdn, uint32_t Steps);

void Combined_Motor_Driver_Mode(TimerMotors_t _motorId, MOTDRIVER_MODE New_Mode);// New_mode: Current or Voltage
void Combined_dispenser_Driver_Mode(uint8_t Dispenser_ID, MOTDRIVER_MODE New_Mode);// New_mode: Current or Voltage

uint32_t MotorGetMicroStepsFromFPGA_Res(TimerMotors_t _motorId);

void test_Home_Pos();
#endif /* DRIVERS_MOTOR_H_ */