From d60f78be71fd705cb6bbea8ecb5b5c49858c973e Mon Sep 17 00:00:00 2001 From: Shlomo Hecht Date: Thu, 10 May 2018 13:57:03 +0300 Subject: change motors config, improve initialization and control, first control run --- Software/Embedded_SW/Embedded/Modules/Thread/Thread.h | 2 ++ Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h | 9 +++++++++ Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c | 12 ++---------- Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c | 1 - 4 files changed, 13 insertions(+), 11 deletions(-) (limited to 'Software/Embedded_SW/Embedded/Modules/Thread') diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h index 7690b131f..0bda4964b 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h @@ -15,6 +15,7 @@ #include "drivers/Motors/Motor.h" #include "drivers/Danser_SSI/ssi_comm.h" +#include "thread_ex.h" typedef struct { @@ -37,6 +38,7 @@ typedef enum threadMotorsEnum SCREW_MOTOR, MAX_THREAD_MOTORS_NUM }threadMotorsEnum; + #define MAX_THREAD_FEED_MOTORS WINDER_MOTOR+1 #define MAX_SYSTEM_DANCERS HARDWARE_DANCER_TYPE__RightDancer+1 diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h index 5f49c7d10..3fb78d2f0 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h @@ -1,6 +1,15 @@ #ifndef MODULES_THREAD_THREAD_EX_H_ #define MODULES_THREAD_THREAD_EX_H_ + +typedef enum +{ + WINDER_DANCER, + POOLER_DANCER, + FEEDER_DANCER, + NUM_OF_DANCERS +} DANCER_ENUM; + uint32_t ThreadPrepareState(void *JobDetails); uint32_t ThreadPreSegmentState(void *JobDetails); uint32_t ThreadSegmentState(void *JobDetails, int SegmentId); diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c index 5a8c3df76..0e276fe92 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c @@ -51,7 +51,6 @@ uint32_t MotorsConfigMessage(HardwareMotor * request) { uint32_t status = PASSED; TimerMotors_t Motor_i; - MotorDriverConfigStruc MotorDriverConfig; Motor_i = request->hardwaremotortype; /*for (i=0;i Date: Sun, 13 May 2018 13:39:39 +0300 Subject: changes for FPGA setup and integration --- .../Embedded_SW/Embedded/Common/SW_Info/SW_Info.c | 3 +- .../Embedded/Drivers/FPGA/FPGA_SPI_Comm.c | 288 +++++++++++++++++---- .../Embedded_SW/Embedded/Drivers/Motors/Motor.c | 5 +- .../Embedded_SW/Embedded/Drivers/SPI/SPI_Comm.c | 15 +- .../Embedded/Modules/Control/MillisecTask.c | 6 +- .../Embedded/Modules/General/GeneralHardware.c | 2 + .../Embedded/Modules/Thread/Thread_print.c | 9 +- 7 files changed, 262 insertions(+), 66 deletions(-) (limited to 'Software/Embedded_SW/Embedded/Modules/Thread') diff --git a/Software/Embedded_SW/Embedded/Common/SW_Info/SW_Info.c b/Software/Embedded_SW/Embedded/Common/SW_Info/SW_Info.c index ab35dfbf1..c51a3e0cc 100644 --- a/Software/Embedded_SW/Embedded/Common/SW_Info/SW_Info.c +++ b/Software/Embedded_SW/Embedded/Common/SW_Info/SW_Info.c @@ -21,7 +21,8 @@ typedef struct static TangoVersion_t _gTangoVersion = {001,000,000,001}; - +#define BUILD_DATE __DATE__ +char Dat[50] = BUILD_DATE; static char _gTangoName [MAX_STRING_LEN] = "Tango01 ";//d uint16_t GetTangoVersion(uint8_t* _buffer) diff --git a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c index 476289568..6d093a2a2 100644 --- a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c +++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c @@ -345,7 +345,7 @@ int MotorSendFPGARequest(TimerMotors_t MotorId,uint32_t Data, uint32_t Length) { INT2SHORT Int2Short; - if((*FpgaMotMap[MotorId].NBUSY == NOTBUSY) && (*FpgaMotMap[MotorId].SPI_Busy == NOTBUSY)) + //if((*FpgaMotMap[MotorId].NBUSY == NOTBUSY) && (*FpgaMotMap[MotorId].SPI_Busy == NOTBUSY)) { Int2Short.uint = Data; @@ -355,8 +355,8 @@ int MotorSendFPGARequest(TimerMotors_t MotorId,uint32_t Data, uint32_t Length) return OK; } - else - return ERROR; + //else + // return ERROR; } @@ -383,40 +383,238 @@ void FPGA_SetMotorsInit() //F3_Prescaler1_reg3 = //3bit spi moto low duty cycle value for pmw //F3_Prescaler1_reg4 = //3bit spi moto high duty cycle value for pmw } +typedef enum +{ + MOTOR_CONFIG_INIT, + MOTOR_CONFIG_HARD_HIZ, + MOTOR_CONFIG_ACC, + MOTOR_CONFIG_DEC, + MOTOR_CONFIG_MAX_SPEED, + MOTOR_CONFIG_STEP_MODE, + MOTOR_CONFIG_CONFIG, + MOTOR_CONFIG_KVAL_HOLD, + MOTOR_CONFIG_KVAL_RUN, + MOTOR_CONFIG_KVAL_ACC, + MOTOR_CONFIG_KVAL_DEC, + MOTOR_CONFIG_ST_SLP, + MOTOR_CONFIG_INT_SPD, + MOTOR_CONFIG_FN_SLP_ACC, + MOTOR_CONFIG_FN_SLP_DEC, + MOTOR_CONFIG_OCD_TH, + MOTOR_CONFIG_STALL_TH, + MOTOR_CONFIG_K_THERM, + MOTOR_CONFIG_END +}MOTOR_CONFIG_STAGES_ENUM; +MOTOR_CONFIG_STAGES_ENUM ConfigStages[NUM_OF_MOTORS] = {MOTOR_CONFIG_INIT}; +uint32_t FPGA_MotorConfig_callback(TimerMotors_t _motorId, uint32_t ReadValue) +{ + uint32_t temp = 0; + if (_motorId > NUM_OF_MOTORS) return ERROR; + switch (ConfigStages[_motorId]) + { + case MOTOR_CONFIG_HARD_HIZ: + ConfigStages[_motorId]++; + temp = x_SET_PARAM | x_ACC; + temp = temp << 24; + temp |= ((int)(MotorDriverCfg[_motorId].maxchangeslope)<<8); + MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback); + break; + case MOTOR_CONFIG_ACC: + ConfigStages[_motorId]++; + temp = x_SET_PARAM | x_DEC; + temp = temp << 24; + temp |= ((int)(MotorDriverCfg[_motorId].maxchangeslope)<<8); + MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback); + break; + case MOTOR_CONFIG_DEC: + ConfigStages[_motorId]++; + temp = x_SET_PARAM | x_MAX_SPEED; + temp = temp << 24; + temp |= MotorDriverCfg[_motorId].maxfrequency<<8; + MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback); + break; + case MOTOR_CONFIG_MAX_SPEED: + ConfigStages[_motorId]++; + if(MotorDriverCfg[_motorId].has_microstep) + { + temp = x_SET_PARAM | x_STEP_MODE; + temp = temp << 24; + temp |= (!x_SYNC_EN | MotorDriverCfg[_motorId].microstep | x_SYNC_SEL_1)<<16; + MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback); + break; + } + case MOTOR_CONFIG_STEP_MODE: + ConfigStages[_motorId]++; + if(MotorDriverCfg[_motorId].has_configword) + { + temp = x_SET_PARAM | x_CONFIG; + temp = temp << 24; + temp |= MotorDriverCfg[_motorId].configword<<8; + MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback); + break; + } + case MOTOR_CONFIG_CONFIG: + ConfigStages[_motorId]++; + if(MotorDriverCfg[_motorId].has_kvalhold) + { + temp = x_SET_PARAM | x_KVAL_HOLD; + temp = temp << 24; + temp |= MotorDriverCfg[_motorId].kvalhold<<16; + if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR; + break; + } + case MOTOR_CONFIG_KVAL_HOLD: + ConfigStages[_motorId]++; + if(MotorDriverCfg[_motorId].has_kvalrun) + { + temp = x_SET_PARAM | x_KVAL_RUN; + temp = temp << 24; + temp |= MotorDriverCfg[_motorId].kvalrun<<16; + if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR; + break; + } + case MOTOR_CONFIG_KVAL_RUN: + ConfigStages[_motorId]++; + if(MotorDriverCfg[_motorId].has_kvalacc) + { + temp = x_SET_PARAM | x_KVAL_ACC; + temp = temp << 24; + temp |= MotorDriverCfg[_motorId].kvalacc<<16; + if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR; + break; + } + case MOTOR_CONFIG_KVAL_ACC: + ConfigStages[_motorId]++; + if(MotorDriverCfg[_motorId].has_kvaldec) + { + temp = x_SET_PARAM | x_KVAL_DEC; + temp = temp << 24; + temp |= MotorDriverCfg[_motorId].kvaldec<<16; + if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR; + break; + } + case MOTOR_CONFIG_KVAL_DEC: + ConfigStages[_motorId]++; + if(MotorDriverCfg[_motorId].has_stslp) + { + temp = x_SET_PARAM | x_ST_SLP; + temp = temp << 24; + temp |= MotorDriverCfg[_motorId].stslp<<16; + if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR; + break; + } + case MOTOR_CONFIG_ST_SLP: + ConfigStages[_motorId]++; + if(MotorDriverCfg[_motorId].has_intspd) + { + temp = x_SET_PARAM | x_INT_SPD; + temp = temp << 24; + temp |= MotorDriverCfg[_motorId].intspd<<8; + if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR; + break; + } + case MOTOR_CONFIG_INT_SPD: + ConfigStages[_motorId]++; + if(MotorDriverCfg[_motorId].has_fnslpacc) + { + temp = x_SET_PARAM | x_FN_SLP_ACC; + temp = temp << 24; + temp |= MotorDriverCfg[_motorId].fnslpacc<<16; + if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR; + break; + } + case MOTOR_CONFIG_FN_SLP_ACC: + ConfigStages[_motorId]++; + if(MotorDriverCfg[_motorId].has_fnslpdec) + { + temp = x_SET_PARAM | x_FN_SLP_DEC; + temp = temp << 8; + temp |= MotorDriverCfg[_motorId].fnslpdec; + if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR; + break; + } + case MOTOR_CONFIG_FN_SLP_DEC: + ConfigStages[_motorId]++; + if(MotorDriverCfg[_motorId].has_overcurrentthreshold) + { + temp = x_SET_PARAM | x_OCD_TH; + temp = temp << 24; + temp |= MotorDriverCfg[_motorId].overcurrentthreshold<<16; + if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR; + break; + } + case MOTOR_CONFIG_OCD_TH: + ConfigStages[_motorId]++; + if(MotorDriverCfg[_motorId].has_stallthreshold) + { + temp = x_SET_PARAM | x_STALL_TH; + temp = temp << 24; + temp |= MotorDriverCfg[_motorId].stallthreshold<<16; + if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR; + break; + } + case MOTOR_CONFIG_STALL_TH: + ConfigStages[_motorId]++; + if(MotorDriverCfg[_motorId].has_thermalcompensationfactor) + { + temp = x_SET_PARAM | x_K_THERM; + temp = temp << 24; + temp |= MotorDriverCfg[_motorId].thermalcompensationfactor<<16; + if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR; + break; + } + case MOTOR_CONFIG_K_THERM: + ConfigStages[_motorId]++; + break; + default: + return ERROR; + } + if (ConfigStages[_motorId] == MOTOR_CONFIG_END) + { + //notify original caller that motor is ready + } +return OK; +} uint32_t FPGA_MotorConfig(TimerMotors_t _motorId, MotorDriverConfigStruc *MotorConfig) +{ + ConfigStages[_motorId] = MOTOR_CONFIG_HARD_HIZ; + MillisecWriteToMotor(_motorId, x_HARD_HIZ, 1, FPGA_MotorConfig_callback); + return OK; +} +/*uint32_t FPGA_MotorConfig(TimerMotors_t _motorId, MotorDriverConfigStruc *MotorConfig) { uint32_t temp; - if( MillisecWriteToMotor(_motorId, x_HARD_HIZ, 1, NULL) < 0 ) return ERROR; + if( MillisecWriteToMotor(_motorId, x_HARD_HIZ, 1, FPGA_MotorConfig_callback) < 0 ) return ERROR; temp = x_SET_PARAM | x_ACC; - temp = temp << 24; - temp |= ((int)(MotorDriverCfg[_motorId].maxchangeslope)<<8); - if( MillisecWriteToMotor(_motorId, temp, 4, NULL) < 0 ) return ERROR; + temp = temp << 12; + temp |= (int)(MotorDriverCfg[_motorId].maxchangeslope); + if( MillisecWriteToMotor(_motorId, temp, 3, FPGA_MotorConfig_callback) < 0 ) return ERROR; temp = x_SET_PARAM | x_DEC; - temp = temp << 24; - temp |= ((int)(MotorDriverCfg[_motorId].maxchangeslope)<<8); - if( MillisecWriteToMotor(_motorId, temp, 4, NULL) < 0 ) return ERROR; + temp = temp << 12; + temp |= (int)(MotorDriverCfg[_motorId].maxchangeslope); + if( MillisecWriteToMotor(_motorId, temp, 3, FPGA_MotorConfig_callback) < 0 ) return ERROR; temp = x_SET_PARAM | x_MAX_SPEED; - temp = temp << 24; - temp |= MotorDriverCfg[_motorId].maxfrequency<<8; - if( MillisecWriteToMotor(_motorId, temp, 4, NULL) < 0 ) return ERROR; + temp = temp << 10; + temp |= MotorDriverCfg[_motorId].maxfrequency; + if( MillisecWriteToMotor(_motorId, temp, 3, FPGA_MotorConfig_callback) < 0 ) return ERROR; if(MotorDriverCfg[_motorId].has_microstep) { temp = x_SET_PARAM | x_STEP_MODE; - temp = temp << 24; - temp |= (!x_SYNC_EN | MotorDriverCfg[_motorId].microstep | x_SYNC_SEL_1)<<16; - if( MillisecWriteToMotor(_motorId, temp, 4, NULL) < 0 ) return ERROR; + temp = temp << 8; + temp |= !x_SYNC_EN | MotorDriverCfg[_motorId].microstep | x_SYNC_SEL_1; + if( MillisecWriteToMotor(_motorId, temp, 2, FPGA_MotorConfig_callback) < 0 ) return ERROR; } if(MotorDriverCfg[_motorId].has_configword) { temp = x_SET_PARAM | x_CONFIG; temp = temp << 24; - temp |= MotorDriverCfg[_motorId].configword<<8; - if( MillisecWriteToMotor(_motorId, temp, 4, NULL) < 0 ) return ERROR; + temp |= MotorDriverCfg[_motorId].configword; + if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR; } /////////////////////////////////////////////////////////////// @@ -424,29 +622,29 @@ uint32_t FPGA_MotorConfig(TimerMotors_t _motorId, MotorDriverConfigStruc *MotorC { temp = x_SET_PARAM | x_KVAL_HOLD; temp = temp << 24; - temp |= MotorDriverCfg[_motorId].kvalhold<<16; - if( MillisecWriteToMotor(_motorId, temp, 4, NULL) < 0 ) return ERROR; + temp |= MotorDriverCfg[_motorId].kvalhold; + if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR; } if(MotorDriverCfg[_motorId].has_kvalrun) { temp = x_SET_PARAM | x_KVAL_RUN; temp = temp << 24; - temp |= MotorDriverCfg[_motorId].kvalrun<<16; - if( MillisecWriteToMotor(_motorId, temp, 4, NULL) < 0 ) return ERROR; + temp |= MotorDriverCfg[_motorId].kvalrun; + if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR; } if(MotorDriverCfg[_motorId].has_kvalacc) { temp = x_SET_PARAM | x_KVAL_ACC; temp = temp << 24; - temp |= MotorDriverCfg[_motorId].kvalacc<<16; - if( MillisecWriteToMotor(_motorId, temp, 4, NULL) < 0 ) return ERROR; + temp |= MotorDriverCfg[_motorId].kvalacc; + if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR; } if(MotorDriverCfg[_motorId].has_kvaldec) { temp = x_SET_PARAM | x_KVAL_DEC; temp = temp << 24; - temp |= MotorDriverCfg[_motorId].kvaldec<<16; - if( MillisecWriteToMotor(_motorId, temp, 4, NULL) < 0 ) return ERROR; + temp |= MotorDriverCfg[_motorId].kvaldec; + if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR; } /////////////////////////////////////////////////////////////// @@ -454,52 +652,42 @@ uint32_t FPGA_MotorConfig(TimerMotors_t _motorId, MotorDriverConfigStruc *MotorC { temp = x_SET_PARAM | x_ST_SLP; temp = temp << 24; - temp |= MotorDriverCfg[_motorId].stslp<<16; - if( MillisecWriteToMotor(_motorId, temp, 4, NULL) < 0 ) return ERROR; + temp |= MotorDriverCfg[_motorId].stslp; + if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR; } if(MotorDriverCfg[_motorId].has_intspd) { temp = x_SET_PARAM | x_INT_SPD; temp = temp << 24; - temp |= MotorDriverCfg[_motorId].intspd<<8; - if( MillisecWriteToMotor(_motorId, temp, 4, NULL) < 0 ) return ERROR; + temp |= MotorDriverCfg[_motorId].intspd; + if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR; } if(MotorDriverCfg[_motorId].has_fnslpacc) { temp = x_SET_PARAM | x_FN_SLP_ACC; temp = temp << 24; - temp |= MotorDriverCfg[_motorId].fnslpacc<<16; - if( MillisecWriteToMotor(_motorId, temp, 4, NULL) < 0 ) return ERROR; + temp |= MotorDriverCfg[_motorId].fnslpacc; + if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR; } if(MotorDriverCfg[_motorId].has_fnslpdec) { temp = x_SET_PARAM | x_FN_SLP_DEC; temp = temp << 24; - temp |= MotorDriverCfg[_motorId].fnslpdec<<16; - if( MillisecWriteToMotor(_motorId, temp, 4, NULL) < 0 ) return ERROR; + temp |= MotorDriverCfg[_motorId].fnslpdec; + if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR; } if(MotorDriverCfg[_motorId].has_overcurrentthreshold) { temp = x_SET_PARAM | x_OCD_TH; temp = temp << 24; - temp |= MotorDriverCfg[_motorId].overcurrentthreshold<<16; - if( MillisecWriteToMotor(_motorId, temp, 4, NULL) < 0 ) return ERROR; + temp |= MotorDriverCfg[_motorId].overcurrentthreshold; + if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR; } /////////////////////////////////////////////////////////////// -/* - protobuf_c_boolean has_overcurrentthreshold; - int32_t overcurrentthreshold; - protobuf_c_boolean has_stallthreshold; - int32_t stallthreshold; - protobuf_c_boolean has_thermalcompensationfactor; - int32_t thermalcompensationfactor; - protobuf_c_boolean has_lowspeedoptimization; - protobuf_c_boolean lowspeedoptimization; -*/ return OK; } - +*/ void FPGA_SetMotSpeed(TimerMotors_t _motorId)//Run { @@ -512,10 +700,12 @@ void FPGA_SetMotSpeed(TimerMotors_t _motorId)//Run MotorDriverRequest[_motorId].Speed = 0xFFFFF; temp |= MotorDriverRequest[_motorId].Speed; - Fpga_Spi[_motorId].TX_MOSI = temp;//<<8; + MillisecSetMotorSpeed(_motorId, temp, 4, NULL); + + //Fpga_Spi[_motorId].TX_MOSI = temp;//<<8; //Fpga_Spi[_motorId].AMT_OF_Words = 4; - FPGA_SPI_Transnit(_motorId); + //FPGA_SPI_Transnit(_motorId); } void FPGA_SetMotPosition(TimerMotors_t _motorId)//Mov diff --git a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c index 0148506ac..1112c858b 100644 --- a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c +++ b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c @@ -109,11 +109,12 @@ uint32_t MotorSetDirection(TimerMotors_t _motorId,bool _direction) uint32_t MotorSetSpeed(TimerMotors_t _motorId, uint32_t _freq, int MicroSteps)//TODO MicroSteps? { + int RunSpeed = SpdCalc(_freq); #ifdef EVALUATION_BOARD - Run_Value = SpdCalc(_freq); + Run_Value = RunSpeed; Mot_Run(); #else - MotorDriverRequest[_motorId].Speed = _freq; + MotorDriverRequest[_motorId].Speed = RunSpeed; FPGA_SetMotSpeed(_motorId); #endif diff --git a/Software/Embedded_SW/Embedded/Drivers/SPI/SPI_Comm.c b/Software/Embedded_SW/Embedded/Drivers/SPI/SPI_Comm.c index 111799bce..bb9ccb06d 100644 --- a/Software/Embedded_SW/Embedded/Drivers/SPI/SPI_Comm.c +++ b/Software/Embedded_SW/Embedded/Drivers/SPI/SPI_Comm.c @@ -594,7 +594,6 @@ void setup(MotorDriverConfigStruc *MotorConfig) } } - if(MotorConfig->maxchangeslope != 0) SetParam(x_DEC, MotorConfig->maxchangeslope);//roll-over after 0x7F - 7 bit ??? (should be 12 bit) AVI else @@ -633,18 +632,16 @@ void setup(MotorDriverConfigStruc *MotorConfig) //while(SSIBusy(SSI2_BASE)){}; timeout = SSI_SPI_TIMEOUT; - while(SSIBusy(SSI2_BASE)) - { - timeout = timeout--; - if(timeout == 0) + while(SSIBusy(SSI2_BASE)) { - return ; + timeout = timeout--; + if(timeout == 0) + { + return ; + } } - } - if(MotorConfig->has_overcurrentthreshold) { - while(SSIBusy(SSI2_BASE)){}; SetParam(x_OCD_TH, MotorConfig->overcurrentthreshold); } else diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c index 05d582eac..6bbf1efc3 100644 --- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c +++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c @@ -91,7 +91,7 @@ void MillisecInit(void) MillisecMsgQ = Mailbox_create(sizeof(MillisecMessageStruc), 20, NULL,&eb); for (i=0;ihardwareconfiguration; if (request->n_winders == 1) status += InternalWinderConfigMessage(request->winders); + status += MotorsInit(); + for (Motor_i = 0; Motor_i < request->n_motors ; Motor_i++) status += MotorsConfigMessage(request->motors[Motor_i]); for (Dancer_i = 0; Dancer_i < request->n_dancers ; Dancer_i++) diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c index 434ac0f21..92939dfe4 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -281,7 +281,7 @@ bool InitialProcess = false; MotorSetDirection((TimerMotors_t)HW_Motor_Id,MotorsCfg[HW_Motor_Id].directionthreadwize); #ifdef DEBUG_TEST_FUNCTIONS - +/* if (Motor_i == FEEDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadLengthCBFunction, eOneMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); if (Motor_i == POOLER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled @@ -315,7 +315,7 @@ uint32_t ThreadPreSegmentState(void *JobDetails) int process_speed = JobTicket->processparameters->dyeingspeed; - process_speed = 10; //debug + process_speed = 50; //debug for (Motor_i = 0;Motor_i <= WINDER_MOTOR;Motor_i++) { @@ -328,6 +328,11 @@ uint32_t ThreadPreSegmentState(void *JobDetails) //ControlStart(); // set the new speed in the dryer motor to the speed of the new segment MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING, OriginalMotorSpd_2PPS[DRYER_MOTOR], MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING].microstep); + //only for testing - when control works, these motors will take their speed from the dryer + MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_LDRIVING, OriginalMotorSpd_2PPS[POOLER_MOTOR], MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDRIVING].microstep); + //only for testing - when control works, these motors will take their speed from the dryer + MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_RDRIVING, OriginalMotorSpd_2PPS[FEEDER_MOTOR], MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].microstep); + MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 1, 1); MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 1,1); -- cgit v1.3.1