From d86a63d6ec2c74b5e0edd49e22a5acd04c6b6f9c Mon Sep 17 00:00:00 2001 From: Shlomo Hecht Date: Wed, 15 Jul 2020 13:34:46 +0300 Subject: fix thread PID handling on job start. cleaning improved. stub for watchdog added --- .../Embedded/Modules/AlarmHandling/AlarmHandling.c | 2 +- .../Embedded/Modules/Diagnostics/Diagnostics.c | 9 +- .../Embedded/Modules/General/GeneralHardware.c | 6 +- .../Embedded_SW/Embedded/Modules/General/Safety.c | 18 ++- .../Embedded_SW/Embedded/Modules/General/buttons.h | 1 - .../Embedded_SW/Embedded/Modules/IDS/IDS_print.c | 6 +- .../Embedded/Modules/Stubs_Handler/Progress.c | 8 + .../Embedded/Modules/Thread/Thread_print.c | 169 ++++++++++++--------- 8 files changed, 133 insertions(+), 86 deletions(-) (limited to 'Software/Embedded_SW/Embedded/Modules') diff --git a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c index 1c2b6662d..fa835ee2e 100644 --- a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c +++ b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c @@ -893,7 +893,7 @@ JobEndReasonEnum AlarmHandlingPrepareJob(void *CurrentJob) if (AlarmItem[Alarm_i].AlarmSource == ALARM_SOURCE_TYPE__CoversAlarm) { - if (AlarmState[Alarm_i].Status == true) + if ((AlarmState[Alarm_i].Status == true)&&(AlarmItem[Alarm_i].Severity == DEBUG_LOG_CATEGORY__Error)) { FoundReason = JOB_TAMPER_ALARM; ReportWithPackageFilter(AlarmFilter,"tamper alarm preventing job", __FILE__,__LINE__,AlarmItem[Alarm_i].EventType, AlarmItem[Alarm_i].Severity, AlarmItem[Alarm_i].DeviceId, 0); diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c index 8e01fe18e..421206b45 100644 --- a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c +++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c @@ -864,10 +864,10 @@ void DiagnosticOneSecCollection(void) tempFlow = HeadFlowMeter; }*/ WasteLevel = GetWHSWasteTankLevelMiliLiter()/1000;//change from ml to litter - static double InitCounter = 0; - if (GetMachineState()=MACHINE_STATE_WAIT_FOR_COOLER)) { - InitCounter+=1.0; + InitCounter-=1.0; DiagnosticsMonitor.chillertemperature = &InitCounter; } @@ -1058,7 +1058,8 @@ void SendDiagnostics(void) DiagnosticsMonitor.dancer2angle = dancer2angle; DiagnosticsMonitor.dancer3angle = dancer3angle; */ - if ((JobIsActive())&&(DiagnosticMode >= Diagnostic_Extended_Mode)) +// if ((JobIsActive())&&(DiagnosticMode >= Diagnostic_Extended_Mode)) + if (DiagnosticMode >= Diagnostic_Extended_Mode) { DiagnosticsMonitor.n_dancer1angle = DancerCounterIndex[0]; DiagnosticsMonitor.n_dancer2angle = DancerCounterIndex[1]; diff --git a/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c b/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c index bec8fc53f..27ed8d0e9 100644 --- a/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c +++ b/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c @@ -51,7 +51,7 @@ #include "Drivers/I2C_Communication/Dispenser_Card/EEPROM/Dispenser_EEPROM.h" #include #include "Modules/General/buttons.h" -#include "Modules/Waste/Waste_ex.h" +#include "Modules/Waste/Waste.h" #include "Drivers/I2C_Communication/Main_Board_EEPROM/Main_EEPROM.h" #include #include @@ -165,8 +165,12 @@ uint32_t HWConfigurationInit(void) #ifndef DISPESER_TEST if (WHS_Type == WHS_TYPE_NEW) newWHS_init(); + //WHS_init(); // remove call to old WHS #endif + + // Waste Init (WHS) Waste_Init(); + ADC_MUX_Init(); GeneralHwReady = true; diff --git a/Software/Embedded_SW/Embedded/Modules/General/Safety.c b/Software/Embedded_SW/Embedded/Modules/General/Safety.c index 89ac8d020..099ac1769 100644 --- a/Software/Embedded_SW/Embedded/Modules/General/Safety.c +++ b/Software/Embedded_SW/Embedded/Modules/General/Safety.c @@ -46,7 +46,7 @@ uint32_t Safety_Main_State(uint32_t IfIndex, uint32_t BusyFlag) bool mDrierDoorAlarmState = false; bool mAirFlowAlarmState = false; bool mAirFilterAlarmState = false; - //bool mWasteOverflowAlarmState = false; + bool mWasteOverflowAlarmState = false; #ifdef CONTROL_DEBUG uint32_t tempp,tempq,delta; uint32_t sys_ticks_start = msec_millisecondCounter,sys_ticks_end,max = 0,dev = 0; @@ -91,6 +91,16 @@ uint32_t Safety_Main_State(uint32_t IfIndex, uint32_t BusyFlag) mDrierDoorAlarmState = true; DrierDoorAlarmState = true; } + else + { + //if (WHS_GPI_WASTE_OVERFULL()) - cannot read this switch + { + //report and handle waste overflow + AlarmHandlingSetAlarm(EVENT_TYPE__CHILLER_DRY_CONTACT, true); + mWasteOverflowAlarmState = true; + WasteOverflowAlarmState = true; + } + } } } @@ -224,6 +234,12 @@ uint32_t Safety_Main_State(uint32_t IfIndex, uint32_t BusyFlag) AlarmHandlingSetAlarm(EVENT_TYPE__AIR_FILTER_NOT_INSTALLED, false); AirFilterAlarmState = mAirFilterAlarmState; } + if ((mWasteOverflowAlarmState != WasteOverflowAlarmState)|| (mWasteOverflowAlarmState == false)) + { + //alarm went off + AlarmHandlingSetAlarm(EVENT_TYPE__WASTE_CONTAINER_OVERFLOW, false); + WasteOverflowAlarmState = mWasteOverflowAlarmState; + } #ifdef CONTROL_DEBUG tempq = HibernateRTCSSGet(); if (tempq < tempp) diff --git a/Software/Embedded_SW/Embedded/Modules/General/buttons.h b/Software/Embedded_SW/Embedded/Modules/General/buttons.h index 646861bc7..6a15d3765 100644 --- a/Software/Embedded_SW/Embedded/Modules/General/buttons.h +++ b/Software/Embedded_SW/Embedded/Modules/General/buttons.h @@ -59,7 +59,6 @@ uint32_t Buttons_Init(void); uint32_t Button_load_Init(void); uint32_t Button_JOG_Init(void); bool SetPowerMachineState(PBmachineState state); -void Ink_Cart_Led(); diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c index c444c7017..ac9bd0deb 100644 --- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c +++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c @@ -43,6 +43,8 @@ typedef struct HardwarePidControl *DispensersControl; HardwarePidControl DispensersCtrl[MAX_SYSTEM_DISPENSERS]; #define MAX_DYE_DISPENSERS 6 +#define IDS_PRESEGMENT_TIME_STEP 50 + int32_t DispenserSamples[MAX_SYSTEM_DISPENSERS][MAX_CONTROL_SAMPLES] = {0}; int DispenserSamplePointer[MAX_SYSTEM_DISPENSERS] = {0}; double DispenserNormalizedErrorCoEfficient[MAX_SYSTEM_DISPENSERS] = {0}; @@ -117,8 +119,9 @@ void IDS_Dispenser_SetPreSegmentWFCFValues(double dispenserpresegmentwfcf, doubl LeftRockerSpeed = ids_leftcleaningmotorspeed; if ( ids_rightcleaningmotorspeed) RightRockerSpeed = ids_rightcleaningmotorspeed; - Report("IDS_Dispenser_SetPreSegmentCleaningValues ",__FILE__,__LINE__,CleaningDispenserSpeed,RpWarning,(int)LeftRockerSpeed,0); + Report("IDS_Dispenser_SetPreSegmentCleaningValues ",__FILE__,__LINE__,RightRockerSpeed,RpWarning,(int)LeftRockerSpeed,0); Report("IDS_Dispenser_SetPreSegmentCleaningValues ",__FILE__,__LINE__,InterSegmentStartSprayCleaner,RpWarning,(int)InterSegmentCenterRockers,0); + Report("IDS_Dispenser actuator times ",__FILE__,(int)LeftRockerSpeed/100*IDS_PRESEGMENT_TIME_STEP,(int)LeftRockerSpeed%100*IDS_PRESEGMENT_TIME_STEP,RpWarning,(int)RightRockerSpeed*IDS_PRESEGMENT_TIME_STEP,0); } @@ -1034,7 +1037,6 @@ uint32_t InactiveDispenserHome(uint32_t DispenserId, uint32_t ReadValue) //******************************************************************************************************************** -#define IDS_PRESEGMENT_TIME_STEP 50 uint32_t IDSPreSegmentStateCallbackRunner(uint32_t IfIndex, uint32_t ReadValue) { JobDispenser **Dispensers; diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c index e5afba3b9..bfea7b13b 100644 --- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c +++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c @@ -526,6 +526,14 @@ void Stub_ProgressRequest(MessageContainer* requestContainer) response.has_progress = true; } else + if(request->amount == 0xAD9) //halt + { + LOG_ERROR(request->delay,"halt"); + memset (0,0,100000); + response.progress = IgnoreConeMissing; + response.has_progress = true; + } + else if((request->amount == 0x01) && ((request->delay &0x010000) == 0x010000)) //change mode powerset01 { response.progress = Power_Step_01_Mode(((request->delay &0x00FF00)>>8), request->delay &0x0000FF); diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c index 42a56ae8a..1c52a9b68 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -352,9 +352,9 @@ uint32_t PoolerThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) } if ((CurrentProcessedLength>=CurrentRequestedLength )&&(CurrentRequestedLength > 0.0)) { - usnprintf(Lenstr, 100, "Total processed length: Feeder: %d Pooler %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength); + usnprintf(Lenstr, 100, "Total processed length: Feeder: %d Puller %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength); SendJobProgress(0.0,0,false, Lenstr); - ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0); + ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,__LINE__,(int)(TotalProcessedLength*100),RpWarning,(int)(PoolerTotalProcessedLength*100),0); // segment/intersegment/distance to spool finished if (ProcessedLengthFuncPtr) ProcessedLengthFuncPtr(); @@ -696,18 +696,19 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) }*`/ }*/ calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index]; + calculated_speed = calculated_speed*InitialDryerSpeed/OriginalMotorSpd_2PPS[DRYER_MOTOR]; //calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*CurrentControlledSpeed[index]; - //if (JobCounter % eHundredMillisecond == 50) - if (FirstCalcInJob == true) + if (0)//(JobCounter % 1000 == 0) + //if (JobCounter < 100)//(FirstCalcInJob == true) { - if (index == POOLER_MOTOR) + if (index == FEEDER_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", - ReadValue,avreageSampleValue,(int)(MotorControlConfig[index].m_mesuredParam*1000000), + // 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", + 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); - ReportWithPackageFilter(ThreadFilter,TMessage,__FILE__,__LINE__,DancerId,RpError,ReadValue,0); + (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); } } @@ -720,6 +721,11 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) { if (calculated_speed>5.0) { + if (calculated_speed>(CurrentControlledSpeed[index]+100)) + { + ReportWithPackageFilter(ThreadFilter,"limit acceleration",__FILE__,calculated_speed,CurrentControlledSpeed[index],RpError,CurrentControlledSpeed[index]+100,0); + calculated_speed=CurrentControlledSpeed[index]+100; + } CurrentControlledSpeed[index] = calculated_speed; MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed); } @@ -845,6 +851,7 @@ uint32_t Release_Right_TFU_Tension() return status; } int SecondFeederCorrection = 4; +int PrepareWaitCount = 0; uint32_t Adjust_Right_TFU_Tension_2nd_Callback(uint32_t MotorId, uint32_t ReadValue) { MotorStop (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,Soft_Stop); //per L6470 errata between mov and run commands @@ -854,6 +861,18 @@ uint32_t Adjust_Right_TFU_Tension_2nd_Callback(uint32_t MotorId, uint32_t ReadVa Report("release tension - job aborted",__FILE__,__LINE__,MotorId,RpMessage,0,0); Release_Right_TFU_Tension(); } + if (PrepareWaitCount) + { + ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback",__FILE__,__LINE__,2,RpWarning,PrepareWaitCount,0); + PrepareWaitCount--; + } + if ((PrepareWaitCount == 0)&&(PrepareState == true)) + { + PrepareState = false; + ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback Prepare Ready",__FILE__,__LINE__,2,RpWarning,PrepareWaitCount,0); + PrepareReady(Module_Thread,ModuleDone); + } + return OK; } uint32_t Adjust_Right_TFU_Tension_Callback(uint32_t MotorId, uint32_t ReadValue) @@ -869,16 +888,16 @@ uint32_t Adjust_Right_TFU_Tension(double tension) uint32_t status = OK; if (tension > 0.5) //0 = lower position, 1 = high position { + //PrepareWaitCount++; if (FPGA_Read_limit_Switches(GPI_LS_RDANCER_UP) == NO_LIMIT) { - MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 40, GPI_LS_RDANCER_UP, Adjust_Right_TFU_Tension_Callback,15000); + MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 15, GPI_LS_RDANCER_UP, Adjust_Right_TFU_Tension_Callback,15000); Report("Adjust_Right_TFU_Tension",__FILE__,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize,HARDWARE_MOTOR_TYPE__MOTO_RDANCER,RpMessage,GPI_LS_RDANCER_UP,0); } } return status; } -int PrepareWaitCount = 0; uint32_t ThreadPrepare_TensionCallback (int DancerId, double tension) { if (PrepareWaitCount) @@ -886,7 +905,7 @@ uint32_t ThreadPrepare_TensionCallback (int DancerId, double tension) ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback",__FILE__,__LINE__,DancerId,RpWarning,PrepareWaitCount,0); PrepareWaitCount--; } - if (PrepareWaitCount == 0) + if ((PrepareWaitCount == 0)&&(PrepareState == true)) { PrepareState = false; ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback Prepare Ready",__FILE__,__LINE__,DancerId,RpWarning,PrepareWaitCount,0); @@ -1046,71 +1065,73 @@ uint32_t ThreadPrepareState(void *JobDetails) { HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i]; Pid_Id = Motor_i;/*ThreadMotorIdToControlId[Motor_i];*/ - 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; - MotorControlConfig[Motor_i].m_params.Kp = MotorsControl[Pid_Id].proportionalgain; - MotorControlConfig[Motor_i].m_params.Ki = MotorsControl[Pid_Id].integraltime; - MotorControlConfig[Motor_i].m_params.IntegralErrorMultiplier = MotorsControl[Pid_Id].setpointramprateorsoftstartramp; - MotorControlConfig[Motor_i].m_params.ProportionalErrorMultiplier = MotorsControl[Pid_Id].outputonoffhysteresisvalue; - MotorControlConfig[Motor_i].m_params.epsilon = MotorsControl[Pid_Id].epsilon; - MotorControlConfig[Motor_i].m_params.dt = MotorsControl[Pid_Id].controloutputtype; - MotorControlConfig[Motor_i].m_ingnoreValue = MotorsControl[Pid_Id].sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses - MotorControlConfig[Motor_i].m_calculatedError = 0; - MotorControlConfig[Motor_i].m_integral = 0; - MotorControlConfig[Motor_i].m_isEnabled = true; - MotorControlConfig[Motor_i].m_isReady = true; - MotorControlConfig[Motor_i].m_mesuredParam = 0; - 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; - if (MotorTiming[Motor_i]) - { - MotorTimer[Motor_i] = MotorTiming[Motor_i]-1; - ReportWithPackageFilter(ThreadFilter,"MotorTiming",__FILE__,Motor_i,MotorTiming[Motor_i],RpWarning,MotorTimer[Motor_i],0); - } + 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; + MotorControlConfig[Motor_i].m_params.Kp = MotorsControl[Pid_Id].proportionalgain; + MotorControlConfig[Motor_i].m_params.Ki = MotorsControl[Pid_Id].integraltime; + MotorControlConfig[Motor_i].m_params.IntegralErrorMultiplier = MotorsControl[Pid_Id].setpointramprateorsoftstartramp; + MotorControlConfig[Motor_i].m_params.ProportionalErrorMultiplier = MotorsControl[Pid_Id].outputonoffhysteresisvalue; + MotorControlConfig[Motor_i].m_params.epsilon = MotorsControl[Pid_Id].epsilon; + MotorControlConfig[Motor_i].m_params.dt = MotorsControl[Pid_Id].controloutputtype; + MotorControlConfig[Motor_i].m_ingnoreValue = MotorsControl[Pid_Id].sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses + MotorControlConfig[Motor_i].m_calculatedError = 0; + MotorControlConfig[Motor_i].m_integral = 0; + MotorControlConfig[Motor_i].m_isEnabled = true; + MotorControlConfig[Motor_i].m_isReady = true; + MotorControlConfig[Motor_i].m_mesuredParam = 0; + 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; + if (MotorTiming[Motor_i]) + { + MotorTimer[Motor_i] = MotorTiming[Motor_i]-1; + ReportWithPackageFilter(ThreadFilter,"MotorTiming",__FILE__,Motor_i,MotorTiming[Motor_i],RpWarning,MotorTimer[Motor_i],0); + } ////////////////////////////////////////////////// - for (i = 0;i < (int)MotorsControl[Motor_i].pvinputfilterfactormode; i++) - { - //if (Motor_i == DRYER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled - MotorSamples[Motor_i][i] = 0; - // else if ((Motor_i == POOLER_MOTOR)||(Motor_i == FEEDER_MOTOR)) - // MotorSamples[Motor_i][i] = DancersCfg[ThreadMotorIdToDancerId[Motor_i]].zeropoint; - //MotorSpeedSamples[Motor_i][i] = 0; - } - MotorSamplePointer[Motor_i] = 0; + for (i = 0;i < (int)MotorsControl[Motor_i].pvinputfilterfactormode; i++) + { + //if (Motor_i == DRYER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled + MotorSamples[Motor_i][i] = 0; + if (Motor_i == FEEDER_MOTOR) + MotorSamples[Motor_i][i] = -500; + // else if ((Motor_i == POOLER_MOTOR)||(Motor_i == FEEDER_MOTOR)) + // MotorSamples[Motor_i][i] = DancersCfg[ThreadMotorIdToDancerId[Motor_i]].zeropoint; + //MotorSpeedSamples[Motor_i][i] = 0; + } + MotorSamplePointer[Motor_i] = 0; ///////////////////////////////////////////////////// - MotorSetDirection((TimerMotors_t)HW_Motor_Id,MotorsCfg[HW_Motor_Id].directionthreadwize); + MotorSetDirection((TimerMotors_t)HW_Motor_Id,MotorsCfg[HW_Motor_Id].directionthreadwize); - 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 + 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 + { + ReportWithPackageFilter(ThreadFilter,"Feeder Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].m_params.Ki,0); + if (SpeedControlId != 0xFF) { - ReportWithPackageFilter(ThreadFilter,"Feeder Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].m_params.Ki,0); - if (SpeedControlId != 0xFF) - { - RemoveControlCallback(SpeedControlId,ThreadLengthCBFunction); - SpeedControlId = 0xFF; - } - //SetMotHome(ThreadMotorIdToMotorId[Motor_i]); - LengthCalculationMultiplier = (MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulleyradius*2*PI)/(MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulseperround*MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].microstep); - SpeedControlId = AddControlCallback(NULL,ThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i); + RemoveControlCallback(SpeedControlId,ThreadLengthCBFunction); + SpeedControlId = 0xFF; } - 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 + //SetMotHome(ThreadMotorIdToMotorId[Motor_i]); + LengthCalculationMultiplier = (MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulleyradius*2*PI)/(MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulseperround*MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].microstep); + SpeedControlId = AddControlCallback(NULL,ThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[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 + { + ReportWithPackageFilter(ThreadFilter,"Puller Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].m_params.Ki,0); + if (PoolerSpeedControlId != 0xFF) { - ReportWithPackageFilter(ThreadFilter,"Puller Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].m_params.Ki,0); - if (PoolerSpeedControlId != 0xFF) - { - if (RemoveControlCallback(PoolerSpeedControlId,PoolerThreadLengthCBFunction)!=OK) - ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)PoolerSpeedControlId,0); - PoolerSpeedControlId = 0xFF; - } - //SetMotHome(ThreadMotorIdToMotorId[Motor_i]); - PoolerLengthCalculationMultiplier = (MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulleyradius*2*PI)/(MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulseperround*MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].microstep); - PoolerSpeedControlId = AddControlCallback(NULL,PoolerThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i); + if (RemoveControlCallback(PoolerSpeedControlId,PoolerThreadLengthCBFunction)!=OK) + ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)PoolerSpeedControlId,0); + PoolerSpeedControlId = 0xFF; } + //SetMotHome(ThreadMotorIdToMotorId[Motor_i]); + PoolerLengthCalculationMultiplier = (MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulleyradius*2*PI)/(MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulseperround*MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].microstep); + PoolerSpeedControlId = AddControlCallback(NULL,PoolerThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i); + } 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 { if (ControlIdtoMotorId[Motor_i] != 0xFF) @@ -1152,10 +1173,6 @@ 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 } -// if (HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled -// AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,TemplateDataReadCBFunction,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],0); - if (Motor_i == ThreadMotorIdToMotorId[DRYER_MOTOR]) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled - continue; } #ifdef TEST_PID_THREAD @@ -1382,7 +1399,7 @@ char Endstr[150]; int Motor_i; ThreadControlActive = false; uint32_t status = OK; - usnprintf(Endstr, 100, "Total _processed length: Feeder: %d Pooler %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength); + usnprintf(Endstr, 100, "Total _processed length: Feeder: %d Puller %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength); SendJobProgress(0.0,0,false, Endstr); ReportWithPackageFilter(ThreadFilter,Endstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0); -- cgit v1.3.1