diff options
Diffstat (limited to 'Software/Embedded_SW/Embedded/Modules')
11 files changed, 215 insertions, 52 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c index 327b5d19b..55148e655 100644 --- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c +++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c @@ -496,9 +496,17 @@ uint32_t MillisecLoop(uint32_t tick) #ifdef test_RTFU_dancer test_dancer_responce_RTFU(); #else +#ifdef FOUR_WINDERS + Dancer_Data[HARDWARE_DANCER_0] = Read_Dancer_Position(HARDWARE_DANCER_0); + Dancer_Data[HARDWARE_DANCER_1] = Read_Dancer_Position(HARDWARE_DANCER_1); + Dancer_Data[HARDWARE_DANCER_2] = Read_Dancer_Position(HARDWARE_DANCER_2); + Dancer_Data[HARDWARE_DANCER_3] = Read_Dancer_Position(HARDWARE_DANCER_3); + Dancer_Data[HARDWARE_DANCER_4] = Read_Dancer_Position(HARDWARE_DANCER_4); +#else Dancer_Data[FEEDER_DANCER] = Read_Dancer_Position(FEEDER_DANCER); Dancer_Data[POOLER_DANCER] = Read_Dancer_Position(POOLER_DANCER); Dancer_Data[WINDER_DANCER] = Read_Dancer_Position(WINDER_DANCER); +#endif #endif #endif @@ -732,7 +740,6 @@ uint32_t MillisecLowLoop(uint32_t tick) //static int Counter = 0; MachineUpdateResponseFunc(); //KeepAliveOneSecondCall(); - //TemperatureListString(Lenstr); //ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,__LINE__,(int)Counter++,RpWarning,(int) msec_millisecondCounter,0); for (Sensor_i = StartPT100;Sensor_i < MAX_HEAD_CARD_TEMP_SENS_ID;Sensor_i++) diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c index a76142b00..05987ee7f 100644 --- a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c +++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c @@ -17,6 +17,7 @@ #include "drivers/Motors/Motor.h" #include "drivers/Heater/TemperatureSensor.h" #include "drivers/SSI_Comm/Dancer/Dancer.h" +#include <Drivers/SSI_Comm/SSI_Comm.h> #include "drivers/FPGA/FPGA_SPI_Comm.h" #include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h" #include "drivers/FPGA/FPGA.h" @@ -407,6 +408,9 @@ void DiagnosticLoadDancerError(int DancerId, double value) // DancerErrorCounterIndex[DancerId] = 0; } +#ifdef FOUR_WINDERS + double dancer2,dancer3,dancer4; +#endif void CopyTemperaturesToMonitor(void) { DiagnosticsMonitor.mixertemperature = @@ -511,6 +515,18 @@ void CopyTemperaturesToMonitor(void) DiagnosticsMonitor.headblowervoltage2 = &headblowervoltage2; DiagnosticsMonitor.n_headblowervoltage1 = 1; DiagnosticsMonitor.n_headblowervoltage2 = 1; +#ifdef FOUR_WINDERS +// double dancer2,dancer3,dancer4; + dancer2 = Control_Read_Dancer_Position(HARDWARE_DANCER_2,0); + dancer3 = Control_Read_Dancer_Position(HARDWARE_DANCER_3,0); + dancer4 = Control_Read_Dancer_Position(HARDWARE_DANCER_4,0); + DiagnosticsMonitor.n_headblower1airflow = 1; + DiagnosticsMonitor.n_headblower2airflow = 1; + DiagnosticsMonitor.headblower1airflow = &dancer4; + DiagnosticsMonitor.headblower2airflow = &dancer3; + DiagnosticsMonitor.headblowervoltage1 = &dancer2; + DiagnosticsMonitor.n_headblowervoltage1 = 1; +#endif DiagnosticsMonitor.dryerzone1temperature = HeaterTemperature[HEATER_TYPE__DryerAirHeater]; DiagnosticsMonitor.dryerzone2temperature = @@ -975,11 +991,19 @@ void Diagnostic100msecCollection(void) { //if (JobIsActive()) { +#ifdef FOUR_WINDERS + DiagnosticLoadMotor(FEEDER_MOTOR, ThreadGetMotorSpeed (WINDER_2_MOTOR)); + DiagnosticLoadMotor(DRYER_MOTOR, ThreadGetMotorSpeed (WINDER_3_MOTOR)); + DiagnosticLoadMotor(POOLER_MOTOR, ThreadGetMotorSpeed (WINDER_4_MOTOR)); + DiagnosticLoadMotor(WINDER_MOTOR, ThreadGetMotorSpeed (WINDER_MOTOR)); + DiagnosticLoadMotor(SCREW_MOTOR, ThreadGetMotorSpeed (SCREW_MOTOR)); +#else DiagnosticLoadMotor(FEEDER_MOTOR, ThreadGetMotorSpeed (FEEDER_MOTOR)); DiagnosticLoadMotor(DRYER_MOTOR, ThreadGetMotorSpeed (DRYER_MOTOR)); DiagnosticLoadMotor(POOLER_MOTOR, ThreadGetMotorSpeed (POOLER_MOTOR)); DiagnosticLoadMotor(WINDER_MOTOR, ThreadGetMotorSpeed (WINDER_MOTOR)); DiagnosticLoadMotor(SCREW_MOTOR, ThreadGetMotorSpeed (SCREW_MOTOR)); +#endif } for (i=0;i<MAX_SYSTEM_DISPENSERS;i++) { diff --git a/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c b/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c index ac2009bcd..019be5c31 100644 --- a/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c +++ b/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c @@ -43,6 +43,7 @@ #include "Drivers/I2C_Communication/I2C.h" #include "drivers/Flash_ram/MCU_E2Prom.h" #include <Drivers/I2C_Communication/WHS_Card/D_Max5805_ADC_Blower/WHS_Blower.h> +#include "drivers/FPGA/Full_Vme/FPGA_Programming_Up.h" #include "StateMachines/Printing/PrintingSTM.h" #include "StateMachines/Initialization/InitSequence.h" @@ -431,6 +432,11 @@ uint32_t HWConfiguration(UploadHardwareConfigurationRequest* UploadRequest) LOG_ERROR(1,"JobIsActive"); return ERROR; } + if (FPGABurningActive == true) + { + LOG_ERROR(1,"FPGABurningActive"); + return ERROR; + } if (watchdogCriticalAlarm == true) { LOG_ERROR(1,"Critical Alarm cleared - Watchdog de-activated"); diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c index 9c82cb1bf..c0ccefde1 100644 --- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c +++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c @@ -1118,8 +1118,8 @@ uint32_t IDS_Cleaning_Stop_Cleaning_Solution (callback_fptr callback); if (CurrentDispenserSpeed[CLEANER_DISPENSER]>0) IDS_Cleaning_Stop_Cleaning_Solution (NULL); setRapidPressureRead(false); - if (JobEndSequence == true) - DistanceToSpoolReady(Module_IDS,ModuleDone); + //if (JobEndSequence == true) + // DistanceToSpoolReady(Module_IDS,ModuleDone); } if (EnableCleaning == true) { diff --git a/Software/Embedded_SW/Embedded/Modules/IFS/ifs.h b/Software/Embedded_SW/Embedded/Modules/IFS/ifs.h index 1edc03264..0f95fb2e2 100644 --- a/Software/Embedded_SW/Embedded/Modules/IFS/ifs.h +++ b/Software/Embedded_SW/Embedded/Modules/IFS/ifs.h @@ -27,5 +27,6 @@ void ResponseDemo(int MidtankId); bool IFS_MidTankFilling(void); MidTank_t IFS_MidTankIsActive(void); bool IFS_CartridgeLowerPresent(); +bool CartridgeValidationResponseFunc(MessageContainer* requestContainer); #endif /* MODULES_IFS_IFS_H_ */ diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h index 1f2da8dc0..13032f49d 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h @@ -54,7 +54,7 @@ typedef struct #define MAX_THREAD_FEED_MOTORS (WINDER_MOTOR+1) -#define MAX_SYSTEM_DANCERS (HARDWARE_DANCER_TYPE__RightDancer+1) +#define MAX_SYSTEM_DANCERS NUM_OF_ROTENC extern TimerMotors_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM]; diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c index 1ce27115d..852389001 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c @@ -347,10 +347,14 @@ int flipflop = 0; uint32_t motspeed; float speedf; int WinderCalculation = 0; +int MotorOrder[4] = {HARDWARE_MOTOR_TYPE__MOTO_SCREW,SCREW_2_Motor,SCREW_3_Motor,SCREW_4_Motor}; + + uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag) { //uint32_t Steps; double temp,tempScrewSpeed; + int i,tempmot; //double screw_horizontal_speed = 0; //double RotationsPerSecond; if (WinderMotorSpeedCounter++>=MAX_WINDER_SPEED_CALCULATION) @@ -369,6 +373,14 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag) ScrewCurrentDirection = 1-ScrewCurrentDirection; CalculationDirectionChangeCounter++; +#ifdef FOUR_WINDERS + tempmot = MotorOrder[0]; + MotorOrder[0] = MotorOrder[1]; + MotorOrder[1] = MotorOrder[2]; + MotorOrder[2] = MotorOrder[3]; + MotorOrder[3] = tempmot; + //Report("order change" ,__FILE__,MotorOrder[0],MotorOrder[1],RpWarning,MotorOrder[2], 0); +#endif if (SampleWinding) return OK; //double calcsteps = (ScrewRunningTime/SYS_CLK_FREQ)*ScrewSpeed; @@ -707,11 +719,13 @@ void ScrewTimerInterrupt(int ARG0) MotorSetDirection (SCREW_3_Motor, ScrewCurrentDirection); MotorSetDirection (SCREW_4_Motor, ScrewCurrentDirection); #endif - MotorSetSpeedDirect(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed); #ifdef FOUR_WINDERS - MotorSetSpeedDirect (SCREW_2_Motor, ScrewSpeed); - MotorSetSpeedDirect (SCREW_3_Motor, ScrewSpeed); - MotorSetSpeedDirect (SCREW_4_Motor, ScrewSpeed); + MotorSetSpeedDirect(MotorOrder[0],ScrewSpeed); + MotorSetSpeedDirect (MotorOrder[1], ScrewSpeed); + MotorSetSpeedDirect (MotorOrder[2], ScrewSpeed); + MotorSetSpeedDirect (MotorOrder[3], ScrewSpeed); +#else + MotorSetSpeedDirect(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed); #endif #ifdef READ_SCREW_ENCODER Read_Screw_Encoder(); diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h index 4339c5312..c3f181b52 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h @@ -9,7 +9,7 @@ #define WINDER_DANCER HARDWARE_DANCER_TYPE__LeftDancer #define POOLER_DANCER HARDWARE_DANCER_TYPE__MiddleDancer #define FEEDER_DANCER HARDWARE_DANCER_TYPE__RightDancer -#define NUM_OF_DANCERS (HARDWARE_DANCER_TYPE__RightDancer+1) +#define NUM_OF_DANCERS NUM_OF_ROTENC //} DANCER_ENUM; typedef enum threadMotorsEnum { @@ -17,6 +17,11 @@ typedef enum threadMotorsEnum DRYER_MOTOR, POOLER_MOTOR, WINDER_MOTOR, +#ifdef FOUR_WINDERS + WINDER_2_MOTOR, + WINDER_3_MOTOR, + WINDER_4_MOTOR, +#endif SCREW_MOTOR, MAX_THREAD_MOTORS_NUM }threadMotorsEnum; diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c index bbb3e4040..1920db03a 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c @@ -151,6 +151,39 @@ uint32_t MotorPidRequestMessage(HardwarePidControl* request) temp = temp*3/2; } DancerStopActivityLimit[Motor_i] = temp/(2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength); +#ifdef FOUR_WINDERS + if (Motor_i == WINDER_MOTOR) + { + int j,winderMotorId; + for (j=0;j<3;j++) + { + winderMotorId = j+WINDER_2_MOTOR; + memcpy (&MotorsControl[winderMotorId],request,sizeof(HardwarePidControl)); + if (MotorsControl[winderMotorId].pvinputfilterfactormode > MAX_CONTROL_SAMPLES) + MotorsControl[winderMotorId].pvinputfilterfactormode = MAX_CONTROL_SAMPLES; + #ifdef TEST_LONGER_PID_THREAD + MotorsControl[Motor_i].pvinputfilterfactormode = 10; //test longer control + #endif + for (i = 0;i < (int)MotorsControl[winderMotorId].pvinputfilterfactormode; i++) + { + MotorSamples[winderMotorId][i] = 0; //reset the samples value for control beginning + MotorSpeedSamples[winderMotorId][i] = 0; + } + NormalizedErrorCoEfficient[winderMotorId] = (2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength); + temp = 1<<(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].resolutionbits); + temp=(NORMAL_COEF_DIVIDER*(temp-1)*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm); + NormalizedErrorCoEfficient[winderMotorId] = NormalizedErrorCoEfficient[winderMotorId] / temp; + // uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES]; + temp = 1<<(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].resolutionbits); + temp = (temp*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm);//*3/2); + if (Motor_i == WINDER_MOTOR) + { + temp = temp*3/2; + } + DancerStopActivityLimit[winderMotorId] = temp/(2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength); + } + } +#endif return OK; } char DancerConfigPath[50] = "0://SysInfo//DancCfg.cfg"; @@ -159,31 +192,37 @@ uint32_t StoreDancerConfigMessage() { uint32_t status = OK; // FRESULT Fresult = FR_OK; - HardwareConfiguration DancerConfig; + //HardwareConfiguration DancerConfig; HardwareDancer Dancers[MAX_SYSTEM_DANCERS]; // uint8_t* response_buffer; // size_t response_size = 0; int Dancer_i; - hardware_configuration__init(&DancerConfig); - DancerConfig.dancers = (HardwareDancer**)my_malloc(sizeof(HardwareDancer*)*MAX_SYSTEM_DANCERS); + //hardware_configuration__init(&DancerConfig); + //DancerConfig.dancers = (HardwareDancer**)my_malloc(sizeof(HardwareDancer*)*MAX_SYSTEM_DANCERS); for (Dancer_i = 0; Dancer_i < MAX_SYSTEM_DANCERS; Dancer_i++) { hardware_dancer__init(&Dancers[Dancer_i]); - DancerConfig.dancers[Dancer_i] = &Dancers[Dancer_i]; + //DancerConfig.dancers[Dancer_i] = &Dancers[Dancer_i]; Dancers[Dancer_i].has_zeropoint = true; Dancers[Dancer_i].hardwaredancertype = Dancer_i; Dancers[Dancer_i].has_hardwaredancertype = true; - DancerConfig.dancers[Dancer_i]->zeropoint=Control_Read_Dancer_Position(Dancer_i,0); - DancerConfig.n_dancers++; - DancersCfg[Dancer_i].zeropoint = DancerConfig.dancers[Dancer_i]->zeropoint; + Dancers[Dancer_i].zeropoint=Control_Read_Dancer_Position(Dancer_i,0); + //DancerConfig.n_dancers++; + DancersCfg[Dancer_i].zeropoint = Dancers[Dancer_i].zeropoint; + DancersCfg[Dancer_i].zeropoint = Control_Read_Dancer_Position(Dancer_i,0); } status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_0,DancersCfg[0].zeropoint); status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_1,DancersCfg[1].zeropoint); status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_2,DancersCfg[2].zeropoint); Report("~~~~~~Store eeprom 0",__FILE__,DancersCfg[0].zeropoint,(int)DancersCfg[1].zeropoint,RpWarning,(int)DancersCfg[2].zeropoint,0); -/* +#ifdef FOUR_WINDERS + status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_3,DancersCfg[HARDWARE_DANCER_3].zeropoint); + status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_4,DancersCfg[HARDWARE_DANCER_4].zeropoint); + Report("~~~~~~Store eeprom 1",__FILE__,__LINE__,(int)DancersCfg[HARDWARE_DANCER_3].zeropoint,RpWarning,(int)DancersCfg[HARDWARE_DANCER_4].zeropoint,0); +#endif + /* response_buffer = my_malloc(hardware_configuration__get_packed_size(&DancerConfig)); if (response_buffer) { @@ -208,11 +247,18 @@ uint32_t LoadDancerConfigMessage() MCU_E2PromRead(EEPROM_STORAGE_DANCER_0,&DancersCfg[0].zeropoint); MCU_E2PromRead(EEPROM_STORAGE_DANCER_1,&DancersCfg[1].zeropoint); MCU_E2PromRead(EEPROM_STORAGE_DANCER_2,&DancersCfg[2].zeropoint); - +#ifdef FOUR_WINDERS + MCU_E2PromRead(EEPROM_STORAGE_DANCER_3,&DancersCfg[3].zeropoint); + MCU_E2PromRead(EEPROM_STORAGE_DANCER_4,&DancersCfg[4].zeropoint); +#endif status |= Report("Dancer 0 Store data eeprom ",__FILE__,__LINE__,0,RpWarning,(int)DancersCfg[0].zeropoint,0); status |= Report("Dancer 1 Store data eeprom ",__FILE__,__LINE__,1,RpWarning,(int)DancersCfg[1].zeropoint,0); status |= Report("Dancer 2 Store data eeprom ",__FILE__,__LINE__,2,RpWarning,(int)DancersCfg[2].zeropoint,0); +#ifdef FOUR_WINDERS + status |= Report("Dancer 3 Store data eeprom ",__FILE__,__LINE__,3,RpWarning,(int)DancersCfg[3].zeropoint,0); + status |= Report("Dancer 4 Store data eeprom ",__FILE__,__LINE__,4,RpWarning,(int)DancersCfg[4].zeropoint,0); +#endif return status; diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c index 79832120b..7ec0ed5e5 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -46,13 +46,15 @@ double CurrentControlledSpeed[MAX_THREAD_MOTORS_NUM] = {0}; -TimerMotors_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM] = {HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,HARDWARE_MOTOR_TYPE__MOTO_LDRIVING,HARDWARE_MOTOR_TYPE__MOTO_WINDER,HARDWARE_MOTOR_TYPE__MOTO_SCREW}; #ifdef FOUR_WINDERS -Dancers_4_Winders ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM] = {FEEDER_DANCER,NUM_OF_DANCERS,POOLER_DANCER,WINDER_DANCER,NUM_OF_DANCERS}; +TimerMotors_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM] = {HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,HARDWARE_MOTOR_TYPE__MOTO_LDRIVING,HARDWARE_MOTOR_TYPE__MOTO_WINDER,Winder_2_Motor,Winder_3_Motor,Winder_4_Motor,HARDWARE_MOTOR_TYPE__MOTO_SCREW}; +Dancers_4_Winders ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM] = {NUM_OF_DANCERS,NUM_OF_DANCERS,HARDWARE_DANCER_1,HARDWARE_DANCER_0,HARDWARE_DANCER_4,HARDWARE_DANCER_3,HARDWARE_DANCER_2,NUM_OF_DANCERS}; +uint32_t ControlIdtoMotorId [MAX_THREAD_MOTORS_NUM] = {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF}; #else +TimerMotors_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM] = {HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,HARDWARE_MOTOR_TYPE__MOTO_LDRIVING,HARDWARE_MOTOR_TYPE__MOTO_WINDER,HARDWARE_MOTOR_TYPE__MOTO_SCREW}; HardwareDancerType ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM] = {FEEDER_DANCER,NUM_OF_DANCERS,POOLER_DANCER,WINDER_DANCER,NUM_OF_DANCERS}; -#endif uint32_t ControlIdtoMotorId [MAX_THREAD_MOTORS_NUM] = {0xFF,0xFF,0xFF,0xFF,0xFF}; +#endif uint32_t SpeedControlId=0xFF; uint32_t PoolerSpeedControlId=0xFF; @@ -432,7 +434,10 @@ uint32_t ThreadControlSpeedReadFunction(uint32_t IfIndex, uint32_t ReadValue) uint16_t BreakSensorCounter = 0; uint16_t BreakSensorLatchCounter = 0; char TMessage[150]; - +#ifdef FOUR_WINDERS +char ATMessage[MAX_THREAD_MOTORS_NUM][150]; +int c = 0; +#endif uint32_t checkBreakSensor(uint32_t index) { if (BreakSensorenabled == true) @@ -550,7 +555,6 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) //#define MAX_CONTROL_SAMPLES 6 //extern uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES]; //extern int MotorSamplePointer[MAX_THREAD_MOTORS_NUM]; - //read value is the dancer angle int i,index=MAX_THREAD_MOTORS_NUM; int len; @@ -621,6 +625,29 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) //pooler dancer is right sided: data is opposite TranslatedReadValue = (-1*TranslatedReadValue); } +#ifdef FOUR_WINDERS + if (index == WINDER_MOTOR) + { + c++; + } + if (index == WINDER_2_MOTOR) + { + c++; + } + if (index == WINDER_3_MOTOR) + { + c++; + } + if (index == WINDER_4_MOTOR) + { + c++; + } + if ((index == WINDER_2_MOTOR)||(index == WINDER_3_MOTOR)) + { + //pooler dancer is right sided: data is opposite + TranslatedReadValue = (-1*TranslatedReadValue); + } +#endif if (index == POOLER_MOTOR) { //pooler dancer is right sided: data is opposite @@ -650,6 +677,10 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) ThreadControlActive = false; //MotorGetStatusFromFPGA(ThreadMotorIdToMotorId[index]); JobEndReason = JOB_WINDER_DANCER_FAIL+DancerId; +#ifdef FOUR_WINDERS + if (DancerId>HARDWARE_DANCER_2) + JobEndReason = JOB_WINDER_DANCER_FAIL+HARDWARE_DANCER_0; +#endif SendJobProgress(0.0,0,false, TMessage); //EndState(CurrentJob,TMessage ); SendSegmentFail(); @@ -702,24 +733,26 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) }*`/ }*/ calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index]; - if (index != WINDER_MOTOR) + if (index < WINDER_MOTOR) calculated_speed = calculated_speed*InitialDryerSpeed/OriginalMotorSpd_2PPS[DRYER_MOTOR]; //calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*CurrentControlledSpeed[index]; - if (0)//(JobCounter % 1000 == 0) - //if (JobCounter < 100)//(FirstCalcInJob == true) + //if (0)//(JobCounter % 1000 == 0) +#ifdef FOUR_WINDERS + if (JobCounter % 500 < 4)//(FirstCalcInJob == true) { - if (index == FEEDER_MOTOR) + if (index >= WINDER_MOTOR) { // FirstCalcInJob = false; - len = usnprintf(TMessage, 150, "read %d avg %d error(6) %d integral(9) %d,delta(9) %d, calc(3) %d speed %d %d", + len = usnprintf(ATMessage[index], 150, "index %d read %d avg %d error(6) %d integral(9) %d,delta(9) %d, calc(3) %d speed %d %d",index, TranslatedReadValue,avreageSampleValue,(int)(MotorControlConfig[index].m_mesuredParam*1000000), (int)(MotorControlConfig[index].m_integral*1000000000),(int)((MotorControlConfig[index].m_mesuredParam*MotorControlConfig[index].m_params.dt)*1000000000), (int)(MotorControlConfig[index].m_calculatedError*1000),(int)calculated_speed, (int)(InitialDryerSpeed*100/OriginalMotorSpd_2PPS[DRYER_MOTOR])); - ReportWithPackageFilter(ThreadFilter,TMessage,__FILE__,MotorSamplePointer[index],JobCounter,RpError,ReadValue,0); + ReportWithPackageFilter(ThreadFilter,ATMessage[index],__FILE__,MotorSamplePointer[index],JobCounter,RpError,ReadValue,0); } + JobCounter++; } - +#endif #ifndef TEST_PID_THREAD if (fabs(calculated_speed-CurrentControlledSpeed[index])> MotorControlConfig[index].m_ingnoreValue) #else @@ -735,14 +768,6 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) }*/ CurrentControlledSpeed[index] = calculated_speed; MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed); -#ifdef FOUR_WINDERS - if (index == WINDER_MOTOR) - { - MotorSetSpeed(Winder_2_Motor, calculated_speed); - MotorSetSpeed(Winder_3_Motor, calculated_speed); - MotorSetSpeed(Winder_4_Motor, calculated_speed); - } -#endif } else { @@ -832,11 +857,32 @@ uint32_t HandleJobThreadControlParameters(ThreadParameters* ThreadParams) MotorControlConfig[POOLER_MOTOR].m_params.Kd = ThreadParams->pullerd; if(ThreadParams->winderp) + { MotorControlConfig[WINDER_MOTOR].m_params.Kp = ThreadParams->winderp; +#ifdef FOUR_WINDERS + MotorControlConfig[WINDER_2_MOTOR].m_params.Kp = ThreadParams->winderp; + MotorControlConfig[WINDER_3_MOTOR].m_params.Kp = ThreadParams->winderp; + MotorControlConfig[WINDER_4_MOTOR].m_params.Kp = ThreadParams->winderp; +#endif + } if(ThreadParams->winderi) + { MotorControlConfig[WINDER_MOTOR].m_params.Ki = ThreadParams->winderi; +#ifdef FOUR_WINDERS + MotorControlConfig[WINDER_2_MOTOR].m_params.Ki = ThreadParams->winderi; + MotorControlConfig[WINDER_3_MOTOR].m_params.Ki = ThreadParams->winderi; + MotorControlConfig[WINDER_4_MOTOR].m_params.Ki = ThreadParams->winderi; +#endif + } if(ThreadParams->winderd) + { MotorControlConfig[WINDER_MOTOR].m_params.Kd = ThreadParams->winderd; +#ifdef FOUR_WINDERS + MotorControlConfig[WINDER_2_MOTOR].m_params.Kd = ThreadParams->winderd; + MotorControlConfig[WINDER_3_MOTOR].m_params.Kd = ThreadParams->winderd; + MotorControlConfig[WINDER_4_MOTOR].m_params.Kd = ThreadParams->winderd; +#endif + } ReportWithPackageFilter(ThreadFilter,"Rockers activity",__FILE__,__LINE__,(int)Thread_Rockers_Bypass,RpWarning,(int)ThreadParams->bypassrockers,0); if(ThreadParams->bypassrockers) Thread_Rockers_Bypass = true; @@ -1032,13 +1078,14 @@ uint32_t ThreadPrepareState(void *JobDetails) IntersegmentLength = JobTicket->intersegmentlength; PrepareWaitCount = 0; +#ifndef FOUR_WINDERS status = ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__LeftDancer, windertension); ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension Winder",__FILE__,HARDWARE_DANCER_TYPE__LeftDancer,PrepareWaitCount,RpWarning,(int)windertension,0); status = ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__MiddleDancer, pullertension); ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension Puller",__FILE__,HARDWARE_DANCER_TYPE__MiddleDancer,PrepareWaitCount,RpWarning,(int)pullertension,0); status = ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__RightDancer, feedertension); ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension Feeder",__FILE__,HARDWARE_DANCER_TYPE__RightDancer,PrepareWaitCount,RpWarning,(int)feedertension,0); - +#endif FirstCalcInJob = true; if(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].speedmaster == false) { @@ -1098,6 +1145,11 @@ uint32_t ThreadPrepareState(void *JobDetails) { HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i]; Pid_Id = Motor_i;/*ThreadMotorIdToControlId[Motor_i];*/ +#ifdef FOUR_WINDERS + if (Motor_i == WINDER_2_MOTOR) Pid_Id = WINDER_MOTOR; + if (Motor_i == WINDER_3_MOTOR) Pid_Id = WINDER_MOTOR; + if (Motor_i == WINDER_4_MOTOR) Pid_Id = WINDER_MOTOR; +#endif MotorControlConfig[Motor_i].m_params.MAX = 1; MotorControlConfig[Motor_i].m_params.MIN = MotorsControl[Pid_Id].outputproportionalpowerlimit*-1; MotorControlConfig[Motor_i].m_params.Kd = MotorsControl[Pid_Id].derivativetime; @@ -1116,7 +1168,6 @@ uint32_t ThreadPrepareState(void *JobDetails) MotorControlConfig[Motor_i].m_preError = 0; MotorControlConfig[Motor_i].m_SetParam = 0;//need to update SetParams on presegment stage - HandleJobThreadControlParameters(JobTicket->threadparameters); //OVERRIDES CONFIGURATION PARAMETERS!!! temp_dt = MotorControlConfig[Motor_i].m_params.dt/0.001; MotorTiming[Motor_i] = (int)temp_dt; @@ -1210,7 +1261,24 @@ uint32_t ThreadPrepareState(void *JobDetails) ControlIdtoMotorId[Motor_i] = AddControlCallback(NULL,ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); #endif } +#ifdef FOUR_WINDERS + if ((Motor_i == WINDER_2_MOTOR)||(Motor_i == WINDER_3_MOTOR)||(Motor_i == WINDER_4_MOTOR)) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will n//ot be controlled + { + ReportWithPackageFilter(ThreadFilter,"Winder 2/3/4 Control",__FILE__,Motor_i,MotorControlConfig[WINDER_MOTOR].m_params.Kp,RpWarning,MotorControlConfig[WINDER_MOTOR].m_params.Ki,0); + if (ControlIdtoMotorId[Motor_i] != 0xFF) + { + if(RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction)!=OK) + ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)PoolerSpeedControlId,0); + CurrentControlledSpeed[Motor_i] = 0; + ControlIdtoMotorId[Motor_i] = 0xFF; + } +#ifndef TEST_PID_THREAD + ControlIdtoMotorId[Motor_i] = AddControlCallback(NULL,ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); +#endif +#endif + } } + HandleJobThreadControlParameters(JobTicket->threadparameters); //OVERRIDES CONFIGURATION PARAMETERS!!! #ifdef TEST_PID_THREAD testDancersControl(); @@ -1274,7 +1342,7 @@ uint32_t UpdatePidDuringRun(HardwarePidControl *request) void SetOriginMotorSpeed(float process_speed) { int Motor_i, HW_Motor_Id; - for (Motor_i = 0; Motor_i <= WINDER_MOTOR; Motor_i++) + for (Motor_i = 0; Motor_i < SCREW_MOTOR; Motor_i++) { HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i]; //(Speed*uStep*PPR)/((2*PI*motor_Radius) @@ -1509,7 +1577,7 @@ char Endstr[150]; PoolerSpeedControlId = 0xFF; } - for ( Motor_i = 0;Motor_i <= WINDER_MOTOR;Motor_i++) + for ( Motor_i = 0;Motor_i < SCREW_MOTOR;Motor_i++) { if (ControlIdtoMotorId[Motor_i] != 0xFF) { @@ -1522,17 +1590,9 @@ char Endstr[150]; } Task_sleep(100); - for ( Motor_i = 0;Motor_i <= WINDER_MOTOR;Motor_i++) + for ( Motor_i = 0;Motor_i < SCREW_MOTOR;Motor_i++) { MotorStop(ThreadMotorIdToMotorId[Motor_i],Hard_Hiz); -#ifdef FOUR_WINDERS - if (Motor_i == WINDER_MOTOR) - { - MotorStop(Winder_2_Motor, Hard_Hiz); - MotorStop(Winder_3_Motor, Hard_Hiz); - MotorStop(Winder_4_Motor, Hard_Hiz); - } -#endif } MotorStop(HARDWARE_MOTOR_TYPE__MOTO_RLOADING,Hard_Hiz); diff --git a/Software/Embedded_SW/Embedded/Modules/Waste/Waste_maint.c b/Software/Embedded_SW/Embedded/Modules/Waste/Waste_maint.c index 222b618d2..7c29a909b 100644 --- a/Software/Embedded_SW/Embedded/Modules/Waste/Waste_maint.c +++ b/Software/Embedded_SW/Embedded/Modules/Waste/Waste_maint.c @@ -76,7 +76,7 @@ void cartCART_EXTRACTED(WasteCartridgeEnum CartId) //{called from polling functi } WasteCartridgeEnum cartSELECT_CART(void) //{ if there is no SELECTED cartridge select the first cartridge in IN state. If there is none, return -1} { - Report("Cartridges select cart", __FILE__, __LINE__, CartState[WasteCartridge_middle], RpMessage, CartState[WasteCartridge_lower], 0); + //Report("Cartridges select cart", __FILE__, __LINE__, CartState[WasteCartridge_middle], RpMessage, CartState[WasteCartridge_lower], 0); if ((CartState[WasteCartridge_middle] == CartridgeStateSELECTED)||(CartState[WasteCartridge_middle] == CartridgeStateACTIVE)) return WasteCartridge_middle; if ((CartState[WasteCartridge_lower] == CartridgeStateSELECTED)||(CartState[WasteCartridge_lower] == CartridgeStateACTIVE)) |