#include #include #include #include #include #include #include "FPGA_SPI_Comm.h" #include "Drivers/Motors/Motor.h" #include "modules/control/millisecTask.h" #include "modules/thread/thread.h" #include "drivers/Heater/TemperatureSensor.h" #include "PMR/Hardware/HardwareMotor.pb-c.h" #include "PMR/Hardware/HardwareDancerType.pb-c.h" #include #include #include "Modules/AlarmHandling/AlarmHandling.h" #include extern TempSensorResponseStruct TempSensorResponse[MAX_TANGO_TEMPERATURE_SENSOR_ID]; #include bool Extended_Motor_Param[NUM_OF_MOTORS]; //to use the Tval parameters only in PowerStep drivers (option to add it also in current drivers) /////////////////////////////////////////////////////////////////////////////// SPI Fpga_Spi[NUM_OF_MOTORS]; typedef struct { volatile short * TX_00; volatile short * TX_01; volatile short * RX_00; volatile short * RX_01; volatile short * WORDS; volatile short * NBUSY; // One bit for every motor volatile short * SPI_Busy; } FpgaMotMap_t; static FpgaMotMap_t FpgaMotMap[NUM_OF_MOTORS] = { //MOTOR_DH_CLEANHEAD - Cleaning head motor {&F1_MOTO_DH_CLEANHEAD_A1_TX_00, &F1_MOTO_DH_CLEANHEAD_A1_TX_01, &F1_MOTO_DH_CLEANHEAD_A1_RX_00 ,&F1_MOTO_DH_CLEANHEAD_A1_RX_01 ,&F1_MOTO_DH_CLEANHEAD_A1_WORDS ,&F1_Moto_Driver_NBUSY1_D, &F1_SPI_Busy1_D}, //MOTOR_DH_CLEANMECH - Cleaning mechanism {&F1_MOTO_DH_CLEANMECH_A1_TX_00, &F1_MOTO_DH_CLEANMECH_A1_TX_01, &F1_MOTO_DH_CLEANMECH_A1_RX_00 ,&F1_MOTO_DH_CLEANMECH_A1_RX_01 ,&F1_MOTO_DH_CLEANMECH_A1_WORDS ,&F1_Moto_Driver_NBUSY1_D, &F1_SPI_Busy1_D}, //MOTOR_DH_LID - Dyeing head lid {&F1_MOTO_DH_LID_A1_TX_00, &F1_MOTO_DH_LID_A1_TX_01, &F1_MOTO_DH_LID_A1_RX_00 ,&F1_MOTO_DH_LID_A1_RX_01 ,&F1_MOTO_DH_LID_A1_WORDS ,&F1_Moto_Driver_NBUSY1_D, &F1_SPI_Busy2_D}, //MOTOR_DRYER_DRIVING - Dryer main motor {&F1_MOTO_DRYER_DRIVING_A1_TX_00, &F1_MOTO_DRYER_DRIVING_A1_TX_01, &F1_MOTO_DRYER_DRIVING_A1_RX_00 ,&F1_MOTO_DRYER_DRIVING_A1_RX_01 ,&F1_MOTO_DRYER_DRIVING_A1_WORDS ,&F1_Moto_Driver_NBUSY1_D, &F1_SPI_Busy1_D}, //MOTOR_DRYER_LID - Drier lid {&F1_MOTO_DRYER_LID_A1_TX_00, &F1_MOTO_DRYER_LID_A1_TX_01, &F1_MOTO_DRYER_LID_A1_RX_00 ,&F1_MOTO_DRYER_LID_A1_RX_01 ,&F1_MOTO_DRYER_LID_A1_WORDS ,&F1_Moto_Driver_NBUSY1_D, &F1_SPI_Busy1_D}, //MOTOR_DRYER_LOADARM - Drier thread loading arm {&F1_MOTO_DRYER_LOADARM_A1_TX_00, &F1_MOTO_DRYER_LOADARM_A1_TX_01, &F1_MOTO_DRYER_LOADARM_A1_RX_00 ,&F1_MOTO_DRYER_LOADARM_A1_RX_01 ,&F1_MOTO_DRYER_LOADARM_A1_WORDS ,&F1_Moto_Driver_NBUSY1_D, &F1_SPI_Busy2_D}, //MOTOR_DISPENSER_1 - Dispenser {&F2_MOTO_DISPENSER_A1_1_TX_00, &F2_MOTO_DISPENSER_A1_1_TX_01, &F2_MOTO_DISPENSER_A1_1_RX_00 ,&F2_MOTO_DISPENSER_A1_1_RX_01 ,&F2_MOTO_DISPENSER_A1_1_WORDS ,&F2_Moto_Driver_NBUSY1_D, &F2_SPI_Busy2_D}, //MOTOR_DISPENSER_2 - Dispenser {&F2_MOTO_DISPENSER_A1_2_TX_00, &F2_MOTO_DISPENSER_A1_2_TX_01, &F2_MOTO_DISPENSER_A1_2_RX_00 ,&F2_MOTO_DISPENSER_A1_2_RX_01 ,&F2_MOTO_DISPENSER_A1_2_WORDS ,&F2_Moto_Driver_NBUSY1_D, &F2_SPI_Busy2_D}, //MOTOR_DISPENSER_3 - Dispenser {&F2_MOTO_DISPENSER_A1_3_TX_00, &F2_MOTO_DISPENSER_A1_3_TX_01, &F2_MOTO_DISPENSER_A1_3_RX_00 ,&F2_MOTO_DISPENSER_A1_3_RX_01 ,&F2_MOTO_DISPENSER_A1_3_WORDS ,&F2_Moto_Driver_NBUSY1_D, &F2_SPI_Busy2_D}, //MOTOR_DISPENSER_4 - Dispenser {&F2_MOTO_DISPENSER_A1_4_TX_00, &F2_MOTO_DISPENSER_A1_4_TX_01, &F2_MOTO_DISPENSER_A1_4_RX_00 ,&F2_MOTO_DISPENSER_A1_4_RX_01 ,&F2_MOTO_DISPENSER_A1_4_WORDS ,&F2_Moto_Driver_NBUSY1_D, &F2_SPI_Busy2_D}, //MOTOR_DISPENSER_5 - Dispenser {&F2_MOTO_DISPENSER_A1_5_TX_00, &F2_MOTO_DISPENSER_A1_5_TX_01, &F2_MOTO_DISPENSER_A1_5_RX_00 ,&F2_MOTO_DISPENSER_A1_5_RX_01 ,&F2_MOTO_DISPENSER_A1_5_WORDS ,&F2_Moto_Driver_NBUSY1_D, &F2_SPI_Busy2_D}, //MOTOR_DISPENSER_6 - Dispenser {&F2_MOTO_DISPENSER_A1_6_TX_00, &F2_MOTO_DISPENSER_A1_6_TX_01, &F2_MOTO_DISPENSER_A1_6_RX_00 ,&F2_MOTO_DISPENSER_A1_6_RX_01 ,&F2_MOTO_DISPENSER_A1_6_WORDS ,&F2_Moto_Driver_NBUSY1_D, &F2_SPI_Busy2_D}, //MOTOR_DISPENSER_7 - Dispenser {&F2_MOTO_DISPENSER_A1_7_TX_00, &F2_MOTO_DISPENSER_A1_7_TX_01, &F2_MOTO_DISPENSER_A1_7_RX_00 ,&F2_MOTO_DISPENSER_A1_7_RX_01 ,&F2_MOTO_DISPENSER_A1_7_WORDS ,&F2_Moto_Driver_NBUSY1_D, &F2_SPI_Busy2_D}, //MOTOR_DISPENSER_8 - Dispenser {&F2_MOTO_DISPENSER_A1_8_TX_00, &F2_MOTO_DISPENSER_A1_8_TX_01, &F2_MOTO_DISPENSER_A1_8_RX_00 ,&F2_MOTO_DISPENSER_A1_8_RX_01 ,&F2_MOTO_DISPENSER_A1_8_WORDS ,&F2_Moto_Driver_NBUSY1_D, &F2_SPI_Busy2_D}, //MOTOR_SCREW - Winder screw {&F1_MOTO_SCREW_A1_TX_00, &F1_MOTO_SCREW_A1_TX_01, &F1_MOTO_SCREW_A1_RX_00 ,&F1_MOTO_SCREW_A1_RX_01 ,&F1_MOTO_SCREW_A1_WORDS ,&F1_Moto_Driver_NSTBYRST2, &F1_SPI_Busy2_D}, //MOTOR_WINDER - Winder {&F1_MOTO_WINDER_A1_TX_00, &F1_MOTO_WINDER_A1_TX_01, &F1_MOTO_WINDER_A1_RX_00 ,&F1_MOTO_WINDER_A1_RX_01 ,&F1_MOTO_WINDER_A1_WORDS ,&F1_Moto_Driver_NSTBYRST2, &F1_SPI_Busy1_D}, //MOTOR_LDANCER1 -Pooler Dancer adjustment {&F1_MOTO_LDANCER1_A1_TX_00, &F1_MOTO_LDANCER1_A1_TX_01, &F1_MOTO_LDANCER1_A1_RX_00 ,&F1_MOTO_LDANCER1_A1_RX_01 ,&F1_MOTO_LDANCER1_A1_WORDS ,&F1_Moto_Driver_NBUSY1_D, &F1_SPI_Busy1_D}, //MOTOR_LDANCER2 - Winder Dancer adjustment #1 {&F1_MOTO_LDANCER2_A1_TX_00, &F1_MOTO_LDANCER2_A1_TX_01, &F1_MOTO_LDANCER2_A1_RX_00 ,&F1_MOTO_LDANCER2_A1_RX_01 ,&F1_MOTO_LDANCER2_A1_WORDS ,&F1_Moto_Driver_NBUSY1_D, &F1_SPI_Busy1_D}, //MOTOR_LDRIVING - Pooler: {&F1_MOTO_LDRIVING_A1_TX_00, &F1_MOTO_LDRIVING_A1_TX_01, &F1_MOTO_LDRIVING_A1_RX_00 ,&F1_MOTO_LDRIVING_A1_RX_01 ,&F1_MOTO_LDRIVING_A1_WORDS ,&F1_Moto_Driver_NBUSY1_D, &F1_SPI_Busy1_D}, //MOTOR_LLOADING - Pooler loading {&F1_MOTO_LLOADING_A1_TX_00, &F1_MOTO_LLOADING_A1_TX_01, &F1_MOTO_LLOADING_A1_RX_00 ,&F1_MOTO_LLOADING_A1_RX_01 ,&F1_MOTO_LLOADING_A1_WORDS ,&F1_Moto_Driver_NBUSY1_D, &F1_SPI_Busy1_D}, //MOTOR_LPIVOT1 - Pooler loading pivot arm {&F1_MOTO_LPIVOT1_A1_TX_00, &F1_MOTO_LPIVOT1_A1_TX_01, &F1_MOTO_LPIVOT1_A1_RX_00 ,&F1_MOTO_LPIVOT1_A1_RX_01 ,&F1_MOTO_LPIVOT1_A1_WORDS ,&F1_Moto_Driver_NBUSY1_D, &F1_SPI_Busy2_D}, //MOTOR_RDANCER - Feeder dancer adjustment: {&F1_MOTO_RDANCER_A1_TX_00, &F1_MOTO_RDANCER_A1_TX_01, &F1_MOTO_RDANCER_A1_RX_00 ,&F1_MOTO_RDANCER_A1_RX_01 ,&F1_MOTO_RDANCER_A1_WORDS ,&F1_Moto_Driver_NBUSY2_D, &F1_SPI_Busy1_D}, //MOTOR_RDRIVING - Feeder: {&F1_MOTO_RDRIVING_A1_TX_00, &F1_MOTO_RDRIVING_A1_TX_01, &F1_MOTO_RDRIVING_A1_RX_00, &F1_MOTO_RDRIVING_A1_RX_01, &F1_MOTO_RDRIVING_A1_WORDS, &F1_Moto_Driver_NBUSY2_D, &F1_SPI_Busy1_D}, //MOTOR_RLOADARM - Right loading arm: {&F1_MOTO_RLOADARM_A1_TX_00, &F1_MOTO_RLOADARM_A1_TX_01, &F1_MOTO_RLOADARM_A1_RX_00 ,&F1_MOTO_RLOADARM_A1_RX_01 ,&F1_MOTO_RLOADARM_A1_WORDS ,&F1_Moto_Driver_NBUSY2_D, &F1_SPI_Busy1_D}, //MOTOR_RLOADING - Right Loading {&F1_MOTO_RLOADING_A1_TX_00, &F1_MOTO_RLOADING_A1_TX_01, &F1_MOTO_RLOADING_A1_RX_00, &F1_MOTO_RLOADING_A1_RX_01, &F1_MOTO_RLOADING_A1_WORDS, &F1_Moto_Driver_NBUSY2_D, &F1_SPI_Busy1_D}, //MOTOR_SPARE1_1 - Spare {&F3_MOTO_SPARE1_1_A1_TX_00, &F3_MOTO_SPARE1_1_A1_TX_01, &F3_MOTO_SPARE1_1_A1_RX_00 ,&F3_MOTO_SPARE1_1_A1_RX_01 ,&F3_MOTO_SPARE1_1_A1_WORDS ,&F3_Moto_Driver_NBUSY1_D, &F3_SPI_Busy1_D}, //MOTOR_SPARE1_2 - Spare {&F3_MOTO_SPARE1_2_A1_TX_00, &F3_MOTO_SPARE1_2_A1_TX_01, &F3_MOTO_SPARE1_2_A1_RX_00 ,&F3_MOTO_SPARE1_2_A1_RX_01 ,&F3_MOTO_SPARE1_2_A1_WORDS ,&F3_Moto_Driver_NBUSY1_D, &F3_SPI_Busy1_D}, //MOTOR_SPARE2_1 - Spare {&F3_MOTO_SPARE2_1_A1_TX_00, &F3_MOTO_SPARE2_1_A1_TX_01, &F3_MOTO_SPARE2_1_A1_RX_00 ,&F3_MOTO_SPARE2_1_A1_RX_01 ,&F3_MOTO_SPARE2_1_A1_WORDS ,&F3_Moto_Driver_NBUSY1_D, &F3_SPI_Busy1_D}, //MOTOR_SPARE2_2 - Spare {&F3_MOTO_SPARE2_2_A1_TX_00, &F3_MOTO_SPARE2_2_A1_TX_01, &F3_MOTO_SPARE2_2_A1_RX_00 ,&F3_MOTO_SPARE2_2_A1_RX_01 ,&F3_MOTO_SPARE2_2_A1_WORDS ,&F3_Moto_Driver_NBUSY1_D, &F3_SPI_Busy1_D}, //MOTOR_SPARE3_1 - Spare //{&F3_MOTO_SPARE3_1_A1_TX_00, &F3_MOTO_SPARE3_1_A1_TX_01, &F3_MOTO_SPARE3_1_A1_RX_00 ,&F3_MOTO_SPARE3_1_A1_RX_01 ,&F3_MOTO_SPARE3_1_A1_WORDS ,&F3_Moto_Driver_NBUSY1_D, &F3_SPI_Busy1_D}, }; uint8_t FPGA_SPI_Transnit(TimerMotors_t _motorId) { INT2SHORT Int2Short; //*FpgaMotMap[_motorId].WORDS = 4; //if((*FpgaMotMap[_motorId].NBUSY == NOTBUSY) && (*FpgaMotMap[_motorId].SPI_Busy == NOTBUSY)) { Int2Short.uint = Fpga_Spi[_motorId].TX_MOSI; *FpgaMotMap[_motorId].TX_01 = Int2Short.ushort.MSB; //SysCtlDelay(1); *FpgaMotMap[_motorId].TX_00 = Int2Short.ushort.LSB; // SysCtlDelay(100); //*FpgaMotMap[_motorId].WORDS = Fpga_Spi[_motorId].AMT_OF_Words; } return OK; } uint8_t FPGA_SPI_Receive(TimerMotors_t _motorId) { INT2SHORT Int2Short; //TODO - check if busy ? Int2Short.ushort.LSB = *FpgaMotMap[_motorId].RX_00; Int2Short.ushort.MSB = *FpgaMotMap[_motorId].RX_01; Fpga_Spi[_motorId].RX_MISO = Int2Short.uint; return OK; } //---------------- GET ------------------------------ void Read_Motors_Driver_ADC(void) { TimerMotors_t i = 0; for(i=0; i < NUM_OF_MOTORS; i++) { Fpga_Spi[i].TX_MOSI = (x_GET_PARAM | x_ADC_OUT)<<8; FPGA_SPI_Transnit(i); } delayms(45); for(i=0; i < NUM_OF_MOTORS; i++) { FPGA_Get_Res(i); MotorDriverResponse[i].ADC = Fpga_Spi[i].RX_MISO; if(MotorDriverResponse[i].ADC == 0) // != powerstep { Change_Motor_SPI_Direction(i,Current_Or_Voltage); } else { //ReportWithPackageFilter(GeneralFilter,"the motor's combined driver adc",__FILE__,__LINE__,i,RpMessage,MotorDriverResponse[i].ADC,0); } } delayms(45); for(i=0; i < NUM_OF_MOTORS; i++) { if(MotorDriverResponse[i].ADC == 0) { Fpga_Spi[i].TX_MOSI = (x_GET_PARAM | x_ADC_OUT)<<8; FPGA_SPI_Transnit(i); } } delayms(45); for(i=0; i < NUM_OF_MOTORS; i++) { if(MotorDriverResponse[i].ADC == 0) { FPGA_Get_Res(i); MotorDriverResponse[i].ADC = Fpga_Spi[i].RX_MISO; //ReportWithPackageFilter(GeneralFilter,"the motor's driver adc",__FILE__,__LINE__,i,RpMessage,MotorDriverResponse[i].ADC,0); } } } uint32_t Read_Motors_Driver_Type(TimerMotors_t i) { uint32_t status = OK; //#ifdef USE_POWERSTEP01 //First communicate with powerSTEP01 static bool first_time_flag = true; if(first_time_flag == true) { Read_Motors_Driver_ADC(); first_time_flag = false; } if((MotorDriverResponse[i].ADC >= 21) && (MotorDriverResponse[i].ADC <= 29)) // //(powerSTEP01, ADC = 24) { MotorDriverResponse[i].DriverType = CombinrdMotDriver; Extended_Motor_Param[i] = true; //default for dispensers, dryer-LID and DH-LID Voltage mode, for other motors Current mode if(i == HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID) MotorDriverResponse[i].DriverMode = Voltage; else MotorDriverResponse[i].DriverMode = Current; } else //#else { //Change the SPI direction and check if this driver is Current or Voltage /* if(MotorDriverResponse[i].DriverType == UnKnownMotDriver) { Change_Motor_SPI_Direction(i,Current_Or_Voltage); } delayms(5); Read_Motors_Driver_ADC(i);*/ Extended_Motor_Param[i] = false; if((MotorDriverResponse[i].ADC >= 9) && (MotorDriverResponse[i].ADC < 13)) { MotorDriverResponse[i].DriverType = CurrentMotDriver; //(L6472, ADC = 11) MotorDriverResponse[i].DriverMode = Current; } else if((MotorDriverResponse[i].ADC >= 15) && (MotorDriverResponse[i].ADC < 19)) { MotorDriverResponse[i].DriverType = VoltageMotDriver; //(L6470, ADC = 17) MotorDriverResponse[i].DriverMode = Voltage; } else { //UnKnownMotDriver MotorDriverResponse[i].DriverType = UnKnownMotDriver; if (isMotorConfigured(i)) ReportWithPackageFilter(GeneralFilter,"Unable to recognize the motor's driver type",__FILE__,__LINE__,i,RpMessage,MotorDriverResponse[i].ADC,0); status = ERROR; } } //#endif return status; } void FPGA_GetClrMotStat_Cmd(TimerMotors_t _motorId) { uint32_t temp; Fpga_Spi[_motorId].TX_MOSI = x_GET_STATUS; Fpga_Spi[_motorId].TX_MOSI = Fpga_Spi[_motorId].TX_MOSI << 24;// move the command to the MSB TODO necessary??? //Fpga_Spi[_motorId].AMT_OF_Words = 1; //FPGA_SPI_Transnit(_motorId); temp = Fpga_Spi[_motorId].TX_MOSI; MillisecReadFromMotor(_motorId, temp, 4, MotorGetStatusFromFPGA_Callback); } void FPGA_GetFPGAMotSpeed_Cmd(TimerMotors_t _motorId) { uint32_t temp; Fpga_Spi[_motorId].TX_MOSI = x_GET_PARAM | x_SPEED; Fpga_Spi[_motorId].TX_MOSI = Fpga_Spi[_motorId].TX_MOSI << 24;// move the command to the MSB TODO necessary??? //Fpga_Spi[_motorId].AMT_OF_Words = 1; temp = Fpga_Spi[_motorId].TX_MOSI; MillisecReadFromMotor(_motorId, temp, 4, MotorGetSpeedFromFPGA_Callback); } void FPGA_GetMotSpeed_Cmd(TimerMotors_t _motorId) { Fpga_Spi[_motorId].TX_MOSI = x_GET_PARAM | x_SPEED; Fpga_Spi[_motorId].TX_MOSI = Fpga_Spi[_motorId].TX_MOSI << 24;// move the command to the MSB TODO necessary??? //Fpga_Spi[_motorId].AMT_OF_Words = 1; FPGA_SPI_Transnit(_motorId); } void FPGA_Get_ADC_OUT_Cmd(TimerMotors_t _motorId) { Fpga_Spi[_motorId].TX_MOSI = x_GET_PARAM | x_ADC_OUT; Fpga_Spi[_motorId].TX_MOSI = Fpga_Spi[_motorId].TX_MOSI << 8;// FPGA_SPI_Transnit(_motorId); } void FPGA_GetMotPosition_Cmd(TimerMotors_t _motorId) { uint32_t temp; Fpga_Spi[_motorId].TX_MOSI = x_GET_PARAM | x_ABS_POS; Fpga_Spi[_motorId].TX_MOSI = Fpga_Spi[_motorId].TX_MOSI << 24;// move the command to the MSB TODO necessary??? //Fpga_Spi[_motorId].AMT_OF_Words = 1; temp = Fpga_Spi[_motorId].TX_MOSI; MillisecReadFromMotor(_motorId, temp, 4, MotorGetPositionFromFPGA_Callback); // FPGA_SPI_Transnit(_motorId); } void FPGA_GetMotMicroSteps_Cmd(TimerMotors_t _motorId) { Fpga_Spi[_motorId].TX_MOSI = x_GET_PARAM | x_STEP_MODE; Fpga_Spi[_motorId].TX_MOSI = Fpga_Spi[_motorId].TX_MOSI << 8;// move the command to the MSB TODO necessary??? //Fpga_Spi[_motorId].AMT_OF_Words = 1; FPGA_SPI_Transnit(_motorId); } void FPGA_Get_Res(TimerMotors_t _motorId) //The answer for every FPGA Get Cmd { FPGA_SPI_Receive(_motorId); } void FPGA_GetBusy() { #ifndef EVALUATION_BOARD // uint8_t i; F1MOTNBUSY1 F1_MOT_NBUSY1; F1MOTNBUSY2 F1_MOT_NBUSY2; F2MOTNBUSY1 F2_MOT_NBUSY1; F3MOTNBUSY1 F3_MOT_NBUSY1; //- for spare motors F1SPIBUSY1 F1_SPI_BUSY1; F1SPIBUSY2 F1_SPI_BUSY2; F2SPIBUSY1 F2_SPI_BUSY1; F2SPIBUSY2 F2_SPI_BUSY2; F3SPIBUSY1 F3_SPI_BUSY1;// - for spare motors //Read NBUSY from FPGA unsigned short readValue = 0; readValue = F1_Moto_Driver_NBUSY1_D; F1_MOT_NBUSY1.ushort = (uint16_t) ~((unsigned int) readValue); readValue = F1_Moto_Driver_NBUSY2_D; F1_MOT_NBUSY2.ushort = (uint16_t) ~((unsigned int) readValue); readValue = F2_Moto_Driver_NBUSY1_D; F2_MOT_NBUSY1.ushort = (uint16_t) ~((unsigned int) readValue); readValue = F3_Moto_Driver_NBUSY1_D; F3_MOT_NBUSY1.ushort = (uint16_t) ~((unsigned int) readValue);; // - for spare motors //Read SPI BUSY from FPGA F1_SPI_BUSY1.ushort = F1_SPI_Busy1_D; F1_SPI_BUSY2.ushort = F1_SPI_Busy2_D; F2_SPI_BUSY1.ushort = F2_SPI_Busy1_D; F2_SPI_BUSY2.ushort = F2_SPI_Busy2_D; F3_SPI_BUSY1.ushort = F3_SPI_Busy1_D; // - for spare motors MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD].Busy = (F1_MOT_NBUSY1.bits.F1_MOTO_DH_CLEANHEAD_A1_NBUSY) | F1_SPI_BUSY1.bits.F1_MOTO_DH_CLEANHEAD_A1_SDI_busy; MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH].Busy = (F1_MOT_NBUSY1.bits.F1_MOTO_DH_CLEANMECH_A1_NBUSY) | F1_SPI_BUSY1.bits.F1_MOTO_DH_CLEANMECH_A1_SDI_busy; MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_DH_LID].Busy = (F1_MOT_NBUSY1.bits.F1_MOTO_DH_LID_A1_NBUSY) | F1_SPI_BUSY2.bits.F1_MOTO_DH_LID_A1_SDI_busy; MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING].Busy = (F1_MOT_NBUSY1.bits.F1_MOTO_DRYER_DRIVING_A1_NBUSY) | F1_SPI_BUSY1.bits.F1_MOTO_DRYER_DRIVING_A1_SDI_busy; MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID].Busy = (F1_MOT_NBUSY1.bits.F1_MOTO_DRYER_LID_A1_NBUSY) | F1_SPI_BUSY1.bits.F1_MOTO_DRYER_LID_A1_SDI_busy; MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].Busy = (F1_MOT_NBUSY1.bits.F1_MOTO_DRYER_LOADARM_A1_NBUSY) | F1_SPI_BUSY2.bits.F1_MOTO_DRYER_LOADARM_A1_SDI_busy; MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1].Busy = (F2_MOT_NBUSY1.bits.F2_MOTO_DISPENSER_A1_1_NBUSY) | F2_SPI_BUSY2.bits.F2_MOTO_DISPENSER_A1_1_BUSY; MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_2].Busy = (F2_MOT_NBUSY1.bits.F2_MOTO_DISPENSER_A1_2_NBUSY) | F2_SPI_BUSY2.bits.F2_MOTO_DISPENSER_A1_1_BUSY; MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_3].Busy = (F2_MOT_NBUSY1.bits.F2_MOTO_DISPENSER_A1_3_NBUSY) | F2_SPI_BUSY2.bits.F2_MOTO_DISPENSER_A1_1_BUSY; MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_4].Busy = (F2_MOT_NBUSY1.bits.F2_MOTO_DISPENSER_A1_4_NBUSY) | F2_SPI_BUSY2.bits.F2_MOTO_DISPENSER_A1_1_BUSY; MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_5].Busy = (F2_MOT_NBUSY1.bits.F2_MOTO_DISPENSER_A1_5_NBUSY) | F2_SPI_BUSY2.bits.F2_MOTO_DISPENSER_A1_1_BUSY; MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_6].Busy = (F2_MOT_NBUSY1.bits.F2_MOTO_DISPENSER_A1_6_NBUSY) | F2_SPI_BUSY2.bits.F2_MOTO_DISPENSER_A1_1_BUSY; MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_7].Busy = (F2_MOT_NBUSY1.bits.F2_MOTO_DISPENSER_A1_7_NBUSY) | F2_SPI_BUSY2.bits.F2_MOTO_DISPENSER_A1_1_BUSY; MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_8].Busy = (F2_MOT_NBUSY1.bits.F2_MOTO_DISPENSER_A1_8_NBUSY) | F2_SPI_BUSY2.bits.F2_MOTO_DISPENSER_A1_1_BUSY; MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_SCREW].Busy = (F1_MOT_NBUSY2.bits.F1_MOTO_SCREW_A1_NBUSY) | F1_SPI_BUSY2.bits.F1_MOTO_SCREW_A1_SDI_busy; MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_WINDER].Busy = (F1_MOT_NBUSY2.bits.F1_MOTO_WINDER_A1_NBUSY) | F1_SPI_BUSY1.bits.F1_MOTO_WINDER_A1_SDI_busy; MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].Busy = (F1_MOT_NBUSY1.bits.F1_MOTO_LDANCER1_A1_NBUSY) | F1_SPI_BUSY2.bits.F1_MOTO_LDANCER1_A1_SDI_busy; MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_LDANCER2].Busy = (F1_MOT_NBUSY1.bits.F1_MOTO_LDANCER2_A1_NBUSY) | F1_SPI_BUSY1.bits.F1_MOTO_LDANCER2_A1_SDI_busy; MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_LDRIVING].Busy = (F1_MOT_NBUSY1.bits.F1_MOTO_LDRIVING_A1_NBUSY) | F1_SPI_BUSY1.bits.F1_MOTO_LDRIVING_A1_SDI_busy; MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].Busy = (F1_MOT_NBUSY1.bits.F1_MOTO_LLOADING_A1_NBUSY) | F1_SPI_BUSY1.bits.F1_MOTO_LLOADING_A1_SDI_busy; MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_LPIVOT1].Busy = (F1_MOT_NBUSY1.bits.F1_MOTO_LPIVOT1_A1_NBUSY) | F1_SPI_BUSY2.bits.F1_MOTO_LPIVOT1_A1_SDI_busy; MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].Busy = (F1_MOT_NBUSY2.bits.F1_MOTO_RDANCER_A1_NBUSY) & F1_SPI_BUSY2.bits.F1_MOTO_RDANCER_A1_SDI_busy; MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].Busy = (F1_MOT_NBUSY2.bits.F1_MOTO_RDRIVING_A1_NBUSY) | F1_SPI_BUSY1.bits.F1_MOTO_RDRIVING_A1_SDI_busy; MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].Busy = (F1_MOT_NBUSY2.bits.F1_MOTO_RLOADING_A1_NBUSY) | F1_SPI_BUSY1.bits.F1_MOTO_RLOADING_A1_SDI_busy; MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_SPARE1_1].Busy = (F3_MOT_NBUSY1.bits.F3_MOTO_SPARE1_1_A1_NBUSY) | F3_SPI_BUSY1.bits.F3_MOTO_SPARE1_1_A1_SDI_busy; MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_SPARE1_2].Busy = (F3_MOT_NBUSY1.bits.F3_MOTO_SPARE1_2_A1_NBUSY) | F3_SPI_BUSY1.bits.F3_MOTO_SPARE1_2_A1_SDI_busy; MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_SPARE2_1].Busy = (F3_MOT_NBUSY1.bits.F3_MOTO_SPARE2_1_A1_NBUSY) | F3_SPI_BUSY1.bits.F3_MOTO_SPARE2_1_A1_SDI_busy; MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_SPARE2_2].Busy = (F3_MOT_NBUSY1.bits.F3_MOTO_SPARE2_2_A1_NBUSY) | F3_SPI_BUSY1.bits.F3_MOTO_SPARE2_2_A1_SDI_busy; TempSensorResponse[TEMP_SENSE_ANALOG_MIXCHIP_TEMP].Busy = F2_SPI_BUSY1.bits.F2_ANALOG_MIXCHIP_TEMP_1_BUSY; TempSensorResponse[TEMP_SENSE_ANALOG_DYEINGH_TEMP1].Busy = F2_SPI_BUSY1.bits.F2_ANALOG_DYEINGH_TEMP1_1_BUSY; TempSensorResponse[TEMP_SENSE_ANALOG_DYEINGH_TEMP2].Busy = F2_SPI_BUSY1.bits.F2_ANALOG_DYEINGH_TEMP2_1_BUSY; TempSensorResponse[TEMP_SENSE_ANALOG_DYEINGH_TEMP3].Busy = F2_SPI_BUSY1.bits.F2_ANALOG_DYEINGH_TEMP3_1_BUSY; TempSensorResponse[TEMP_SENSE_ANALOG_DYEINGH_TEMP4].Busy = F2_SPI_BUSY1.bits.F2_ANALOG_DYEINGH_TEMP4_1_BUSY; TempSensorResponse[TEMP_SENSE_ANALOG_DYEINGH_TEMP5].Busy = F2_SPI_BUSY1.bits.F2_ANALOG_DYEINGH_TEMP5_1_BUSY; TempSensorResponse[TEMP_SENSE_ANALOG_DRYER_TEMP1].Busy = F2_SPI_BUSY1.bits.F2_ANALOG_DRYER_TEMP1_1_BUSY; TempSensorResponse[TEMP_SENSE_ANALOG_DRYER_TEMP2].Busy = F2_SPI_BUSY1.bits.F2_ANALOG_DRYER_TEMP2_1_BUSY; TempSensorResponse[TEMP_SENSE_ANALOG_DRYER_TEMP3].Busy = F2_SPI_BUSY1.bits.F2_ANALOG_DRYER_TEMP3_1_BUSY; TempSensorResponse[TEMP_SENSE_AN_ENCLOSURETEMP1].Busy = F2_SPI_BUSY1.bits.F2_AN_ENCLOSURETEMP1_1_BUSY; TempSensorResponse[TEMP_SENSE_AN_ENCLOSURETEMP2].Busy = F2_SPI_BUSY1.bits.F2_AN_ENCLOSURETEMP2_1_BUSY; TempSensorResponse[TEMP_SENSE_AN_ENCLOSURETEMP3].Busy = F2_SPI_BUSY1.bits.F2_AN_ENCLOSURETEMP3_1_BUSY; Dancer_Busy[HARDWARE_DANCER_TYPE__MiddleDancer] = F1_SPI_BUSY1.bits.F1_LDANCER2_ROTENC_DATA_p_busy; Dancer_Busy[HARDWARE_DANCER_TYPE__LeftDancer] = F1_SPI_BUSY1.bits.F1_LDANCER1_ROTENC_DATA_p_busy; Dancer_Busy[HARDWARE_DANCER_TYPE__RightDancer] = F1_SPI_BUSY1.bits.F1_RDANCER_ROTENC_DATA_p_busy; SpeedSensorResponseS.Busy = F1_SPI_BUSY1.bits.F1_RSPARE_ROTENC_DATA_p_busy; #endif } int MotorGetFPGAResponse(TimerMotors_t MotorId,uint32_t *Data) //returns -1 on busy or other error { INT2SHORT Int2Short; //if((*FpgaMotMap[MotorId].NBUSY == NOTBUSY) && (*FpgaMotMap[MotorId].SPI_Busy == NOTBUSY)) //TODO - check if busy ? { Int2Short.ushort.LSB = *FpgaMotMap[MotorId].RX_00; Int2Short.ushort.MSB = *FpgaMotMap[MotorId].RX_01; *Data = Int2Short.uint; return OK; } //else // return ERROR; } //---------------- SET ------------------------------ //uint32_t MotorSentData[NUM_OF_MOTORS][100] = {0}; //int MotorDataIndex[NUM_OF_MOTORS] = {0}; int MotorSendFPGARequest(TimerMotors_t MotorId,uint32_t Data, uint32_t Length) //returns -1 on busy or other error { INT2SHORT Int2Short; // MotorSentData[MotorId][MotorDataIndex[MotorId]] = Data; // MotorDataIndex[MotorId]+=1; // if (MotorDataIndex[MotorId] == 99) MotorDataIndex[MotorId] = 0; //if((*FpgaMotMap[MotorId].NBUSY == NOTBUSY) && (*FpgaMotMap[MotorId].SPI_Busy == NOTBUSY)) { ROM_IntMasterDisable(); Int2Short.uint = Data; *FpgaMotMap[MotorId].TX_01 = Int2Short.ushort.MSB; *FpgaMotMap[MotorId].TX_00 = Int2Short.ushort.LSB; //*FpgaMotMap[MotorId].WORDS = Length; ROM_IntMasterEnable(); return OK; } //else // return ERROR; } typedef enum { MOTOR_CONFIG_INIT, MOTOR_CONFIG_HARD_HIZ, //MOTOR_CONFIG_READ_ADC, MOTOR_CONFIG_ACC, MOTOR_CONFIG_DEC, MOTOR_CONFIG_MIN_SPEED, MOTOR_CONFIG_MAX_SPEED, MOTOR_CONFIG_STEP_MODE, MOTOR_CONFIG_CONFIG, MOTOR_CONFIG_KVAL_TVAL_HOLD, MOTOR_CONFIG_KVAL_TVAL_RUN, MOTOR_CONFIG_KVAL_TVAL_ACC, MOTOR_CONFIG_KVAL_TVAL_DEC, MOTOR_CONFIG_ST_SLP_T_FAST, MOTOR_CONFIG_INT_SPD, MOTOR_CONFIG_FN_SLP_ACC_TON_MIN, MOTOR_CONFIG_FN_SLP_DEC_TOFF_MIN, MOTOR_CONFIG_OCD_TH, MOTOR_CONFIG_STALL_TH, MOTOR_CONFIG_K_THERM, MOTOR_CONFIG_FS_SPD, MOTOR_CONFIG_GATECFG1, MOTOR_CONFIG_GATECFG2, MOTOR_CONFIG_END }MOTOR_CONFIG_STAGES_ENUM; MOTOR_CONFIG_STAGES_ENUM ConfigStages[NUM_OF_MOTORS] = {MOTOR_CONFIG_INIT}; void FPGA_SetMotorsInit() { uint8_t i=0; MotorConfiguredTimeout = 100; //since powerstep in stby on powerup) F1_Moto_Driver_NSTBYRST1 = 0xFFFF; F1_Moto_Driver_NSTBYRST2 = 0xFFFF; F2_Moto_Driver_NSTBYRST1 = 0xFFFF; F3_Moto_Driver_NSTBYRST1 = 0xFFFF; SysCtlDelay(120000);//1mSec for(i=0;i5) && (i<14))//dispensers //{ //delayms(100); /* //limit the current to protect the motors // MotorDriverRequest[i].KvalHold = 0x10; // MotorDriverRequest[i].KvalRun = 0x10; // MotorDriverRequest[i].KvalAcc = 0x10; // MotorDriverRequest[i].KvalDec = 0x10; MotorsCfg[i].kvalhold = 0x10; MotorsCfg[i].kvalrun = 0x10; MotorDriverRequest[i].KvalAcc = 0x10; MotorDriverRequest[i].KvalDec = 0x10; FPGA_SetMotKvalHold(i); //delayms(100); FPGA_SetMotKvalRun(i); //delayms(100); FPGA_SetMotKvalAcc(i); //delayms(100); FPGA_SetMotKvalDec(i); //delayms(100); //} */ if(MotorDriverResponse[i].DriverType == CombinrdMotDriver) { FPGA_SetMotPowerStep01GateCfg(0, i); //delayms(1); } if(MotorDriverResponse[i].DriverType == CombinrdMotDriver) { FPGA_SetMotPowerStep01GateCfg(1, i); //delayms(1); } // } #endif } //TODO Prescaler settings for SPI //F1_Prescaler1_reg3 = //3bit spi moto low duty cycle value for pmw //F1_Prescaler1_reg4 = //3bit spi moto high duty cycle value for pmw //F2_Prescaler1_reg4 = //3bit spi moto low duty cycle value for pmw //F2_Prescaler1_reg5 = //3bit spi moto high duty cycle value for pmw //F3_Prescaler1_reg3 = //3bit spi moto low duty cycle value for pmw //F3_Prescaler1_reg4 = //3bit spi moto high duty cycle value for pmw } //char nop; uint32_t FPGA_MotorConfig_callback(TimerMotors_t _motorId, uint32_t ReadValue) { char MultiStep[8] = {1,2,4,8,16,32,64,128}; char i; char good = 0xFF; uint32_t temp = 0; uint32_t input_value = 0; uint32_t speed_calc = 0; //static char Motor_Type_Flag = 0; char CM_VM = 0; #ifdef RUN_AS_MAIN_JIG return OK;// don't use parameters on JIG #else if (_motorId > NUM_OF_MOTORS) { LOG_ERROR (_motorId, "Invalid motor id"); return ERROR; } switch (ConfigStages[_motorId]) { /*case MOTOR_CONFIG_READ_ADC: - no need done in init ConfigStages[_motorId]++; if(MotorDriverResponse[_motorId].DriverType == UnKnownMotDriver) { Read_Motors_Driver_Type(_motorId); } // else // { // nop++; // } //intentional fall through*/ case MOTOR_CONFIG_ACC: ConfigStages[_motorId]++; temp = x_SET_PARAM | x_ACC; temp = temp << 24; temp |= ((int)(MotorsCfg[_motorId].maxchangeslope)<<8); MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback); break; case MOTOR_CONFIG_DEC: ConfigStages[_motorId]++; temp = x_SET_PARAM | x_DEC; temp = temp << 24; temp |= ((int)(MotorsCfg[_motorId].maxchangeslope)<<8); MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback); break; case MOTOR_CONFIG_MIN_SPEED: ConfigStages[_motorId]++; if(MotorsCfg[_motorId].lowspeedoptimization!=0) { temp = x_SET_PARAM | x_MIN_SPEED; temp = temp << 24; speed_calc = MaxSpdCalc(MotorsCfg[_motorId].minfrequency); speed_calc|= 0x800; //LSPD_OPT temp |= speed_calc<<8; MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback); break; } case MOTOR_CONFIG_MAX_SPEED: ConfigStages[_motorId]++; temp = x_SET_PARAM | x_MAX_SPEED; temp = temp << 24; speed_calc = MaxSpdCalc(MotorsCfg[_motorId].maxfrequency); temp |= speed_calc<<8; MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback); break; case MOTOR_CONFIG_STEP_MODE: ConfigStages[_motorId]++; if(MotorsCfg[_motorId].microstep!=0) { for (i = 0; i < 8; i++) { if (MotorsCfg[_motorId].microstep == MultiStep[i]) { good = i; break; } } if(good != 0xFF) { temp = x_SET_PARAM | x_STEP_MODE; temp = temp << 24; // //if((_motorId == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING) || (_motorId == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING) || (_motorId == HARDWARE_MOTOR_TYPE__MOTO_WINDER)) // if(MotorDriverResponse[_motorId].ADC < 14) // = 11 // { // CM_VM = CurrentMotDriver; // } // else // //if(MotorDriverResponse[_motorId].ADC >= 14) // = 17 // { // //CM_VM = (MotorsCfg[_motorId].setmicrostep << 3) ; // CM_VM = VoltageMotDriver; // } // /*else // if(MotorDriverResponse[_motorId].ADC > TBD) // = TBD // { // //powerSTEP01 // if(_motorId == TBD) // { // CM_VM = CurrentMotDriver; // } // else // { // CM_VM = VoltageMotDriver; // } // }*/ /* if(MotorDriverResponse[_motorId].DriverType == CombinrdMotDriver) { CM_VM = CurrentMotDriver; } else CM_VM = MotorDriverResponse[_motorId].DriverType & 0xFF; */ CM_VM = MotorDriverResponse[_motorId].DriverMode;// 1<<3 or 0<<3 temp |= (!x_SYNC_EN | CM_VM | good | x_SYNC_SEL_1)<<16; MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback); break; } } case MOTOR_CONFIG_CONFIG: ConfigStages[_motorId]++; if(MotorsCfg[_motorId].configword!=0) { //temp = x_SET_PARAM | x_CONFIG; if(MotorDriverResponse[_motorId].DriverType == CombinrdMotDriver) { temp = x_SET_PARAM | x_POWERSTEP01_CONFIG; temp = temp << 24; if(MotorsCfg[_motorId].has_p01configword == true) { if( (_motorId == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING) || (_motorId == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING) || (_motorId == HARDWARE_MOTOR_TYPE__MOTO_SCREW) || (_motorId == HARDWARE_MOTOR_TYPE__MOTO_WINDER) ) { #ifdef EMC_FORCE_MOT_CLOCK_INTERNAL temp |= (MotorsCfg[_motorId].p01configword & ~(0x000F))<<8; #else temp |= MotorsCfg[_motorId].p01configword<<8; #endif } else { temp |= MotorsCfg[_motorId].p01configword<<8; } MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback); } else//backward compatibility { if(MotorDriverResponse[_motorId].DriverMode == Voltage) { //temp |= 0x2E8F<<8;//voltage temp |= (0x2E80 | (MotorsCfg[_motorId].configword & 0x000F)) <<8;//voltage MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback); } else { if((_motorId >= HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1) && (_motorId <= HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_8)) { //temp |= MotorsCfg[_motorId].configword<<8; //temp |= 0x1C8F<<8;//need to change in current mode? 0x1C0F with OCD 6 or 0x1C8F with OCD 15 temp |= (0x1C80 | (MotorsCfg[_motorId].configword & 0x000F)) <<8;// MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback); } else { if( (_motorId == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING) || (_motorId == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING) || (_motorId == HARDWARE_MOTOR_TYPE__MOTO_SCREW) || (_motorId == HARDWARE_MOTOR_TYPE__MOTO_WINDER) ) { #ifdef EMC_FORCE_MOT_CLOCK_INTERNAL temp |= 0x1C80<<8; #else temp |= (0x1C80 | (MotorsCfg[_motorId].configword & 0x000F)) <<8; #endif } else { temp |= (0x1C80 | (MotorsCfg[_motorId].configword & 0x000F)) <<8; } MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback); } } } } else { temp = x_SET_PARAM | x_CONFIG; temp = temp << 24; temp |= MotorsCfg[_motorId].configword<<8; MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback); } break; } case MOTOR_CONFIG_KVAL_TVAL_HOLD: ConfigStages[_motorId]++; input_value = 0; temp = x_SET_PARAM | x_KVAL_TVAL_HOLD; temp = temp << 24; if((MotorDriverResponse[_motorId].DriverMode == Current) && (Extended_Motor_Param[_motorId])) { input_value = MotorsCfg[_motorId].tvalhold; } else { input_value = MotorsCfg[_motorId].kvalhold; } if((MotorDriverResponse[_motorId].DriverMode == Current) && (MotorDriverResponse[_motorId].DriverType == CombinrdMotDriver)) { //in order use the same value as it is in L6472 (current limited to 4A instead of 10A in PowrStep01) input_value*=4; input_value/=10; } if(input_value != 0) { temp |= input_value<<16; if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR; break; } else { LOG_ERROR (_motorId,"kvalhold / tvalhold is Zero"); AlarmHandlingSetAlarm (EVENT_TYPE__MACHINE_STATE_HW_CONFIG_FAILED,ON); } case MOTOR_CONFIG_KVAL_TVAL_RUN: ConfigStages[_motorId]++; input_value = 0; temp = x_SET_PARAM | x_KVAL_TVAL_RUN; temp = temp << 24; if((MotorDriverResponse[_motorId].DriverMode == Current) && (Extended_Motor_Param[_motorId])) { input_value = MotorsCfg[_motorId].tvalrun; } else { input_value = MotorsCfg[_motorId].kvalrun; } if((MotorDriverResponse[_motorId].DriverMode == Current) && (MotorDriverResponse[_motorId].DriverType == CombinrdMotDriver)) { //in order use the same value as it is in L6472 (current limited to 4A instead of 10A in PowrStep01) input_value*=4; input_value/=10; } if(input_value != 0) { temp |= input_value<<16; if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR; break; } else { LOG_ERROR (_motorId,"kvalrun / tvalrun is Zero"); AlarmHandlingSetAlarm (EVENT_TYPE__MACHINE_STATE_HW_CONFIG_FAILED,ON); } case MOTOR_CONFIG_KVAL_TVAL_ACC: ConfigStages[_motorId]++; input_value = 0; temp = x_SET_PARAM | x_KVAL_TVAL_ACC; temp = temp << 24; if((MotorDriverResponse[_motorId].DriverMode == Current) && (Extended_Motor_Param[_motorId])) { input_value = MotorsCfg[_motorId].tvalacc; } else { input_value = MotorsCfg[_motorId].kvalacc; } if((MotorDriverResponse[_motorId].DriverMode == Current) && (MotorDriverResponse[_motorId].DriverType == CombinrdMotDriver)) { //in order use the same value as it is in L6472 (current limited to 4A instead of 10A in PowrStep01) input_value*=4; input_value/=10; } if(input_value != 0) { temp |= input_value<<16; if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR; break; } else { LOG_ERROR (_motorId,"kvalacc / tvalacc is Zero"); AlarmHandlingSetAlarm (EVENT_TYPE__MACHINE_STATE_HW_CONFIG_FAILED,ON); } case MOTOR_CONFIG_KVAL_TVAL_DEC: ConfigStages[_motorId]++; input_value = 0; temp = x_SET_PARAM | x_KVAL_TVAL_DEC; temp = temp << 24; if((MotorDriverResponse[_motorId].DriverMode == Current) && (Extended_Motor_Param[_motorId])) { input_value = MotorsCfg[_motorId].tvaldec; } else { if(Extended_Motor_Param[_motorId]) input_value = MotorsCfg[_motorId].kvaldec; else input_value = MotorsCfg[_motorId].kvalacc;//for old parameters files (we used kvaldec for x_FS_SPD } if((MotorDriverResponse[_motorId].DriverMode == Current) && (MotorDriverResponse[_motorId].DriverType == CombinrdMotDriver)) { //in order use the same value as it is in L6472 (current limited to 4A instead of 10A in PowrStep01) input_value*=4; input_value/=10; } if(input_value != 0) { temp |= input_value<<16; if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR; break; } else { LOG_ERROR (_motorId,"kvaldec / tvaldec is Zero"); AlarmHandlingSetAlarm (EVENT_TYPE__MACHINE_STATE_HW_CONFIG_FAILED,ON); } case MOTOR_CONFIG_ST_SLP_T_FAST: ConfigStages[_motorId]++; input_value = 0; temp = x_SET_PARAM | x_ST_SLP_T_FAST; temp = temp << 24; if((MotorDriverResponse[_motorId].DriverMode == Current) && (Extended_Motor_Param[_motorId])) { input_value = MotorsCfg[_motorId].tfast; } else { input_value = MotorsCfg[_motorId].stslp; } if(input_value != 0) { temp |= input_value<<16;; if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR; break; } else { LOG_ERROR (_motorId,"stslp / tfast is Zero"); AlarmHandlingSetAlarm (EVENT_TYPE__MACHINE_STATE_HW_CONFIG_FAILED,ON); } case MOTOR_CONFIG_INT_SPD: ConfigStages[_motorId]++; if(MotorsCfg[_motorId].intspd!=0) { temp = x_SET_PARAM | x_INT_SPD; temp = temp << 24; temp |= MotorsCfg[_motorId].intspd<<8; if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR; break; } case MOTOR_CONFIG_FN_SLP_ACC_TON_MIN: ConfigStages[_motorId]++; input_value = 0; temp = x_SET_PARAM | x_FN_SLP_ACC_TON_MIN; temp = temp << 24; if((MotorDriverResponse[_motorId].DriverMode == Current) && (Extended_Motor_Param[_motorId])) { input_value = MotorsCfg[_motorId].tonmin; } else { input_value = MotorsCfg[_motorId].fnslpacc; } temp |= input_value<<16;; if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR; break; case MOTOR_CONFIG_FN_SLP_DEC_TOFF_MIN: ConfigStages[_motorId]++; input_value = 0; temp = x_SET_PARAM | x_FN_SLP_DEC_TOFF_MIN; temp = temp << 24; if((MotorDriverResponse[_motorId].DriverMode == Current) && (Extended_Motor_Param[_motorId])) { input_value = MotorsCfg[_motorId].toffmin; } else { input_value = MotorsCfg[_motorId].fnslpdec; } temp |= input_value<<16;; if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR; break; case MOTOR_CONFIG_OCD_TH: ConfigStages[_motorId]++; if(MotorsCfg[_motorId].overcurrentthreshold!=0) { temp = x_SET_PARAM | x_OCD_TH; temp = temp << 24; if(MotorsCfg[_motorId].overcurrentthreshold == 0) { LOG_ERROR (_motorId,"OCD_TH is Zero"); AlarmHandlingSetAlarm (EVENT_TYPE__MACHINE_STATE_HW_CONFIG_FAILED,ON); } if(MotorDriverResponse[_motorId].DriverType == CombinrdMotDriver) { temp |= (MotorsCfg[_motorId].overcurrentthreshold * 2)<<16;//1.24 use 5 bits in powerstep01 and 4 bits in current/voltage drivers } else temp |= MotorsCfg[_motorId].overcurrentthreshold<<16; if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR; break; } case MOTOR_CONFIG_STALL_TH: ConfigStages[_motorId]++; if(MotorsCfg[_motorId].stallthreshold!=0) { temp = x_SET_PARAM | x_STALL_TH; temp = temp << 24; temp |= MotorsCfg[_motorId].stallthreshold<<16; if(MotorsCfg[_motorId].stallthreshold == 0) { LOG_ERROR (_motorId,"STALL_TH is Zero"); AlarmHandlingSetAlarm (EVENT_TYPE__MACHINE_STATE_HW_CONFIG_FAILED,ON); } if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR; break; } case MOTOR_CONFIG_K_THERM: ConfigStages[_motorId]++; if(MotorsCfg[_motorId].thermalcompensationfactor!=0) { temp = x_SET_PARAM | x_K_THERM; temp = temp << 24; temp |= MotorsCfg[_motorId].thermalcompensationfactor<<16; if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR; break; } case MOTOR_CONFIG_FS_SPD: ConfigStages[_motorId]++; if(MotorsCfg[_motorId].fsspd!=0) { temp = x_SET_PARAM | x_FS_SPD; temp = temp << 24; temp |= (/*FSCalc*/(MotorsCfg[_motorId].fsspd))<<8; if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR; break; } case MOTOR_CONFIG_GATECFG1: ConfigStages[_motorId]++; if(MotorDriverResponse[_motorId].DriverType == CombinrdMotDriver) { temp = x_SET_PARAM | x_POWERSTEP01_GATECFG1; temp = temp << 24; //Gate Current of 8mA for 2000 ns (boost: 125 ns) Clock watchdog disabled //temp |= (POWERSTEP01_TCC_2000ns | POWERSTEP01_IGATE_16mA | POWERSTEP01_TBOOST_125ns | POWERSTEP01_WD_EN_DISABLE)<<8; if((Extended_Motor_Param[_motorId]) && (MotorsCfg[_motorId].gatecfg1!=0)) { temp |= MotorsCfg[_motorId].gatecfg1<<8; } else { temp |= (POWERSTEP01_TCC_1000ns | POWERSTEP01_IGATE_24mA | POWERSTEP01_TBOOST_125ns | POWERSTEP01_WD_EN_DISABLE)<<8; } if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR; break; } case MOTOR_CONFIG_GATECFG2: ConfigStages[_motorId]++; if(MotorDriverResponse[_motorId].DriverType == CombinrdMotDriver) { temp = x_SET_PARAM | x_POWERSTEP01_GATECFG2; temp = temp << 24; //Blanking time: 125 ns, dead time: 125 ns if((Extended_Motor_Param[_motorId]) && (MotorsCfg[_motorId].gatecfg2!=0)) { temp |= MotorsCfg[_motorId].gatecfg2<<16; } else { temp |= (POWERSTEP01_TBLANK_125ns | POWERSTEP01_TDT_250ns)<<16; } if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR; break; } break; default: return ERROR; } if (ConfigStages[_motorId] >= MOTOR_CONFIG_END) { //notify original caller that motor is ready } return OK; #endif } uint32_t FPGA_MotorConfig(TimerMotors_t _motorId, MotorDriverConfigStruc *MotorConfig) { ConfigStages[_motorId] = MOTOR_CONFIG_HARD_HIZ; uint32_t temp; temp = x_HARD_HIZ; /*if (_motorId == HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM) { temp = x_HARD_STOP; }*/ temp = temp << 24; MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback); ConfigStages[_motorId]++; return OK; } void FPGA_SetMotSpeed(TimerMotors_t _motorId)//Run { uint32_t temp; temp = x_RUN | MotorDriverRequest[_motorId].Direction; temp = temp << 24; if (MotorDriverRequest[_motorId].Speed > 0xFFFFF) //TODO to check if it is needed here MotorDriverRequest[_motorId].Speed = 0xFFFFF; temp |= MotorDriverRequest[_motorId].Speed; MillisecSetMotorSpeed(_motorId, temp, 4, NULL); ///-------------------------------------------- // Fpga_Spi[_motorId].TX_MOSI = temp;//<<8; //Fpga_Spi[_motorId].AMT_OF_Words = 4; // FPGA_SPI_Transnit(_motorId); //----------------------------------------------------- } void FPGA_SetMotSpeedDirect(TimerMotors_t _motorId)//Run { uint32_t temp; temp = x_RUN | MotorDriverRequest[_motorId].Direction; temp = temp << 24; if (MotorDriverRequest[_motorId].Speed > 0xFFFFF) //TODO to check if it is needed here MotorDriverRequest[_motorId].Speed = 0xFFFFF; temp |= MotorDriverRequest[_motorId].Speed; Fpga_Spi[_motorId].TX_MOSI = temp; Fpga_Spi[_motorId].AMT_OF_Words = 4; //FPGA_SPI_Transnit(_motorId); MotorSendFPGARequest(_motorId,temp, 4); //returns -1 on busy or other error } void FPGA_SetMotMaxSpeed(TimerMotors_t _motorId)//Mov { uint32_t temp; temp = x_SET_PARAM | x_MAX_SPEED; temp = temp << 24; uint16_t speed_calc = MaxSpdCalc(MotorsCfg[_motorId].maxfrequency); if (speed_calc > 0x3FF) //TODO to check if it is needed here speed_calc = 0x3FF; temp |= speed_calc<<8; MillisecWriteToMotor(_motorId, temp, 4, NULL); } void FPGA_SetMotMicroStep(TimerMotors_t _motorId)// { char MultiStep[8] = {1,2,4,8,16,32,64,128}; char i; char good = 0xFF; uint32_t temp; char CM_VM = 0; for (i = 0; i < 8; i++) { if (MotorDriverRequest[_motorId].microstep == MultiStep[i]) { good = i; break; } } if(good != 0xFF) { temp = x_SET_PARAM | x_STEP_MODE; temp = temp << 24; // //if((_motorId == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING) || (_motorId == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING) || (_motorId == HARDWARE_MOTOR_TYPE__MOTO_WINDER)) // if(MotorDriverResponse[_motorId].ADC < 14) // = 11 // { // CM_VM = CurrentMotDriver; // } // else // //if(MotorDriverResponse[_motorId].ADC >= 14) // = 17 // { // //CM_VM = (MotorsCfg[_motorId].setmicrostep << 3) ; // CM_VM = VoltageMotDriver; // } // /*else // if(MotorDriverResponse[_motorId].ADC > TBD) // = TBD // { // //powerSTEP01 // if(_motorId == TBD) // { // CM_VM = CurrentMotDriver; // } // else // { // CM_VM = VoltageMotDriver; // } // }*/ //CM_VM = MotorDriverResponse[_motorId].DriverType & 0xFF; //The CM_VM bit sets the current control method between voltage and current mode: CM_VM = MotorDriverResponse[_motorId].DriverMode; temp |= (!x_SYNC_EN | CM_VM | good | x_SYNC_SEL_1)<<16; //ReportWithPackageFilter(GeneralFilter,"FPGA_SetMotMicroStep",__FILE__,__LINE__,_motorId,RpMessage,i,0); MillisecWriteToMotor(_motorId, temp, 4, NULL); } } void FPGA_SetGoToPosition(TimerMotors_t _motorId)//GoTo { uint32_t temp; temp = x_GOTO; temp = temp << 24; if (MotorDriverRequest[_motorId].Position > 0x3FFFFF) //TODO to check if it is needed here MotorDriverRequest[_motorId].Position = 0x3FFFFF; temp |= (MotorDriverRequest[_motorId].Position); Fpga_Spi[_motorId].TX_MOSI = temp; //MillisecWriteToMotor(_motorId, temp, 4, NULL); //FPGA_SPI_Transnit(_motorId); MillisecWriteToMotor(_motorId, temp, 4, NULL); } void FPGA_SetGoToDirPosition(TimerMotors_t _motorId)//GoTo_DIR command { uint32_t temp; temp = x_GOTO_DIR | MotorDriverRequest[_motorId].Direction; temp = temp << 24; if (MotorDriverRequest[_motorId].Position > 0x3FFFFF) //TODO to check if it is needed here MotorDriverRequest[_motorId].Position = 0x3FFFFF; temp |= (MotorDriverRequest[_motorId].Position); Fpga_Spi[_motorId].TX_MOSI = temp; //MillisecWriteToMotor(_motorId, temp, 4, NULL); FPGA_SPI_Transnit(_motorId); } void FPGA_SetMotPosition(TimerMotors_t _motorId)//Mov { uint32_t temp; temp = x_MOVE | MotorDriverRequest[_motorId].Direction; temp = temp << 24; if (MotorDriverRequest[_motorId].Position > 0x3FFFFF) //TODO to check if it is needed here MotorDriverRequest[_motorId].Position = 0x3FFFFF; temp |= MotorDriverRequest[_motorId].Position; Fpga_Spi[_motorId].TX_MOSI = temp; //Fpga_Spi[_motorId].AMT_OF_Words = 4; MillisecWriteToMotor(_motorId, temp, 4, NULL); // FPGA_SPI_Transnit(_motorId); } void FPGA_SetGoMotHome(TimerMotors_t _motorId) { Fpga_Spi[_motorId].TX_MOSI = x_GO_HOME; //TODO to check how to set the number of bytes to send Fpga_Spi[_motorId].TX_MOSI = Fpga_Spi[_motorId].TX_MOSI << 24;// move the command to the MSB TODO necessary??? //Fpga_Spi[_motorId].AMT_OF_Words = 1; FPGA_SPI_Transnit(_motorId); } void FPGA_SetMotHome(TimerMotors_t _motorId) { uint32_t temp; Fpga_Spi[_motorId].TX_MOSI = x_RESET_POS; //TODO to check how to set the number of bytes to send Fpga_Spi[_motorId].TX_MOSI = Fpga_Spi[_motorId].TX_MOSI << 24;// move the command to the MSB TODO necessary??? //Fpga_Spi[_motorId].AMT_OF_Words = 1; temp = Fpga_Spi[_motorId].TX_MOSI; if( MillisecWriteToMotor(_motorId, temp, 4, NULL) < 0 ) return ; //FPGA_SPI_Transnit(_motorId); } void FPGA_SetMotStop(TimerMotors_t _motorId) { uint32_t temp; switch(MotorDriverRequest[_motorId].Stop) { case Hard_Stop: Fpga_Spi[_motorId].TX_MOSI = x_HARD_STOP; break; case Soft_Stop: Fpga_Spi[_motorId].TX_MOSI = x_SOFT_STOP; break; case Hard_Hiz: Fpga_Spi[_motorId].TX_MOSI = x_HARD_HIZ ; break; case Soft_Hiz: Fpga_Spi[_motorId].TX_MOSI = x_SOFT_HIZ; break; default: Fpga_Spi[_motorId].TX_MOSI = x_HARD_STOP; //TODO which default value? break; } Fpga_Spi[_motorId].TX_MOSI = Fpga_Spi[_motorId].TX_MOSI << 24;// move the command to the MSB TODO necessary??? //Fpga_Spi[_motorId].AMT_OF_Words = 1;//TODO ? temp = Fpga_Spi[_motorId].TX_MOSI; if( MillisecWriteToMotor(_motorId, temp, 4, NULL) < 0 ) return ; // FPGA_SPI_Transnit(_motorId); } void FPGA_SetMotKvalHold(TimerMotors_t _motorId) { uint32_t temp; uint32_t input_value = 0; temp = x_SET_PARAM | x_KVAL_HOLD; temp = temp << 24; if((MotorDriverResponse[_motorId].DriverMode == Current) && (MotorDriverRequest[_motorId].KvalHold > 0x7F)) MotorDriverRequest[_motorId].KvalHold = 0x7F; if((MotorDriverResponse[_motorId].DriverMode == Current) && (Extended_Motor_Param[_motorId])) { input_value = MotorsCfg[_motorId].tvalhold; } else { input_value = MotorsCfg[_motorId].kvalhold; } if((MotorDriverResponse[_motorId].DriverMode == Current) && (MotorDriverResponse[_motorId].DriverType == CombinrdMotDriver)) { //in order use the same value as it is in L6472 (current limited to 4A instead of 10A in PowrStep01) input_value*=4; input_value/=10; } temp |= input_value<<16; Fpga_Spi[_motorId].TX_MOSI = temp; Fpga_Spi[_motorId].AMT_OF_Words = 4; ReportWithPackageFilter(GeneralFilter,"FPGA SetKvalHold",__FILE__,__LINE__,_motorId,RpMessage,MotorDriverResponse[_motorId].DriverMode,0); MillisecWriteToMotor(_motorId,temp,4,NULL); } void FPGA_SetMotKvalRun(TimerMotors_t _motorId) { uint32_t temp; uint32_t input_value = 0; temp = x_SET_PARAM | x_KVAL_RUN; temp = temp << 24; if((MotorDriverResponse[_motorId].DriverMode == Current) && (MotorDriverRequest[_motorId].KvalRun > 0x7F)) MotorDriverRequest[_motorId].KvalRun = 0x7F; if((MotorDriverResponse[_motorId].DriverMode == Current) && (Extended_Motor_Param[_motorId])) { input_value = MotorsCfg[_motorId].tvalrun; } else { input_value = MotorsCfg[_motorId].kvalrun; } if((MotorDriverResponse[_motorId].DriverMode == Current) && (MotorDriverResponse[_motorId].DriverType == CombinrdMotDriver)) { //in order use the same value as it is in L6472 (current limited to 4A instead of 10A in PowrStep01) input_value*=4; input_value/=10; } temp |= input_value<<16; Fpga_Spi[_motorId].TX_MOSI = temp; Fpga_Spi[_motorId].AMT_OF_Words = 4; ReportWithPackageFilter(GeneralFilter,"FPGA SetKvalRun",__FILE__,__LINE__,_motorId,RpMessage,temp,0); MillisecWriteToMotor(_motorId, temp, 4, NULL); //FPGA_SPI_Transnit(_motorId); } void FPGA_SetMotKvalAcc(TimerMotors_t _motorId) { uint32_t temp; uint32_t input_value = 0; temp = x_SET_PARAM | x_KVAL_ACC; temp = temp << 24; if((MotorDriverResponse[_motorId].DriverMode == Current) && (MotorDriverRequest[_motorId].KvalAcc > 0x7F)) MotorDriverRequest[_motorId].KvalAcc = 0x7F; if((MotorDriverResponse[_motorId].DriverMode == Current) && (Extended_Motor_Param[_motorId])) { input_value = MotorDriverRequest[_motorId].TvalAcc; } else { input_value = MotorDriverRequest[_motorId].KvalAcc; } if((MotorDriverResponse[_motorId].DriverMode == Current) && (MotorDriverResponse[_motorId].DriverType == CombinrdMotDriver)) { //in order use the same value as it is in L6472 (current limited to 4A instead of 10A in PowrStep01) input_value*=4; input_value/=10; } temp |= input_value<<16; Fpga_Spi[_motorId].TX_MOSI = temp; Fpga_Spi[_motorId].AMT_OF_Words = 4; MillisecWriteToMotor(_motorId, temp, 4, NULL); // FPGA_SPI_Transnit(_motorId); } void FPGA_SetMotKvalDec(TimerMotors_t _motorId) { uint32_t temp; uint32_t input_value = 0; temp = x_SET_PARAM | x_KVAL_DEC; temp = temp << 24; if((MotorDriverResponse[_motorId].DriverMode == Current) && (MotorDriverRequest[_motorId].KvalDec > 0x7F)) MotorDriverRequest[_motorId].KvalDec = 0x7F; if((MotorDriverResponse[_motorId].DriverMode == Current) && (Extended_Motor_Param[_motorId])) { input_value = MotorDriverRequest[_motorId].TvalDec; } else { input_value = MotorDriverRequest[_motorId].KvalDec; } if((MotorDriverResponse[_motorId].DriverMode == Current) && (MotorDriverResponse[_motorId].DriverType == CombinrdMotDriver)) { //in order use the same value as it is in L6472 (current limited to 4A instead of 10A in PowrStep01) input_value*=4; input_value/=10; } temp |= input_value<<16; Fpga_Spi[_motorId].TX_MOSI = temp; Fpga_Spi[_motorId].AMT_OF_Words = 4; FPGA_SPI_Transnit(_motorId); } void FPGA_SetMotPowerStep01GateCfg(bool GatecfgNo, TimerMotors_t _motorId) { uint32_t temp; if(GatecfgNo == 0) { temp = x_SET_PARAM | x_POWERSTEP01_GATECFG1; temp = temp << 24; //Gate Current of 8mA for 2000 ns (boost: 125 ns) Clock watchdog disabled //temp |= (POWERSTEP01_TCC_2000ns | POWERSTEP01_IGATE_16mA | POWERSTEP01_TBOOST_125ns | POWERSTEP01_WD_EN_DISABLE)<<8; if((Extended_Motor_Param[_motorId]) && (MotorsCfg[_motorId].gatecfg1!=0)) { temp |= MotorsCfg[_motorId].gatecfg1<<8; } else { temp |= (POWERSTEP01_TCC_1000ns | POWERSTEP01_IGATE_24mA | POWERSTEP01_TBOOST_125ns | POWERSTEP01_WD_EN_DISABLE)<<8; } Fpga_Spi[_motorId].TX_MOSI = temp; Fpga_Spi[_motorId].AMT_OF_Words = 4; //FPGA_SPI_Transnit(_motorId); //MotorSendFPGARequest(_motorId,temp, 4); //returns -1 on busy or other error MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback); //------------------------------------- //delayms(100); //------------------------------------- } else { temp = x_SET_PARAM | x_POWERSTEP01_GATECFG2; temp = temp << 24; //Blanking time: 125 ns, dead time: 125 ns if((Extended_Motor_Param[_motorId]) && (MotorsCfg[_motorId].gatecfg2!=0)) { temp |= MotorsCfg[_motorId].gatecfg2<<16; } else { temp |= (POWERSTEP01_TBLANK_125ns | POWERSTEP01_TDT_250ns)<<16; } Fpga_Spi[_motorId].TX_MOSI = temp; Fpga_Spi[_motorId].AMT_OF_Words = 4; //FPGA_SPI_Transnit(_motorId); //MotorSendFPGARequest(_motorId,temp, 4); //returns -1 on busy or other error MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback); } } void FPGA_SetOCD_TH(TimerMotors_t _motorId) { uint32_t temp; temp = x_SET_PARAM | x_OCD_TH; temp = temp << 24; if(MotorDriverRequest[_motorId].ocd_th != 0) { if(MotorDriverResponse[_motorId].DriverType == CombinrdMotDriver) { temp |= (MotorDriverRequest[_motorId].ocd_th * 2)<<16;//1.24 use 5 bits in powerstep01 and 4 bits in current/voltage drivers } else temp |= MotorDriverRequest[_motorId].ocd_th<<16; MillisecWriteToMotor(_motorId, temp, 4, NULL); } } /////////////////////////////////////////////////////////////////////////////// /* SPI spi; uint8_t SPI_Transnit(SPI Spi) { INT2SHORT Int2Short; Int2Short.uint = Spi.TX_MOSI; //Check Busy register and see if the SPI desired is available while((F1_SPI_Busy1_D != NOTBUSY ) || (F1_SPI_Busy2_D != NOTBUSY )); switch(Spi.MotID) { case MOTOR_RLOADING: F1_MOTO_RLOADING_A1_TX_01 = Int2Short.ushort.MSB; //Read the TX_01 register and make sure that the value was written properly. while( Int2Short.ushort.MSB != F1_MOTO_RLOADING_A1_TX_01); //Send a write to TX_00 F1_MOTO_RLOADING_A1_TX_00 = Int2Short.ushort.LSB; //Read the TX_00 register and make sure that the value was written properly. while( Int2Short.ushort.LSB != F1_MOTO_RLOADING_A1_TX_00); break; case MOTOR_RDRIVING: F1_MOTO_RDRIVING_A1_TX_01 = Int2Short.ushort.MSB; //Read the TX_01 register and make sure that the value was written properly. while( Int2Short.ushort.MSB != F1_MOTO_RDRIVING_A1_TX_01); //Send a write to TX_00 F1_MOTO_RDRIVING_A1_TX_00 = Int2Short.ushort.LSB; //Read the TX_00 register and make sure that the value was written properly. while( Int2Short.ushort.LSB != F1_MOTO_RDRIVING_A1_TX_00); break; case MOTOR_LDRIVING: F1_MOTO_LDRIVING_A1_TX_01 = Int2Short.ushort.MSB; //Read the TX_01 register and make sure that the value was written properly. while( Int2Short.ushort.MSB != F1_MOTO_LDRIVING_A1_TX_01); //Send a write to TX_00 F1_MOTO_LDRIVING_A1_TX_00 = Int2Short.ushort.LSB; //Read the TX_00 register and make sure that the value was written properly. while( Int2Short.ushort.LSB != F1_MOTO_LDRIVING_A1_TX_00); break; case MOTOR_LLOADING: F1_MOTO_LLOADING_A1_TX_01 = Int2Short.ushort.MSB; //Read the TX_01 register and make sure that the value was written properly. while( Int2Short.ushort.MSB != F1_MOTO_LLOADING_A1_TX_01); //Send a write to TX_00 F1_MOTO_LLOADING_A1_TX_00 = Int2Short.ushort.LSB; //Read the TX_00 register and make sure that the value was written properly. while( Int2Short.ushort.LSB != F1_MOTO_LLOADING_A1_TX_00); break; case MOTOR_DRYER_LOADARM: F1_MOTO_DRYER_LOADARM_A1_TX_01 = Int2Short.ushort.MSB; //Read the TX_01 register and make sure that the value was written properly. while( Int2Short.ushort.MSB != F1_MOTO_DRYER_LOADARM_A1_TX_01); //Send a write to TX_00 F1_MOTO_DRYER_LOADARM_A1_TX_00 = Int2Short.ushort.LSB; //Read the TX_00 register and make sure that the value was written properly. while( Int2Short.ushort.LSB != F1_MOTO_DRYER_LOADARM_A1_TX_00); break; case MOTOR_DRYER_DRIVING: F1_MOTO_DRYER_DRIVING_A1_TX_01 = Int2Short.ushort.MSB; //Read the TX_01 register and make sure that the value was written properly. while( Int2Short.ushort.MSB != F1_MOTO_DRYER_DRIVING_A1_TX_01); //Send a write to TX_00 F1_MOTO_DRYER_DRIVING_A1_TX_00 = Int2Short.ushort.LSB; //Read the TX_00 register and make sure that the value was written properly. while( Int2Short.ushort.LSB != F1_MOTO_DRYER_DRIVING_A1_TX_00); break; case MOTOR_DH_CLEANHEAD: F1_MOTO_DH_CLEANHEAD_A1_TX_01 = Int2Short.ushort.MSB; //Read the TX_01 register and make sure that the value was written properly. while( Int2Short.ushort.MSB != F1_MOTO_DH_CLEANHEAD_A1_TX_01); //Send a write to TX_00 F1_MOTO_DH_CLEANHEAD_A1_TX_00 = Int2Short.ushort.LSB; //Read the TX_00 register and make sure that the value was written properly. while( Int2Short.ushort.LSB != F1_MOTO_DH_CLEANHEAD_A1_TX_00); break; case MOTOR_DH_CLEANMECH: F1_MOTO_DH_CLEANMECH_A1_TX_01 = Int2Short.ushort.MSB; //Read the TX_01 register and make sure that the value was written properly. while( Int2Short.ushort.MSB != F1_MOTO_DH_CLEANMECH_A1_TX_01); //Send a write to TX_00 F1_MOTO_DH_CLEANMECH_A1_TX_00 = Int2Short.ushort.LSB; //Read the TX_00 register and make sure that the value was written properly. while( Int2Short.ushort.LSB != F1_MOTO_DH_CLEANMECH_A1_TX_00); break; case MOTOR_SCREW: F1_MOTO_SCREW_A1_TX_01 = Int2Short.ushort.MSB; //Read the TX_01 register and make sure that the value was written properly. while( Int2Short.ushort.MSB != F1_MOTO_SCREW_A1_TX_01); //Send a write to TX_00 F1_MOTO_SCREW_A1_TX_00 = Int2Short.ushort.LSB; //Read the TX_00 register and make sure that the value was written properly. while( Int2Short.ushort.LSB != F1_MOTO_SCREW_A1_TX_00); break; case MOTOR_WINDER: F1_MOTO_WINDER_A1_TX_01 = Int2Short.ushort.MSB; //Read the TX_01 register and make sure that the value was written properly. while( Int2Short.ushort.MSB != F1_MOTO_WINDER_A1_TX_01); //Send a write to TX_00 F1_MOTO_WINDER_A1_TX_00 = Int2Short.ushort.LSB; //Read the TX_00 register and make sure that the value was written properly. while( Int2Short.ushort.LSB != F1_MOTO_WINDER_A1_TX_00); break; case MOTOR_RLOADARM: F1_MOTO_RLOADARM_A1_TX_01 = Int2Short.ushort.MSB; //Read the TX_01 register and make sure that the value was written properly. while( Int2Short.ushort.MSB != F1_MOTO_RLOADARM_A1_TX_01); //Send a write to TX_00 F1_MOTO_RLOADARM_A1_TX_00 = Int2Short.ushort.LSB; //Read the TX_00 register and make sure that the value was written properly. while( Int2Short.ushort.LSB != F1_MOTO_RLOADARM_A1_TX_00); break; case MOTOR_RDANCER: F1_MOTO_RDANCER_A1_TX_01 = Int2Short.ushort.MSB; //Read the TX_01 register and make sure that the value was written properly. while( Int2Short.ushort.MSB != F1_MOTO_RDANCER_A1_TX_01); //Send a write to TX_00 F1_MOTO_RDANCER_A1_TX_00 = Int2Short.ushort.LSB; //Read the TX_00 register and make sure that the value was written properly. while( Int2Short.ushort.LSB != F1_MOTO_RDANCER_A1_TX_00); break; case MOTOR_LDANCER1: F1_MOTO_LDANCER1_A1_TX_01 = Int2Short.ushort.MSB; //Read the TX_01 register and make sure that the value was written properly. while( Int2Short.ushort.MSB != F1_MOTO_LDANCER1_A1_TX_01); //Send a write to TX_00 F1_MOTO_LDANCER1_A1_TX_00 = Int2Short.ushort.LSB; //Read the TX_00 register and make sure that the value was written properly. while( Int2Short.ushort.LSB != F1_MOTO_LDANCER1_A1_TX_00); break; case MOTOR_LDANCER2: F1_MOTO_LDANCER2_A1_TX_01 = Int2Short.ushort.MSB; //Read the TX_01 register and make sure that the value was written properly. while( Int2Short.ushort.MSB != F1_MOTO_LDANCER2_A1_TX_01); //Send a write to TX_00 F1_MOTO_LDANCER2_A1_TX_00 = Int2Short.ushort.LSB; //Read the TX_00 register and make sure that the value was written properly. while( Int2Short.ushort.LSB != F1_MOTO_LDANCER2_A1_TX_00); break; case MOTOR_DRYER_LID: F1_MOTO_DRYER_LID_A1_TX_01 = Int2Short.ushort.MSB; //Read the TX_01 register and make sure that the value was written properly. while( Int2Short.ushort.MSB != F1_MOTO_DRYER_LID_A1_TX_01); //Send a write to TX_00 F1_MOTO_DRYER_LID_A1_TX_00 = Int2Short.ushort.LSB; //Read the TX_00 register and make sure that the value was written properly. while( Int2Short.ushort.LSB != F1_MOTO_DRYER_LID_A1_TX_00); break; case MOTOR_DH_LID: F1_MOTO_DH_LID_A1_TX_01 = Int2Short.ushort.MSB; //Read the TX_01 register and make sure that the value was written properly. while( Int2Short.ushort.MSB != F1_MOTO_DH_LID_A1_TX_01); //Send a write to TX_00 F1_MOTO_DH_LID_A1_TX_00 = Int2Short.ushort.LSB; //Read the TX_00 register and make sure that the value was written properly. while( Int2Short.ushort.LSB != F1_MOTO_DH_LID_A1_TX_00); break; case MOTOR_LPIVOT1: F1_MOTO_LPIVOT1_A1_TX_01 = Int2Short.ushort.MSB; //Read the TX_01 register and make sure that the value was written properly. while( Int2Short.ushort.MSB != F1_MOTO_LPIVOT1_A1_TX_01); //Send a write to TX_00 F1_MOTO_LPIVOT1_A1_TX_00 = Int2Short.ushort.LSB; //Read the TX_00 register and make sure that the value was written properly. while( Int2Short.ushort.LSB != F1_MOTO_LPIVOT1_A1_TX_00); break; default: break; } return 0; } uint8_t SPI_Receive (SPI Spi) { INT2SHORT Int2Short; //Check busy before trying to read RX registers. while((F1_SPI_Busy1_D != NOTBUSY ) || (F1_SPI_Busy2_D != NOTBUSY )); switch(Spi.MotID) { case MOTOR_RLOADING: Int2Short.ushort.LSB = F1_MOTO_RLOADING_A1_RX_00; Int2Short.ushort.MSB = F1_MOTO_RLOADING_A1_RX_01; break; case MOTOR_RDRIVING: Int2Short.ushort.LSB = F1_MOTO_RDRIVING_A1_RX_00; Int2Short.ushort.MSB = F1_MOTO_RDRIVING_A1_RX_01; break; case MOTOR_LDRIVING: Int2Short.ushort.LSB = F1_MOTO_LDRIVING_A1_RX_00; Int2Short.ushort.MSB = F1_MOTO_LDRIVING_A1_RX_01; break; case MOTOR_LLOADING: Int2Short.ushort.LSB = F1_MOTO_LLOADING_A1_RX_00; Int2Short.ushort.MSB = F1_MOTO_LLOADING_A1_RX_01; break; case MOTOR_DRYER_LOADARM: Int2Short.ushort.LSB = F1_MOTO_DRYER_LOADARM_A1_RX_00; Int2Short.ushort.MSB = F1_MOTO_DRYER_LOADARM_A1_RX_01; break; case MOTOR_DRYER_DRIVING: Int2Short.ushort.LSB = F1_MOTO_DRYER_DRIVING_A1_RX_00; Int2Short.ushort.MSB = F1_MOTO_DRYER_DRIVING_A1_RX_01; break; case MOTOR_DH_CLEANHEAD: Int2Short.ushort.LSB = F1_MOTO_DH_CLEANHEAD_A1_RX_00; Int2Short.ushort.MSB = F1_MOTO_DH_CLEANHEAD_A1_RX_01; break; case MOTOR_DH_CLEANMECH: Int2Short.ushort.LSB = F1_MOTO_DH_CLEANMECH_A1_RX_00; Int2Short.ushort.MSB = F1_MOTO_DH_CLEANMECH_A1_RX_01; break; case MOTOR_SCREW: Int2Short.ushort.LSB = F1_MOTO_SCREW_A1_RX_00; Int2Short.ushort.MSB = F1_MOTO_SCREW_A1_RX_01; break; case MOTOR_WINDER: Int2Short.ushort.LSB = F1_MOTO_WINDER_A1_RX_00; Int2Short.ushort.MSB = F1_MOTO_WINDER_A1_RX_01; break; case MOTOR_RLOADARM: Int2Short.ushort.LSB = F1_MOTO_RLOADARM_A1_RX_00; Int2Short.ushort.MSB = F1_MOTO_RLOADARM_A1_RX_01; break; case MOTOR_RDANCER: Int2Short.ushort.LSB = F1_MOTO_RDANCER_A1_RX_00; Int2Short.ushort.MSB = F1_MOTO_RDANCER_A1_RX_01; break; case MOTOR_LDANCER1: Int2Short.ushort.LSB = F1_MOTO_LDANCER1_A1_RX_00; Int2Short.ushort.MSB = F1_MOTO_LDANCER1_A1_RX_01; break; case MOTOR_LDANCER2: Int2Short.ushort.LSB = F1_MOTO_LDANCER2_A1_RX_00; Int2Short.ushort.MSB = F1_MOTO_LDANCER2_A1_RX_01; break; case MOTOR_DRYER_LID: Int2Short.ushort.LSB = F1_MOTO_DRYER_LID_A1_RX_00; Int2Short.ushort.MSB = F1_MOTO_DRYER_LID_A1_RX_01; break; case MOTOR_DH_LID: Int2Short.ushort.LSB = F1_MOTO_DH_LID_A1_RX_00; Int2Short.ushort.MSB = F1_MOTO_DH_LID_A1_RX_01; break; case MOTOR_LPIVOT1: Int2Short.ushort.LSB = F1_MOTO_LPIVOT1_A1_RX_00; Int2Short.ushort.MSB = F1_MOTO_LPIVOT1_A1_RX_01; break; default: break; } Spi.RX_MISO = Int2Short.uint; return 0; } void FPGA_GetClrMotStat(SPI MotFPGA_Spi) { MotFPGA_Spi.TX_MOSI = x_GET_STATUS; SPI_Transnit(MotFPGA_Spi); SPI_Receive(MotFPGA_Spi); } void FPGA_GetMotSpeed(SPI MotFPGA_Spi) { MotFPGA_Spi.TX_MOSI = x_GET_PARAM | x_SPEED; SPI_Transnit(MotFPGA_Spi); SPI_Receive(MotFPGA_Spi); } */ //////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////// SPI TempSense_Spi[MAX_MAIN_CARD_TEMP_SENS_ID]; typedef struct { volatile short * TX_00; volatile short * TX_01; volatile short * RX_00; volatile short * RX_01; volatile short * WORDS; volatile short * SPI_Busy; // TODO - remove this fied and combine with the mtor struct } FpgaSpiMap_t; static FpgaSpiMap_t FpgaTempSenseMap[MAX_MAIN_CARD_TEMP_SENS_ID] = { //TEMP_SENSE_ANALOG_MIXCHIP_TEMP, //ANALOG_MIXCHIP_TEMP {&F2_ANALOG_MIXCHIP_TEMP_1_TX_00, &F2_ANALOG_MIXCHIP_TEMP_1_TX_01, &F2_ANALOG_MIXCHIP_TEMP_1_RX_00 ,&F2_ANALOG_MIXCHIP_TEMP_1_RX_01 ,&F2_ANALOG_MIXCHIP_TEMP_1_WORDS ,&F2_SPI_Busy1_D}, //TEMP_SENSE_ANALOG_DYEINGH_TEMP1, //ANALOG_DYEINGH_TEMP1_1 {&F2_ANALOG_DYEINGH_TEMP1_1_TX_00, &F2_ANALOG_DYEINGH_TEMP1_1_TX_01, &F2_ANALOG_DYEINGH_TEMP1_1_RX_00 ,&F2_ANALOG_DYEINGH_TEMP1_1_RX_01 ,&F2_ANALOG_DYEINGH_TEMP1_1_WORDS ,&F2_SPI_Busy1_D}, //TEMP_SENSE_ANALOG_DYEINGH_TEMP2, //ANALOG_DYEINGH_TEMP2 {&F2_ANALOG_DYEINGH_TEMP2_TX_00, &F2_ANALOG_DYEINGH_TEMP2_TX_01, &F2_ANALOG_DYEINGH_TEMP2_RX_00 ,&F2_ANALOG_DYEINGH_TEMP2_RX_01 ,&F2_ANALOG_DYEINGH_TEMP2_WORDS ,&F2_SPI_Busy1_D}, //TEMP_SENSE_ANALOG_DYEINGH_TEMP3, //ANALOG_DYEINGH_TEMP3 {&F2_ANALOG_DYEINGH_TEMP3_1_TX_00, &F2_ANALOG_DYEINGH_TEMP3_1_TX_01, &F2_ANALOG_DYEINGH_TEMP3_1_RX_00 ,&F2_ANALOG_DYEINGH_TEMP3_1_RX_01 ,&F2_ANALOG_DYEINGH_TEMP3_1_WORDS ,&F2_SPI_Busy1_D}, //TEMP_SENSE_ANALOG_DYEINGH_TEMP4, //ANALOG_DYEINGH_TEMP4 {&F2_ANALOG_DYEINGH_TEMP4_1_TX_00, &F2_ANALOG_DYEINGH_TEMP4_1_TX_01, &F2_ANALOG_DYEINGH_TEMP4_1_RX_00 ,&F2_ANALOG_DYEINGH_TEMP4_1_RX_01 ,&F2_ANALOG_DYEINGH_TEMP4_1_WORDS ,&F2_SPI_Busy1_D}, //TEMP_SENSE_ANALOG_DYEINGH_TEMP5, //ANALOG_DYEINGH_TEMP5 {&F2_ANALOG_DYEINGH_TEMP5_1_TX_00, &F2_ANALOG_DYEINGH_TEMP5_1_TX_01, &F2_ANALOG_DYEINGH_TEMP5_1_RX_00 ,&F2_ANALOG_DYEINGH_TEMP5_1_RX_01 ,&F2_ANALOG_DYEINGH_TEMP5_1_WORDS ,&F2_SPI_Busy1_D}, //TEMP_SENSE_ANALOG_DRYER_TEMP1, //ANALOG_DRYER_TEMP1 {&F2_ANALOG_DRYER_TEMP1_1_TX_00, &F2_ANALOG_DRYER_TEMP1_1_TX_01, &F2_ANALOG_DRYER_TEMP1_1_RX_00 ,&F2_ANALOG_DRYER_TEMP1_1_RX_01 ,&F2_ANALOG_DRYER_TEMP1_1_WORDS ,&F2_SPI_Busy1_D}, //TEMP_SENSE_ANALOG_DRYER_TEMP2, //ANALOG_DRYER_TEMP2 {&F2_ANALOG_DRYER_TEMP2_1_TX_00, &F2_ANALOG_DRYER_TEMP2_1_TX_01, &F2_ANALOG_DRYER_TEMP2_1_RX_00 ,&F2_ANALOG_DRYER_TEMP2_1_RX_01 ,&F2_ANALOG_DRYER_TEMP2_1_WORDS ,&F2_SPI_Busy1_D}, //TEMP_SENSE_ANALOG_DRYER_TEMP3, //ANALOG_DRYER_TEMP3 {&F2_ANALOG_DRYER_TEMP3_1_TX_00, &F2_ANALOG_DRYER_TEMP3_1_TX_01, &F2_ANALOG_DRYER_TEMP3_1_RX_00 ,&F2_ANALOG_DRYER_TEMP3_1_RX_01 ,&F2_ANALOG_DRYER_TEMP3_1_WORDS ,&F2_SPI_Busy1_D}, //TEMP_SENSE_AN_ENCLOSURETEMP1, //AN_ENCLOSURETEMP1_1 {&F2_AN_ENCLOSURETEMP1_1_TX_00, &F2_AN_ENCLOSURETEMP1_1_TX_01, &F2_AN_ENCLOSURETEMP1_1_RX_00 ,&F2_AN_ENCLOSURETEMP1_1_RX_01 ,&F2_AN_ENCLOSURETEMP1_1_WORDS ,&F2_SPI_Busy1_D}, //TEMP_SENSE_AN_ENCLOSURETEMP2, //AN_ENCLOSURETEMP2_1 {&F2_AN_ENCLOSURETEMP2_1_TX_00, &F2_AN_ENCLOSURETEMP2_1_TX_01, &F2_AN_ENCLOSURETEMP2_1_RX_00 ,&F2_AN_ENCLOSURETEMP2_1_RX_01 ,&F2_AN_ENCLOSURETEMP2_1_WORDS ,&F2_SPI_Busy1_D}, //TEMP_SENSE_AN_ENCLOSURETEMP3, //AN_ENCLOSURETEMP3_1 {&F2_AN_ENCLOSURETEMP3_1_TX_00, &F2_AN_ENCLOSURETEMP3_1_TX_01, &F2_AN_ENCLOSURETEMP3_1_RX_00 ,&F2_AN_ENCLOSURETEMP3_1_RX_01 ,&F2_AN_ENCLOSURETEMP3_1_WORDS ,&F2_SPI_Busy1_D}, }; //void FPGA_WRITE() //{ // /*int i,j; // uint16_t *Temp; // Temp = &F1_Ver1_D; // uint16_t a[209]; // memset(Temp,0xFFFF,200); // memcpy(a,Temp,208); // for(j=0;j<32000;j++) // { // j++; // for(i=0;i<32000;i++) // { // *(&F2_ANALOG_DRYER_TEMP3_1_TX_00+i) = 0xFFFF; // } // }*/ //} int SPISendFPGARequest(TEMPERATURE_SENSOR_ID_ENUM SensorId,uint32_t Data, uint32_t Length) //returns -1 on busy or other error { INT2SHORT Int2Short; int Status; if(TempSensorResponse[SensorId].Busy == NOTBUSY) { Int2Short.uint = Data; *FpgaTempSenseMap[SensorId].WORDS = Length; *FpgaTempSenseMap[SensorId].TX_01 = Int2Short.ushort.MSB; *FpgaTempSenseMap[SensorId].TX_00 = Int2Short.ushort.LSB; Status = OK; } else Status = ERROR; return Status; } int SPIGetFPGAResponse(TEMPERATURE_SENSOR_ID_ENUM SensorId,uint32_t *Data) //returns -1 on busy or other error { INT2SHORT Int2Short; int Status; if(TempSensorResponse[SensorId].Busy == NOTBUSY) //TODO - check if busy ? { Int2Short.ushort.LSB = *FpgaTempSenseMap[SensorId].RX_00; Int2Short.ushort.MSB = *FpgaTempSenseMap[SensorId].RX_01; *Data = Int2Short.uint; Status = OK; } else Status = ERROR; return Status; } //////////////////////////////////////////////////////////////// /* //temporary till the FPGA change: unsigned short F1_Moto_Driver_SPI_DIRECTION1 = 0; unsigned short F1_Moto_Driver_SPI_DIRECTION2 = 0; unsigned short F2_Moto_Driver_SPI_DIRECTION1 = 0; unsigned short F3_Moto_Driver_SPI_DIRECTION1 = 0; */ F1_SPI_DIRECTION1 F1_SPI_Direction1; F1_SPI_DIRECTION2 F1_SPI_Direction2; F2_SPI_DIRECTION1 F2_SPI_Direction1; F3_SPI_DIRECTION1 F3_SPI_Direction1; void Change_Motor_SPI_Direction(TimerMotors_t _motorId, SPI_MOT_DRIVER_DIRECTION Spi_Direction) { /* //read direction registers F1_SPI_Direction1.ushort = F1_Moto_Driver_SPI_DIRECTION1; F1_SPI_Direction2.ushort = F1_Moto_Driver_SPI_DIRECTION2; F2_SPI_Direction1.ushort = F2_Moto_Driver_SPI_DIRECTION1; F3_SPI_Direction1.ushort = F3_Moto_Driver_SPI_DIRECTION1; */ //change direction switch(_motorId) { case HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD: //MOTOR_DH_CLEANHEAD - Cleaning head motor F1_SPI_Direction1.bits.F1_MOTO_DH_CLEANHEAD = Spi_Direction; break; case HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH: //MOTOR_DH_CLEANMECH - Cleaning mechanism F1_SPI_Direction1.bits.F1_MOTO_DH_CLEANMECH = Spi_Direction; break; case HARDWARE_MOTOR_TYPE__MOTO_DH_LID: //MOTOR_DH_LID - Dyeing head lid F1_SPI_Direction1.bits.F1_MOTO_DH_LID = Spi_Direction; break; case HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING: //MOTOR_DRYER_DRIVING - Dryer main motor F1_SPI_Direction1.bits.F1_MOTO_DRYER_DRIVING = Spi_Direction; break; case HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID: //MOTOR_DRYER_LID - Drier lid F1_SPI_Direction1.bits.F1_MOTO_DRYER_LID = Spi_Direction; break; case HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM: //MOTOR_DRYER_LOADARM - Drier thread loading arm F1_SPI_Direction1.bits.F1_MOTO_DRYER_LOADARM = Spi_Direction; break; case HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1: //MOTOR_DISPENSER_1 - Dispenser F2_SPI_Direction1.bits.F2_MOTO_DISPENSER_1 = Spi_Direction; break; case HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_2: //MOTOR_DISPENSER_2 - Dispenser F2_SPI_Direction1.bits.F2_MOTO_DISPENSER_2 = Spi_Direction; break; case HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_3: //MOTOR_DISPENSER_3 - Dispenser F2_SPI_Direction1.bits.F2_MOTO_DISPENSER_3 = Spi_Direction; break; case HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_4: //MOTOR_DISPENSER_4 - Dispenser F2_SPI_Direction1.bits.F2_MOTO_DISPENSER_4 = Spi_Direction; break; case HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_5: //MOTOR_DISPENSER_5 - Dispenser F2_SPI_Direction1.bits.F2_MOTO_DISPENSER_5 = Spi_Direction; break; case HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_6: //MOTOR_DISPENSER_6 - Dispenser F2_SPI_Direction1.bits.F2_MOTO_DISPENSER_6 = Spi_Direction; break; case HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_7: //MOTOR_DISPENSER_7 - Dispenser F2_SPI_Direction1.bits.F2_MOTO_DISPENSER_7 = Spi_Direction; break; case HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_8: //MOTOR_DISPENSER_8 - Dispenser F2_SPI_Direction1.bits.F2_MOTO_DISPENSER_8 = Spi_Direction; case HARDWARE_MOTOR_TYPE__MOTO_SCREW: //MOTOR_SCREW - Winder screw F1_SPI_Direction2.bits.F1_MOTO_SCREW = Spi_Direction; break; case HARDWARE_MOTOR_TYPE__MOTO_WINDER: //MOTOR_WINDER - Winder F1_SPI_Direction2.bits.F1_MOTO_WINDER = Spi_Direction; break; case HARDWARE_MOTOR_TYPE__MOTO_LDANCER1: //MOTOR_LDANCER1 - Winder Dancer F1_SPI_Direction1.bits.F1_MOTO_LDANCER1 = Spi_Direction; break; case HARDWARE_MOTOR_TYPE__MOTO_LDANCER2: //MOTOR_LDANCER2 - Pooler Dancer F1_SPI_Direction1.bits.F1_MOTO_LDANCER2 = Spi_Direction; break; case HARDWARE_MOTOR_TYPE__MOTO_LDRIVING: //MOTOR_LDRIVING - Pooler: F1_SPI_Direction1.bits.F1_MOTO_LDRIVING = Spi_Direction; break; case HARDWARE_MOTOR_TYPE__MOTO_LLOADING: //MOTOR_LLOADING - Pooler loading F1_SPI_Direction1.bits.F1_MOTO_LLOADING = Spi_Direction; break; case HARDWARE_MOTOR_TYPE__MOTO_LPIVOT1: //MOTOR_LPIVOT1 - Pooler loading pivot arm F1_SPI_Direction1.bits.F1_MOTO_LPIVOT1 = Spi_Direction; break; case HARDWARE_MOTOR_TYPE__MOTO_RDANCER: //MOTOR_RDANCER - Feeder dancer adjustment: F1_SPI_Direction2.bits.F1_MOTO_RDANCER = Spi_Direction; break; case HARDWARE_MOTOR_TYPE__MOTO_RDRIVING: //MOTOR_RDRIVING - Feeder: F1_SPI_Direction2.bits.F1_MOTO_RDRIVING = Spi_Direction; break; case HARDWARE_MOTOR_TYPE__MOTO_RLOADARM: //MOTOR_RLOADARM - Right loading arm: F1_SPI_Direction2.bits.F1_MOTO_RLOADARM = Spi_Direction; break; case HARDWARE_MOTOR_TYPE__MOTO_RLOADING: //MOTOR_RLOADING - Right Loading F1_SPI_Direction2.bits.F1_MOTO_RLOADING = Spi_Direction; break; case HARDWARE_MOTOR_TYPE__MOTO_SPARE1_1: //MOTOR_RLOADING - Right Loading F3_SPI_Direction1.bits.F3_MOTO_SPARE1_1 = Spi_Direction; break; case HARDWARE_MOTOR_TYPE__MOTO_SPARE1_2: //MOTOR_RLOADING - Right Loading F3_SPI_Direction1.bits.F3_MOTO_SPARE1_2 = Spi_Direction; break; case HARDWARE_MOTOR_TYPE__MOTO_SPARE2_1: //MOTOR_RLOADING - Right Loading F3_SPI_Direction1.bits.F3_MOTO_SPARE2_1 = Spi_Direction; break; case HARDWARE_MOTOR_TYPE__MOTO_SPARE2_2: //MOTOR_RLOADING - Right Loading F3_SPI_Direction1.bits.F3_MOTO_SPARE2_2 = Spi_Direction; break; case HARDWARE_MOTOR_TYPE__MOTO_SPARE3_1: //MOTOR_RLOADING - Right Loading F3_SPI_Direction1.bits.F3_MOTO_SPARE3_1 = Spi_Direction; break; default: break; } //write direction registers F1_Moto_Driver_SPI_DIRECTION1 = F1_SPI_Direction1.ushort; F1_Moto_Driver_SPI_DIRECTION2 = F1_SPI_Direction2.ushort; F2_Moto_Driver_SPI_DIRECTION1 = F2_SPI_Direction1.ushort; F3_Moto_Driver_SPI_DIRECTION1 = F3_SPI_Direction1.ushort; } extern bool MotorConfigState[NUM_OF_MOTORS]; uint32_t Power_Step_01_Mode(TimerMotors_t _motorId, MOTDRIVER_MODE New_Mode) { /* POWERSTEP01 1. need support in machine studio + create new parameters file the (for current and voltage modes) 2. The default mode is voltage for dispensers and + DH_LID + DRYER_LID and Current for all the rest. 3. I order to change mode need to: a. Stop the motor MotorDriverRequest[_motorId].Stop = Hard_Hiz; FPGA_SetMotStop(motorId); b. call the function Combined_Motor_Driver_Mode(motorId, New_Mode)/ Combined_dispenser_Driver_Mode(Disp_ID, New_Mode)// Current or Voltage c. change the step to MOTOR_CONFIG_INIT and call the function MotorConfig( motorId, &MotorsCfg[motorId]) d. read the STEP_MODE register and verify that the CM_VM bit 0 for Voltage mode and 1 for current mode */ uint32_t status = OK; /* uint8_t MicroStepsReg = 0, CM_VM = 0;; if((New_Mode != MotorDriverResponse[_motorId].DriverMode) && (MotorDriverResponse[_motorId].DriverType == CombinrdMotDriver)) { MotorConfigState[_motorId] = false; //Stop the motor MotorDriverRequest[_motorId].Stop = Hard_Hiz; FPGA_SetMotStop(_motorId); //Change the flag mode Combined_Motor_Driver_Mode(_motorId, New_Mode); //Set the new configuration according to the flag mode ConfigStages[_motorId] = MOTOR_CONFIG_INIT; MotorConfig( _motorId, &MotorsCfg[_motorId]); delayms(50); //read the CM_VM bit MotorGetMicroStepsFromFPGA(_motorId); delayms(5); MicroStepsReg = MotorGetMicroStepsFromFPGA_Res(_motorId); CM_VM = (MicroStepsReg & 0x08); if(((New_Mode == Voltage) && (CM_VM != Voltage)) || ((New_Mode == Current) && (CM_VM != Current))) { status = ERROR; } ReportWithPackageFilter(ControlFilter,"Change PowerStep01 mode", __FILE__,__LINE__,_motorId,RpMessage,New_Mode,0); } */ return status; } uint32_t Power_Step_01_Dispenser_Mode(uint8_t Dispenser_ID, MOTDRIVER_MODE New_Mode) { uint32_t status = OK; //status = Power_Step_01_Mode(Dispenser_ID + HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1, New_Mode); return status; } void Init_Motors_Drivers_After_FPGA_WD() { uint8_t i; for(i=0;i