From 20310eb98a85ef1b4908c0d7b482f089c5a81bfb Mon Sep 17 00:00:00 2001 From: Shlomo Hecht Date: Wed, 4 Jul 2018 11:25:10 +0300 Subject: FS_SPD inmotors handled. some bugs ahndling --- .../Embedded/Modules/Thread/Thread_Winder.c | 54 ++++++++++++---------- .../Embedded/Modules/Thread/Thread_ex.h | 2 +- 2 files changed, 30 insertions(+), 26 deletions(-) (limited to 'Software/Embedded_SW/Embedded/Modules/Thread') diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c index e564e52d0..aa9e6ec94 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c @@ -145,37 +145,41 @@ uint32_t WinderPresegmentReady(uint32_t deviceID, uint32_t ReadValue) return PreSegmentReady(Module_Winder,ModuleDone); } -uint32_t Winder_Presegment(void *JobDetails) +uint32_t Winder_Presegment(void *JobDetails, uint32_t SegmentId) { JobTicket* JobTicket = JobDetails; int process_speed = JobTicket->processparameters->dyeingspeed; float screw_speed = 0; float RotationsPerSecond; - ScrewCurrentDirection = (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize); - - // * speed is set by the winding parameters and by winder rotational speed (read POSITION every 10msec) - // * calculate - // * 1. calculate speed according to JobTicket->processparameters->dyeingspeed - // * calculation input: traverse length in milimeters/pulses, number of rotations per traverse ==> length of traverse per rotation. - screw_speed = InternalWinderCfg.segmentoffsetpulses / InternalWinderCfg.NumberOfRotationPerPassage; - // calculation input#2: number of rotations per second - (basically: speed/winder perimeter. later - according to winder actual speed - calculate according to winder position accumulation in the last second. - RotationsPerSecond = process_speed / (InternalWinderCfg.diameter * PI); - // calculation input#3: speed = rotation per second * traverse per rotation = traverse per second. speed set: traverse per second (mm) * pulses per mm. - ScrewSpeed = screw_speed*RotationsPerSecond; - - MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed); - //screw_speed = InternalWinderCfg.milimetersperrotation -// * 2. determine optimal micro-step setting -// * 3. calculate cart travel length from winding parameters -// * 4. start move of travel length -// * 5. register motor nBusy callback. this callback will flip between move(traverse length, hardstop) and goto(0), with handline og the coneshape and adjusting maxspeed - ScrewNumberOfSteps = InternalWinderCfg.segmentoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep; - MotorMove (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewCurrentDirection,ScrewNumberOfSteps); //process: set point 0, set max speed, move to the specified length, return back. - ScrewControlId = AddControlCallback(ScrewDirectionChange, eOneMillisecond,MotorControlGetnBusyState,(IfTypeMotors*0x100+HARDWARE_MOTOR_TYPE__MOTO_SCREW), HARDWARE_MOTOR_TYPE__MOTO_SCREW, 0); - - // MotorSetSpeedWithCallback (HARDWARE_MOTOR_TYPE__MOTO_SCREW, screw_speed,WinderPresegmentReady); - //in a callback: calculate backing rate for top and bottom, update point 0, update passing length, call the appropriate move to 0 / move; + if (SegmentId == 0) // do all this only in the beginning of the job. do not touch after that (assuming spool does not change mid job) + { + ScrewCurrentDirection = (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize); + + // * speed is set by the winding parameters and by winder rotational speed (read POSITION every 10msec) + // * calculate + // * 1. calculate speed according to JobTicket->processparameters->dyeingspeed + // * calculation input: traverse length in milimeters/pulses, number of rotations per traverse ==> length of traverse per rotation. + screw_speed = InternalWinderCfg.segmentoffsetpulses / InternalWinderCfg.NumberOfRotationPerPassage; + // calculation input#2: number of rotations per second - (basically: speed/winder perimeter. later - according to winder actual speed - calculate according to winder position accumulation in the last second. + RotationsPerSecond = process_speed / (InternalWinderCfg.diameter * PI); + // calculation input#3: speed = rotation per second * traverse per rotation = traverse per second. speed set: traverse per second (mm) * pulses per mm. + ScrewSpeed = screw_speed*RotationsPerSecond; + + MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed); + //screw_speed = InternalWinderCfg.milimetersperrotation + // * 2. determine optimal micro-step setting + // * 3. calculate cart travel length from winding parameters + // * 4. start move of travel length + // * 5. register motor nBusy callback. this callback will flip between move(traverse length, hardstop) and goto(0), with handline og the coneshape and adjusting maxspeed + ScrewNumberOfSteps = InternalWinderCfg.segmentoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep; + MotorMove (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewCurrentDirection,ScrewNumberOfSteps); //process: set point 0, set max speed, move to the specified length, return back. + ScrewControlId = AddControlCallback(ScrewDirectionChange, eOneMillisecond,MotorControlGetnBusyState,(IfTypeMotors*0x100+HARDWARE_MOTOR_TYPE__MOTO_SCREW), HARDWARE_MOTOR_TYPE__MOTO_SCREW, 0); + + // MotorSetSpeedWithCallback (HARDWARE_MOTOR_TYPE__MOTO_SCREW, screw_speed,WinderPresegmentReady); + //in a callback: calculate backing rate for top and bottom, update point 0, update passing length, call the appropriate move to 0 / move; + + } PreSegmentReady(Module_Winder,ModuleDone); return OK; diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h index b7401c77f..4a7f1144c 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h @@ -23,7 +23,7 @@ uint32_t ThreadInitialTestStub(); //uint32_t MotorPidRequestMessage(HardwarePidControl* request); uint32_t Winder_Init(void); uint32_t Winder_Prepare(void); -uint32_t Winder_Presegment(void *JobDetails); +uint32_t Winder_Presegment(void *JobDetails, uint32_t SegmentId); uint32_t Winder_End(void); -- cgit v1.3.1 From c8807df7010acb129997cb7ca130e6b7928ce809 Mon Sep 17 00:00:00 2001 From: Shlomo Hecht Date: Sun, 8 Jul 2018 09:44:28 +0300 Subject: reports in dancer control --- Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c | 8 ++++++++ 1 file changed, 8 insertions(+) (limited to 'Software/Embedded_SW/Embedded/Modules/Thread') diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c index 6740d6304..cfff7773b 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -286,7 +286,15 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) { DancerId = ThreadMotorIdToDancerId[index]; if (ReadValue < 10) + { + REPORT_MSG(ReadValue, "Dancer value read too small."); return OK; + } + if (ReadValue == 0x3FFF) + { + LOG_ERROR(index, "Dancer value invalid."); + return OK; + } TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint; if (index == POOLER_MOTOR) { -- cgit v1.3.1 From 01313ef571f4ca7376c8579db38d83a892360e0a Mon Sep 17 00:00:00 2001 From: Shlomo Hecht Date: Sun, 8 Jul 2018 18:58:19 +0300 Subject: some debug info --- .../Embedded/Modules/Thread/Thread_print.c | 39 +++++++++++++++++++--- .../Embedded/StateMachines/Printing/PrintingSTM.c | 2 +- 2 files changed, 35 insertions(+), 6 deletions(-) (limited to 'Software/Embedded_SW/Embedded/Modules/Thread') diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c index cfff7773b..f91c2a733 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -122,8 +122,8 @@ void ThreadUpdateProcessLength (double length, void *Funcptr) ProcessedLengthFuncPtr = (ProcessedLengthFunc)Funcptr; initialpos = 0xFFFF; } -double MotorSentData[1000] = {0}; -uint32_t PosDif[1000] = {0}; +double MotorSentData[100] = {0}; +uint32_t PosDif[100] = {0}; int MotorDataIndex = 0; @@ -175,7 +175,7 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) //PosDif[MotorDataIndex] = positionDiff; MotorSentData[MotorDataIndex] = length; MotorDataIndex+=1; - if (MotorDataIndex == 999) MotorDataIndex = 0; + if (MotorDataIndex == 99) MotorDataIndex = 0; static int pooler_counter = 0; pooler_counter++; if (pooler_counter%10 == 0) @@ -248,6 +248,12 @@ uint32_t ThreadControlSpeedReadFunction(uint32_t IfIndex, uint32_t ReadValue) } return OK; } +double calculatedError[1000]; +int readValue[1000]; +int calculatedspeed[1000]; +int controlIndex = 0; +int32_t KeepReadValue = 0; + uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) { //#define MAX_CONTROL_SAMPLES 6 @@ -259,6 +265,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) int DancerId; static int pooler_counter = 0; int32_t TranslatedReadValue, avreageSampleValue = 0; + double tempcalcspeed = 0; uint32_t calculated_speed; double NormalizedError; char Message[60]; @@ -324,10 +331,26 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation((float)MotorControlConfig[index].m_SetParam , (float)MotorControlConfig[index].m_mesuredParam, &MotorControlConfig[index].m_params, &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral); if (index != FEEDER_MOTOR) //feeder unit handles errors opposite to left unit + { MotorControlConfig[index].m_calculatedError = (-1*MotorControlConfig[index].m_calculatedError); + } else - KeepNormalizedError = NormalizedError; + { + //KeepNormalizedError = NormalizedError; + } calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index]; + if (index == POOLER_MOTOR) + { + if (KeepReadValue != TranslatedReadValue) + { + calculatedError[controlIndex] = MotorControlConfig[index].m_calculatedError; + readValue[controlIndex] = TranslatedReadValue; + calculatedspeed[controlIndex] = calculated_speed; + controlIndex++; + if (controlIndex >= 999) controlIndex = 0; + KeepReadValue = TranslatedReadValue; + } + } if (abs(calculated_speed-CurrentControlledSpeed[index])>5) { CurrentControlledSpeed[index] = calculated_speed; @@ -489,7 +512,13 @@ uint32_t ThreadPreSegmentState(void *JobDetails) float process_speed; if (JobTicket->processparameters) - process_speed= JobTicket->processparameters->dyeingspeed; + if (JobTicket->processparameters->dyeingspeed) + process_speed= JobTicket->processparameters->dyeingspeed; + else + { + LOG_ERROR (-1," unknown job speed"); + return ERROR; + } SetOriginMotorSpeed(process_speed); //ControlStart(); diff --git a/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.c b/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.c index fafe1dd8e..55de7cf57 100644 --- a/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.c +++ b/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.c @@ -347,7 +347,7 @@ void PrintSTMMsgHandler(void * msg) JobMessageStruc *Message = msg; PrintMessageStruc *PrtMessage = (PrintMessageStruc *)Message->messageData; - Report(REPORT_LINE("PrintSTMMsgHandler"),__FILE__,__LINE__, RpMessage,0x1000,Message->messageId,PrtMessage->messageId); + Report("PrintSTMMsgHandler",__FILE__,__LINE__, RpMessage,0x1000,Message->messageId,PrtMessage->messageId); if (Message->messageId != PrintMessage) { -- cgit v1.3.1 From 47396728e782e433a11768904cda94c47dc02933 Mon Sep 17 00:00:00 2001 From: Shlomo Hecht Date: Mon, 9 Jul 2018 13:08:00 +0300 Subject: Debug info and improve job handling --- .../Embedded_SW/Embedded/Communication/Container.c | 11 ++++++++++ .../Embedded_SW/Embedded/Drivers/Motors/Motor.c | 5 ++++- .../Embedded_SW/Embedded/Modules/General/process.c | 24 +++++++++++++++++++++- .../Embedded/Modules/Thread/Thread_Winder.c | 2 +- .../Embedded/Modules/Thread/Thread_print.c | 6 ++++-- .../Embedded/StateMachines/Printing/PrintingSTM.c | 9 ++++++++ .../Embedded/StateMachines/Printing/PrintingSTM.h | 2 +- 7 files changed, 53 insertions(+), 6 deletions(-) (limited to 'Software/Embedded_SW/Embedded/Modules/Thread') diff --git a/Software/Embedded_SW/Embedded/Communication/Container.c b/Software/Embedded_SW/Embedded/Communication/Container.c index f7c9ef9fc..2b12cbeec 100644 --- a/Software/Embedded_SW/Embedded/Communication/Container.c +++ b/Software/Embedded_SW/Embedded/Communication/Container.c @@ -63,6 +63,11 @@ MessageContainer createContainer(MessageType type, char* token, protobuf_c_boole //free(response); return container; } +uint32_t msgId[50] = {0}; +uint32_t Length[50] = {0}; +uint32_t DataLength[50] = {0}; +byte index = 0; + void receive_callback(char* buffer, size_t length) { //SendChars(buffer,length); @@ -75,7 +80,13 @@ void receive_callback(char* buffer, size_t length) //writeLine("Parsing message..."); MessageContainer* requestContainer = message_container__unpack(NULL, length, (uint8_t*)buffer); + HeatingTestSendResonse(0, false,true,true, /*OriginalMotorSpd_2PPS[index]*/length,requestContainer->type,0,0, "Container"); + msgId[index] = requestContainer->type; + Length[index] = length; + DataLength[index] = requestContainer->data.len; + if (index++>=50) + index = 0; switch(requestContainer->type) { case MESSAGE_TYPE__CalculateRequest: diff --git a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c index bc7e08e75..a03d74dce 100644 --- a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c +++ b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c @@ -461,7 +461,10 @@ uint32_t MotorMovetoLimitSwitch (TimerMotors_t _motorId,bool direction, uint32_t assert (LimitSwitchId<=MAX_GPI); if (MotorMovetoLimitSwitchControlId[_motorId] != 0xFF) - return ERROR; + { + RemoveControlCallback(MotorMovetoLimitSwitchControlId[_motorId], MotorMoveToLimitSwitchCallBackFunction ); + //return ERROR; + } //call driver action to device id with the parameter //SetMotorSpeed (deviceId, parameter); diff --git a/Software/Embedded_SW/Embedded/Modules/General/process.c b/Software/Embedded_SW/Embedded/Modules/General/process.c index 41af277a8..cd827b6bf 100644 --- a/Software/Embedded_SW/Embedded/Modules/General/process.c +++ b/Software/Embedded_SW/Embedded/Modules/General/process.c @@ -85,6 +85,28 @@ uint32_t HandleProcessParameters(ProcessParameters* ProcessParams) HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ4, false, ProcessParams->headzone4temp); } + if (ProcessParams->has_headzone5temp) + { + if (ProcessParams->headzone5temp) + status |= HeaterCommandRequestMessage( + HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ5, true, + ProcessParams->headzone5temp); + else + status |= HeaterCommandRequestMessage( + HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ5, false, + ProcessParams->headzone5temp); + } + if (ProcessParams->has_headzone6temp) + { + if (ProcessParams->headzone6temp) + status |= HeaterCommandRequestMessage( + HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ6, true, + ProcessParams->headzone6temp); + else + status |= HeaterCommandRequestMessage( + HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ6, false, + ProcessParams->headzone6temp); + } if (ProcessParams->has_dryerzone1temp) { if (ProcessParams->dryerzone1temp) @@ -150,7 +172,7 @@ uint32_t HandleProcessParameters(ProcessParameters* ProcessParams) { dryerairflow = ProcessParams->dryerairflow; } - return status; + return OK;//status; } void ProcessRequestFunc(MessageContainer* requestContainer) diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c index aa9e6ec94..a119b0b16 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c @@ -199,7 +199,7 @@ void Winder_ScrewHomeLimitSwitchInterrupt(void) uint32_t status; if (Winder_ScrewHoming) { - MotorStop(HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Stop); //stop ASAP + MotorStop(HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz); //stop ASAP } status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);//make sure to move the cart out } diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c index f91c2a733..1ae1145fb 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -72,6 +72,7 @@ void ThreadInterSegmentEnded(void); void ThreadDistanceToSpoolEnded(void); double KeepNormalizedError = 0; +bool ThreadControlActive = false; ////////////////////////Slow Motor State//////////////////////////////////// //uint32_t ThreadPreSegmentState(void *JobDetails); @@ -416,7 +417,7 @@ bool InitialProcess = false; 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.epsilon = 0.1; - MotorControlConfig[Motor_i].m_params.dt = 50; + MotorControlConfig[Motor_i].m_params.dt = 1000; MotorControlConfig[Motor_i].m_calculatedError = 0; MotorControlConfig[Motor_i].m_integral = 0; MotorControlConfig[Motor_i].m_isEnabled = true; @@ -521,7 +522,7 @@ uint32_t ThreadPreSegmentState(void *JobDetails) } SetOriginMotorSpeed(process_speed); - //ControlStart(); + ThreadControlActive = true; // 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]); //only for testing - when control works, these motors will take their speed from the dryer @@ -585,6 +586,7 @@ uint32_t ThreadSegmentState(void *JobDetails, int SegmentId) uint32_t ThreadEndState(void *JobDetails) { int Motor_i; + ThreadControlActive = false; ThreadUpdateProcessLength (0.0,(void *)NULL); SetOriginMotorSpeed(0); diff --git a/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.c b/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.c index 55de7cf57..d0b17fbf5 100644 --- a/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.c +++ b/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.c @@ -288,11 +288,17 @@ uint32_t EndState(void *JobDetails, char *Message) DiagnosticsStop(); if (Configured[Module_Winder]) { + PrepareWaiting[Module_Winder] = ModuleIdle; + SegmentWaiting[Module_Winder] = ModuleIdle; + PreSegmentWaiting[Module_Winder] = ModuleIdle; // EndWaiting[Module_Winder] = ModuleWaiting; Winder_End(); } if (Configured[Module_IDS]) { + PrepareWaiting[Module_IDS] = ModuleIdle; + SegmentWaiting[Module_IDS] = ModuleIdle; + PreSegmentWaiting[Module_IDS] = ModuleIdle; //EndWaiting[Module_IDS] = ModuleWaiting; IDSEndState(JobDetails); } @@ -304,6 +310,9 @@ uint32_t EndState(void *JobDetails, char *Message) }*/ if (Configured[Module_Thread]) { + PrepareWaiting[Module_Thread] = ModuleIdle; + SegmentWaiting[Module_Thread] = ModuleIdle; + PreSegmentWaiting[Module_Thread] = ModuleIdle; //EndWaiting[Module_Thread] = ModuleWaiting; ThreadEndState(CurrentJob); } diff --git a/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.h b/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.h index 0a210eeb1..61a51ba92 100644 --- a/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.h +++ b/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.h @@ -119,7 +119,7 @@ typedef struct PrintMessage{ }PrintMessageStruc; extern bool Configured[MAX_SYSTEM_MODULES]; - +extern ModuleStateEnum PrepareWaiting[MAX_SYSTEM_MODULES]; extern JobTicket *CurrentJob; //extern JobTicket *PreviousJob; extern Mailbox_Handle JobmsgQ; -- cgit v1.3.1