From f502659ef54f1f798952089989923d1e76ab06cb Mon Sep 17 00:00:00 2001 From: Shlomo Hecht Date: Wed, 13 Jun 2018 15:15:42 +0300 Subject: screw handling initial works --- .../Embedded_SW/Embedded/Modules/Thread/Thread.h | 2 +- .../Embedded/Modules/Thread/Thread_Winder.c | 65 ++++++++++++++-------- .../Embedded/Modules/Thread/Thread_ex.h | 4 ++ .../Embedded/Modules/Thread/Thread_init.c | 7 ++- .../Embedded/Modules/Thread/Thread_print.c | 10 ++-- 5 files changed, 54 insertions(+), 34 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 57b7a9d99..dad7a1db6 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h @@ -55,7 +55,7 @@ extern HardwareDancer DancersCfg[MAX_SYSTEM_DANCERS]; extern int32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES]; extern int MotorSamplePointer[MAX_THREAD_MOTORS_NUM]; extern double NormalizedErrorCoEfficient[MAX_THREAD_MOTORS_NUM]; -extern double DancerStopActivityLimit[MAX_THREAD_MOTORS_NUM]; +extern int DancerStopActivityLimit[MAX_THREAD_MOTORS_NUM]; uint32_t InternalWinderConfigMessage(HardwareWinder* request); diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c index 164e5eb23..37f60e8cf 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c @@ -9,6 +9,7 @@ #include "Drivers/Peripheral_GPIO/GPIO.h" #include "drivers/Motors/Motor.h" #include "StateMachines/Printing/PrintingSTM.h" +#include "Modules/Control/Control.h" #include "drivers/FPGA/FPGA.h" @@ -16,10 +17,12 @@ #include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h" bool Winder_ScrewHoming = false; -uint32_t Winder_ScrewAtOffsetCallback(uint32_t NumberOfSteps); +uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag); uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue); +bool ScrewCurrentDirection = false; double ScrewSpeed = 0; +uint32_t ScrewControlId = 0xFF; uint32_t Winder_Init(void) { @@ -75,7 +78,7 @@ uint32_t Winder_Prepare(void *JobDetails) float process_speed = JobTicket->processparameters->dyeingspeed; // double ScrewSpeed = (50 * MotorsCfg[SCREW_MOTOR].pulseperround * MotorsCfg[SCREW_MOTOR].microstep)/(2*PI* MotorsCfg[SCREW_MOTOR].pulleyradius); - double ScrewSpeed = (process_speed * MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulseperround)/(2*PI* MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulleyradius); + double ScrewSpeed = (process_speed*20 );//* MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulseperround)/(2*PI* MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulleyradius); /* * 1. move home to the limit switch (check that the cart is clear from the limit switch, start moving, with acceleration to maximal speed. enable interrupt on the limit switch, upon interrupt stop. @@ -90,22 +93,23 @@ uint32_t Winder_Prepare(void *JobDetails) else { Winder_ScrewHoming = true; -// status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed, GPI_LS_SCREW_RIGHT, Winder_PrepareStage2); - status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed/10, GPI_LS_SCREW_RIGHT, Winder_Test1); + status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed, GPI_LS_SCREW_LEFT, Winder_PrepareStage2); +// status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed/10, GPI_LS_SCREW_RIGHT, Winder_Test1); } return status; } uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue) { - uint32_t status; - uint32_t numOfSteps = InternalWinderCfg.segmentoffsetpulses; + uint32_t status=OK; + uint32_t numOfSteps = InternalWinderCfg.startoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep; /* * 1. move home to the limit switch (check that the cart is clear from the limit switch, start moving, with acceleration to maximal speed. enable interrupt on the limit switch, upon interrupt stop. * 2. move back x steps - according to thehw specifications and bobine definitions in the job. move for a predefined number of steps. get a callback when done * report ready to the job STM */ - status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,(1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize));//make sur to move the cart out -// status |= MotorMove(HARDWARE_MOTOR_TYPE__MOTO_SCREW, numOfSteps, Winder_ScrewAtOffsetCallback); + //status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,(1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize));//make sur to move the cart out + //status |= StopMotor(HARDWARE_MOTOR_TYPE__MOTO_SCREW,0); + status |= MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_SCREW, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize),numOfSteps, Winder_ScrewAtOffsetCallback); //status |= MotorMoveSteps (HARDWARE_MOTOR_TYPE__MOTO_SCREW, numOfSteps, Winder_ScrewAtOffsetCallback); //set motor location 0 here return status; @@ -116,6 +120,24 @@ uint32_t WinderPresegmentReady(uint32_t deviceID, uint32_t ReadValue) { return PreSegmentReady(Module_Winder,ModuleDone); } +uint32_t ScrewDirectionChange(uint32_t deviceID, uint32_t BusyFlag) +{ + //ScrewCurrentDirection: false moves out, true moves home + if (BusyFlag == NOTBUSY) + { + if (ScrewCurrentDirection == false) + { + ScrewCurrentDirection = true; + } + else + { + ScrewCurrentDirection = false; + } + MotorMove (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewCurrentDirection,InternalWinderCfg.segmentoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep); //process: set point 0, set max speed, move to the specified length, return back. + } + return OK; + +} uint32_t Winder_Presegment(void *JobDetails) { JobTicket* JobTicket = JobDetails; @@ -123,8 +145,9 @@ uint32_t Winder_Presegment(void *JobDetails) float screw_speed = 0; float RotationsPerSecond; + ScrewCurrentDirection = (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize); // double ScrewSpeed = (process_speed * MotorsCfg[SCREW_MOTOR].pulseperround * MotorsCfg[SCREW_MOTOR].microstep)/(2*PI* MotorsCfg[SCREW_MOTOR].pulleyradius); - double ScrewSpeed = (process_speed * MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulseperround)/(2*PI* MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulleyradius); + //double ScrewSpeed = (process_speed * MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulseperround)/(2*PI* MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulleyradius); /*typedef struct { @@ -150,16 +173,19 @@ uint32_t Winder_Presegment(void *JobDetails) // * 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 + MotorMove (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewCurrentDirection,InternalWinderCfg.segmentoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep); //process: set point 0, set max speed, move to the specified length, return back. + ScrewControlId = AddControlCallback(ScrewDirectionChange, eTenMilliSecond,MotorControlGetnBusyState,(IfTypeMotors*0x100+HARDWARE_MOTOR_TYPE__MOTO_SCREW), HARDWARE_MOTOR_TYPE__MOTO_SCREW, 0); - //MotorMove (InternalWinderCfg.segmentoffsetpulses,screw_speed); process: set point 0, set max speed, move to the specified length, return back. - MotorSetSpeedWithCallback (HARDWARE_MOTOR_TYPE__MOTO_SCREW, screw_speed,WinderPresegmentReady); + // 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; } uint32_t Winder_End(void) { //stop screw + RemoveControlCallback(ScrewControlId,ScrewDirectionChange); return StopMotor (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz); } void Winder_ScrewHomeLimitSwitchInterrupt(void) @@ -176,19 +202,10 @@ void Winder_ScrewOutLimitSwitchInterrupt(void) uint32_t status; status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);//make sure to move the cart out } -uint32_t Winder_ScrewAtOffsetCallback(uint32_t NumberOfSteps) +uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag) { - if (NumberOfSteps == InternalWinderCfg.segmentoffsetpulses) - { - PrepareReady(Module_Winder, ModuleDone); - return OK; - } - else - { - //do we want to do something? - PrepareReady(Module_Winder, ModuleFail); - return ERROR; - } - + SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_SCREW); //set this point as the spool home + PrepareReady(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 28a2346c1..8e8ea60e0 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h @@ -2,6 +2,8 @@ #ifndef MODULES_THREAD_THREAD_EX_H_ #define MODULES_THREAD_THREAD_EX_H_ +#include "PMR/Printing/JobSpool.pb-c.h" + //typedef enum //{ #define WINDER_DANCER HARDWARE_DANCER_TYPE__LeftDancer @@ -10,6 +12,8 @@ #define NUM_OF_DANCERS HARDWARE_DANCER_TYPE__RightDancer+1 //} DANCER_ENUM; +uint32_t InternalWindingConfigMessage(JobSpool* request); + 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 e2d492b47..3c9adbc31 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c @@ -17,7 +17,7 @@ HardwarePidControl MotorsControl[MAX_THREAD_MOTORS_NUM] = {0}; int32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES] = {0}; int MotorSamplePointer[MAX_THREAD_MOTORS_NUM] = {0}; double NormalizedErrorCoEfficient[MAX_THREAD_MOTORS_NUM] = {0}; -double DancerStopActivityLimit[MAX_THREAD_MOTORS_NUM] = {0}; +int DancerStopActivityLimit[MAX_THREAD_MOTORS_NUM] = {0}; InternalWinderConfigStruc InternalWinderCfg = {0}; HardwareDancer DancersCfg[MAX_SYSTEM_DANCERS] = {0}; @@ -101,8 +101,9 @@ uint32_t MotorPidRequestMessage(HardwarePidControl* request) temp=(10*(temp-1)*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm); NormalizedErrorCoEfficient[Motor_i] = NormalizedErrorCoEfficient[Motor_i] / temp; // uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES]; - DancerStopActivityLimit[Motor_i] = ((1<<(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].resolutionbits)*(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm)*2)); - DancerStopActivityLimit[Motor_i] = DancerStopActivityLimit[Motor_i]/(2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength); + temp = 1<<(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].resolutionbits); + temp = (temp*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm*2); + DancerStopActivityLimit[Motor_i] = temp/(2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength); return OK; } uint32_t DancerConfigMessage(HardwareDancer * request) diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c index 56e1bf844..fdae44171 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -121,7 +121,6 @@ void ThreadUpdateProcessLength (double length, void *Funcptr) } double MotorSentData[1000] = {0}; uint32_t PosDif[1000] = {0}; -uint32_t tick[1000] = {0}; int MotorDataIndex = 0; @@ -172,15 +171,14 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) PosDif[MotorDataIndex] = CurrentPosition; //PosDif[MotorDataIndex] = positionDiff; MotorSentData[MotorDataIndex] = length; - tick[MotorDataIndex] = UsersysTickGet(); MotorDataIndex+=1; if (MotorDataIndex == 999) MotorDataIndex = 0; static int pooler_counter = 0; pooler_counter++; if (pooler_counter%10 == 0) { - //SendJobProgress(CurrentProcessedLength/CurrentRequestedLength,CurrentSegmentId,false); - SendJobProgress(/*KeepNormalizedError*/MotorControlConfig[index].m_calculatedError,CurrentSegmentId,false); + SendJobProgress(CurrentProcessedLength/CurrentRequestedLength,CurrentSegmentId,false); + //SendJobProgress(/*KeepNormalizedError*/MotorControlConfig[index].m_calculatedError,CurrentSegmentId,false); } if (pooler_counter>=100) @@ -486,10 +484,10 @@ uint32_t ThreadPreSegmentState(void *JobDetails) //#warning rocker disabled if (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].has_directionthreadwize) MotorSetDirection((TimerMotors_t)HARDWARE_MOTOR_TYPE__MOTO_RLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize); - MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 10); + MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 5); if (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].has_directionthreadwize) MotorSetDirection((TimerMotors_t)HARDWARE_MOTOR_TYPE__MOTO_LLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize); - MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 10); + MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 5); //#warning rocker disabled // MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize, 0, GPI_LS_RLOADMOTOR_UP, EndState); //TODO -- cgit v1.3.1 From 87973c0cc97ba449e2b17c92ff6eb68e2a28a871 Mon Sep 17 00:00:00 2001 From: Shlomo Hecht Date: Thu, 14 Jun 2018 17:48:34 +0300 Subject: thread winder improvements --- .../Embedded_SW/Embedded/Communication/Container.c | 1 - Software/Embedded_SW/Embedded/DataDef.h | 1 - .../Embedded/Drivers/FPGA/FPGA_SPI_Comm.c | 16 +- .../Embedded/Drivers/FPGA/FPGA_SPI_Comm.h | 1 + .../Embedded_SW/Embedded/Drivers/Motors/Motor.c | 13 +- .../Embedded_SW/Embedded/Drivers/Motors/Motor.h | 4 +- Software/Embedded_SW/Embedded/Main.c | 1 + .../Embedded/Modules/AlarmHandling/AlarmHandling.c | 8 - .../Embedded/Modules/Control/MillisecTask.c | 3 +- .../Embedded/Modules/Heaters/Heaters_print.c | 2 +- .../Embedded_SW/Embedded/Modules/IDS/IDS_print.c | 41 +++-- .../Modules/Stubs_Handler/Stub_Cartridge.c | 9 +- .../Embedded/Modules/Stubs_Handler/Stub_I2C.c | 4 +- .../Embedded/Modules/Thread/Thread_Winder.c | 173 ++++++++++----------- .../Embedded/Modules/Thread/Thread_print.c | 4 +- .../Embedded/StateMachines/Printing/JobSTM.c | 35 +++-- .../Embedded/StateMachines/Printing/PrintingSTM.c | 16 +- 17 files changed, 187 insertions(+), 145 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 d33d913d0..cf1cb3ef5 100644 --- a/Software/Embedded_SW/Embedded/Communication/Container.c +++ b/Software/Embedded_SW/Embedded/Communication/Container.c @@ -10,7 +10,6 @@ #include "Modules/Stubs_Handler/Stub_Dispenser.h" #include "Modules/Stubs_Handler/Stub_GPIO.h" #include "Modules/Stubs_Handler/Stub_Heater.h" -#include "Modules/Stubs_Handler/Stub_MotorEncoder.h" #include "Modules/Stubs_Handler/Stub_OptLimitSwitch.h" #include "Modules/Stubs_Handler/Stub_SteperMotor.h" #include "Modules/Stubs_Handler/Stub_Valve.h" diff --git a/Software/Embedded_SW/Embedded/DataDef.h b/Software/Embedded_SW/Embedded/DataDef.h index 95d0bd6f1..12f896c77 100644 --- a/Software/Embedded_SW/Embedded/DataDef.h +++ b/Software/Embedded_SW/Embedded/DataDef.h @@ -13,7 +13,6 @@ #define ON 1 #define OFF 0 -#define test enum { 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 3739950d4..4ee88833f 100644 --- a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c +++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c @@ -571,6 +571,20 @@ void FPGA_SetMotSpeed(TimerMotors_t _motorId)//Run //FPGA_SPI_Transnit(_motorId); } +void FPGA_SetMotMaxSpeed(TimerMotors_t _motorId)//Mov +{ + uint32_t temp; + + + temp = x_SET_PARAM | x_MAX_SPEED; + temp = temp << 24; + uint16_t speed_calc = MaxSpdCalc(MotorsCfg[_motorId].maxfrequency); + if (speed_calc > 0x3FF) //TODO to check if it is needed here + speed_calc = 0x3FF; + temp |= speed_calc<<8; + + MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback); +} void FPGA_SetMotPosition(TimerMotors_t _motorId)//Mov { @@ -608,7 +622,7 @@ void FPGA_SetMotHome(TimerMotors_t _motorId) //Fpga_Spi[_motorId].AMT_OF_Words = 1; temp = Fpga_Spi[_motorId].TX_MOSI; - if( MillisecWriteToMotor(_motorId, temp, 4, NULL) < 0 ) return ERROR; + if( MillisecWriteToMotor(_motorId, temp, 4, NULL) < 0 ) return ; //FPGA_SPI_Transnit(_motorId); } diff --git a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.h b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.h index ca3477e7c..ac126042a 100644 --- a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.h +++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.h @@ -20,6 +20,7 @@ void FPGA_GetBusy(); void FPGA_SetMotStop(TimerMotors_t _motorId); void FPGA_GetMotMicroSteps_Cmd(TimerMotors_t _motorId); void FPGA_GetMotPosition_Cmd(TimerMotors_t _motorId); +void FPGA_SetMotMaxSpeed(TimerMotors_t _motorId); void FPGA_SetMotPosition(TimerMotors_t _motorId); void FPGA_SetMotSpeed(TimerMotors_t _motorId); void FPGA_SetMotorsInit(); diff --git a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c index 2f7f48ccc..eb2f25100 100644 --- a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c +++ b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c @@ -311,6 +311,14 @@ uint32_t SetMotHome(TimerMotors_t _motorId) return OK; } +uint32_t MotorSetMaxSpeed(TimerMotors_t _motorId, uint32_t MaxSpeed) +{ + MotorsCfg[_motorId].maxfrequency = MaxSpeed; + FPGA_SetMotMaxSpeed(_motorId); + + return OK; +} + uint32_t MotorMove(TimerMotors_t _motorId,bool direction, uint32_t Steps) { @@ -400,7 +408,7 @@ uint32_t MotorSetSpeedCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag) //TO } uint32_t MotorMoveToLimitSwitchCallBackFunction(uint32_t IfIndex, bool LimitSwitch) //TODO { - uint32_t MotorId; + TimerMotors_t MotorId; if (IfIndex>>8 != IfTypeMotors) { LOG_ERROR (IfIndex, "Wrong Interface type"); @@ -410,9 +418,10 @@ uint32_t MotorMoveToLimitSwitchCallBackFunction(uint32_t IfIndex, bool LimitSwit if (LimitSwitch == LIMIT) { - StopMotor(MotorId,Hard_Hiz); //stop this control loop RemoveControlCallback(MotorMovetoLimitSwitchControlId[MotorId], MotorMoveToLimitSwitchCallBackFunction ); + StopMotor(MotorId,Hard_Hiz); //TODO in run time limit switch just reverse direction + //possibly: start regular control (speed etc) //uint32_t ControlId = AddControlCallback(ControlCBFunction Callback, eOneMillisecond, (IfTypeMotors*0x100+_motorId), deviceId, Parameter ); diff --git a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h index e40ca1ed7..c59481113 100644 --- a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h +++ b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h @@ -122,6 +122,8 @@ uint32_t MotorSetDirection(TimerMotors_t _motorId,bool _direction); uint32_t MotorSetSpeed(TimerMotors_t _motorId, uint32_t _freq); +uint32_t MotorSetMaxSpeed(TimerMotors_t _motorId, uint32_t MaxSpeed); + uint32_t MotorGetSpeed(TimerMotors_t _motorId); uint32_t MotorGetPosition(TimerMotors_t _motorId); @@ -175,7 +177,5 @@ uint32_t MotorGetStatusFromFPGA_Callback(TimerMotors_t _motorId,uint32_t Data); uint32_t MotorMovetoLimitSwitch (TimerMotors_t _motorId,bool direction, uint32_t Freq, uint32_t LimitSwitchId, callback_fptr callback); -uint32_t MotorSetSpeedCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag); -uint32_t MotorMoveToLimitSwitchCallBackFunction(uint32_t IfIndex, bool LimitSwitch); #endif /* DRIVERS_MOTOR_H_ */ diff --git a/Software/Embedded_SW/Embedded/Main.c b/Software/Embedded_SW/Embedded/Main.c index c83164055..01470c3de 100644 --- a/Software/Embedded_SW/Embedded/Main.c +++ b/Software/Embedded_SW/Embedded/Main.c @@ -159,6 +159,7 @@ int main(void) CommunicationTaskInit(); utilsInit(ui32SysClock); + MotorsInit(); // init_graphics(ui32SysClock); // clear(); diff --git a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c index 9dc03601c..5ec7c6b16 100644 --- a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c +++ b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c @@ -26,7 +26,6 @@ Task_Handle AlarmHandling_Task_Handle; Mailbox_Handle AlarmHandlingMsgQ = NULL; -static GateMutex_Handle gateAlarmHandlingDB; /******************** Functions ********************************************/ //uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Read); @@ -57,13 +56,6 @@ void AlarmHandlingInit(void) //memset(AlarmHandlingDatalog,0,sizeof(uint32_t)*MAX_TANGO_CONTROL_DEVICES); - /*gateAlarmHandlingDB = GateMutex_create(NULL, &eb); - if (gateAlarmHandlingDB == NULL) - { - System_abort("Could not create USB Wait gate"); - }*/ - - return; } void AlarmHandlingIterate(UArg arg0) diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c index 847b777fa..a1de29b08 100644 --- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c +++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c @@ -31,7 +31,6 @@ #include "drivers/FPGA/FPGA.h" #include "modules/thread/thread_ex.h" -#include "Drivers/Peripheral_GPIO/GPIO.h" Task_Handle Millisecond_Task_Handle; /******************** Definitions ********************************************/ @@ -237,6 +236,7 @@ uint32_t MillisecLoop(uint32_t tick) FPGA_Read_limit_Switches_Registers(); //Read_FPGA_GPI_Rgisters();//FPGA_Read_limit_Switches(); #endif + temp += MotorActivity; if (MotorActivity) { @@ -340,7 +340,6 @@ uint32_t MillisecLoop(uint32_t tick) { for (Sensor_i = 0;Sensor_i < MAX_TEMPERATURE_SENSOR_ID;Sensor_i++) MillisecReadFromTempSensor(Sensor_i, NULL); - ACTIVITY_RED_LED_CHANGE_MODE; } if (Onesecond_Tick) diff --git a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c index ff9acdb94..69ff06880 100644 --- a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c @@ -486,7 +486,7 @@ uint32_t HeatersControlLoop(uint32_t tick) { //char str[100]; //uint8_t len = 0; - int DcHeaterId,HeaterSliceCounter; + int DcHeaterId; /*len = usnprintf(str, 100, "\r\n EightMilliSecondHeatersInterrupt SliceCounter %d Owner %d H1000 %d H2000 %d" ,SliceCounter,TimeSliceAllocation[SliceCounter],HeatersRestart,NumberOFSlicesInUse); Report(str, __FILE__,__LINE__,0, RpMessage, SliceCounter, TimeSliceAllocation[SliceCounter]); diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c index 6739619a7..abd9794b8 100644 --- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c +++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c @@ -35,6 +35,7 @@ double DispenserNormalizedErrorCoEfficient[MAX_SYSTEM_DISPENSERS] = {0}; HardwarePidControlType ThreadDispenserIdToControlId[MAX_SYSTEM_DISPENSERS] = { HARDWARE_PID_CONTROL_TYPE__Dispenser1,HARDWARE_PID_CONTROL_TYPE__Dispenser2,HARDWARE_PID_CONTROL_TYPE__Dispenser3,HARDWARE_PID_CONTROL_TYPE__Dispenser4,HARDWARE_PID_CONTROL_TYPE__Dispenser5,HARDWARE_PID_CONTROL_TYPE__Dispenser6,HARDWARE_PID_CONTROL_TYPE__Dispenser7,HARDWARE_PID_CONTROL_TYPE__Dispenser8}; +bool DispenserReady[MAX_SYSTEM_DISPENSERS] = {true}; /******************** STRUCTURES AND ENUMs ********************************************/ /******************** GLOBAL PARAMETERS ********************************************/ @@ -83,7 +84,7 @@ uint32_t IDS_DispenserPidRequestMessage(HardwarePidControl* request) //callback - calls printing stm with the result // registration - 10 msec, dispenser pressure sensor -// AddControlCallback(DeviceId2Heater[HeaterId], HeaterControlCBFunction, eHundredMillisecond); +// AddControlCallback(DeviceId2Dispenser[DispenserId], DispenserControlCBFunction, eHundredMillisecond); // start the dispenser pressure building - move up in a TBD speed, valve closed //Dispenser Speed control //callback - handles speed @@ -94,7 +95,7 @@ uint32_t IDS_DispenserPidRequestMessage(HardwarePidControl* request) //******************************************************************************************************************** uint32_t IDSPrepareState(void *JobDetails) { - int Motor_i, HW_Motor_Id, Pid_Id; + int Motor_i, HW_Motor_Id, Pid_Id,i; //start IDS control for all motors for (Motor_i = 0;Motor_i < MAX_SYSTEM_DISPENSERS;Motor_i++) { @@ -117,6 +118,11 @@ uint32_t IDS_DispenserPidRequestMessage(HardwarePidControl* request) MotorSetDirection((TimerMotors_t)HW_Motor_Id,MotorsCfg[HW_Motor_Id].directionthreadwize); //set the dispenser to the #warning activate valve + /*Start the dispensers to build initial pressure + * check different handling for dispensers that participate in the first segment and idle dispensers + * start control for initial pressure + * + */ //ValveCommand (Enable,MixerDirection); #ifdef DEBUG_TEST_FUNCTIONS // add control here @@ -124,16 +130,33 @@ uint32_t IDS_DispenserPidRequestMessage(HardwarePidControl* request) #endif } //set 3 dancers to the profile positions + for (i = 0; i < MAX_SYSTEM_DISPENSERS; i++) + { + DispenserReady[i] = true; + } return OK; } + void DispenserPrepareReady(void) + { + int i; + for (i=0;idata.len, requestContainer->data.data); - MillisecStart(); - ControlStart(); - - //MotorMovetoLimitSwitch (14,1, 1000, 32, MotorSetSpeedCallBackFunction); - MotorSetSpeed( 15, 1000); - //request->cartridgeid + //request->cartridgeid writeLine("cartridge Request: "); diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_I2C.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_I2C.c index 9584ee39b..9202d9936 100644 --- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_I2C.c +++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_I2C.c @@ -119,7 +119,7 @@ void Stub_I2CWriteBytesRequest(MessageContainer* requestContainer) //---------------------- - status = I2C_Write(request->i2cid, request->slaveaddress, I2C_W_buf, request->n_bytestwrite); + status = Write_I2C(request->i2cid, request->slaveaddress, I2C_W_buf, request->n_bytestwrite); //-------------------------- @@ -159,7 +159,7 @@ void Stub_I2CReadBytesRequest(MessageContainer* requestContainer) response.slaveaddress = request->slaveaddress; response.has_slaveaddress = true; - status = I2C_Read(request->i2cid, request->slaveaddress, I2C_R_buf, request->numberofbytestoread); + status = Read_I2C(request->i2cid, request->slaveaddress, I2C_R_buf, request->numberofbytestoread); //response.readbyte = I2C_control(request->i2cid, request->slaveaddress, request->readorwrite,request->bytetowrite ); //response.has_readbyte= true; diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c index 37f60e8cf..f80f26e53 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c @@ -20,65 +20,30 @@ bool Winder_ScrewHoming = false; uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag); uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue); -bool ScrewCurrentDirection = false; -double ScrewSpeed = 0; +bool ScrewCurrentDirection = false; //holds current screw direction +bool BusyfirstCall = true; //Ignores first call after activating the screw - too early +double ScrewSpeed = 0; //ScrewSpeed uint32_t ScrewControlId = 0xFF; +uint32_t ScrewNumberOfSteps = 0; //holds the current number of steps for the next screw run - will be used to build the cone +uint32_t DirectionChangeCounter = 0; //holds the current number of runs of the screw - will be used to build the cone uint32_t Winder_Init(void) { return OK; } -uint32_t Winder_Test2(uint32_t deviceID, uint32_t ReadValue); -uint32_t Winder_Test1(uint32_t deviceID, uint32_t ReadValue) -{ - uint32_t status; - uint32_t numOfSteps = InternalWinderCfg.segmentoffsetpulses; - uint32_t position; - /* - * 1. move home to the limit switch (check that the cart is clear from the limit switch, start moving, with acceleration to maximal speed. enable interrupt on the limit switch, upon interrupt stop. - * 2. move back x steps - according to thehw specifications and bobine definitions in the job. move for a predefined number of steps. get a callback when done - * report ready to the job STM - */ - MotorGetPositionFromFPGA(HARDWARE_MOTOR_TYPE__MOTO_SCREW); - //deley TODO - SysCtlDelay(100000); - position = Fpga_Spi[HARDWARE_MOTOR_TYPE__MOTO_SCREW].RX_MISO;//MotorGetPositionFromFPGA_Res(Motor_Id); - - status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed/10, GPI_LS_SCREW_LEFT, Winder_Test2); -// status |= MotorMove (HARDWARE_MOTOR_TYPE__MOTO_SCREW, numOfSteps, Winder_ScrewAtOffsetCallback); - //set motor location 0 here - return status; - -} -uint32_t Winder_Test2(uint32_t deviceID, uint32_t ReadValue) -{ - uint32_t status; - uint32_t numOfSteps = InternalWinderCfg.segmentoffsetpulses; - uint32_t position; - /* - * 1. move home to the limit switch (check that the cart is clear from the limit switch, start moving, with acceleration to maximal speed. enable interrupt on the limit switch, upon interrupt stop. - * 2. move back x steps - according to thehw specifications and bobine definitions in the job. move for a predefined number of steps. get a callback when done - * report ready to the job STM - */ - MotorGetPositionFromFPGA(HARDWARE_MOTOR_TYPE__MOTO_SCREW); - //deley TODO - SysCtlDelay(100000); - position = Fpga_Spi[HARDWARE_MOTOR_TYPE__MOTO_SCREW].RX_MISO;//MotorGetPositionFromFPGA_Res(Motor_Id); - - status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed/10, GPI_LS_SCREW_RIGHT, Winder_Test1); - //status |= MotorMoveSteps (HARDWARE_MOTOR_TYPE__MOTO_SCREW, numOfSteps, Winder_ScrewAtOffsetCallback); - //set motor location 0 here - return status; +/* + *uint32_t Winder_Prepare(void *JobDetails) -} -uint32_t Winder_Prepare(void *JobDetails) + * 1. move home to the limit switch (check that the cart is clear from the limit switch, start moving, with acceleration to maximal speed. enable interrupt on the limit switch, upon interrupt stop. + * report ready to the job STM + */ +uint32_t Winder_Prepare(void) { uint32_t status = 0; - JobTicket* JobTicket = JobDetails; + //JobTicket* JobTicket = JobDetails; - float process_speed = JobTicket->processparameters->dyeingspeed; -// double ScrewSpeed = (50 * MotorsCfg[SCREW_MOTOR].pulseperround * MotorsCfg[SCREW_MOTOR].microstep)/(2*PI* MotorsCfg[SCREW_MOTOR].pulleyradius); - double ScrewSpeed = (process_speed*20 );//* MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulseperround)/(2*PI* MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulleyradius); + //float process_speed = JobTicket->processparameters->dyeingspeed; + double ScrewSpeed = 500;//(process_speed*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulleyradius); // we will use pulley radius of the screw for this purpose, as of now /* * 1. move home to the limit switch (check that the cart is clear from the limit switch, start moving, with acceleration to maximal speed. enable interrupt on the limit switch, upon interrupt stop. @@ -87,41 +52,67 @@ uint32_t Winder_Prepare(void *JobDetails) */ if (FPGA_Read_limit_Switches(GPI_LS_SCREW_RIGHT)==LIMIT) { - //go to prepare stage 2 Winder_PrepareStage2(0,0); } else { Winder_ScrewHoming = true; - status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed, GPI_LS_SCREW_LEFT, Winder_PrepareStage2); -// status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed/10, GPI_LS_SCREW_RIGHT, Winder_Test1); + status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed, GPI_LS_SCREW_RIGHT, Winder_PrepareStage2); } return status; } +/* + * uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue) + * 2. move back x steps - according to thehw specifications and bobine definitions in the job. move for a predefined number of steps. get a callback when done + * report ready to the job STM + */ + uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue) { uint32_t status=OK; uint32_t numOfSteps = InternalWinderCfg.startoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep; - /* - * 1. move home to the limit switch (check that the cart is clear from the limit switch, start moving, with acceleration to maximal speed. enable interrupt on the limit switch, upon interrupt stop. - * 2. move back x steps - according to thehw specifications and bobine definitions in the job. move for a predefined number of steps. get a callback when done - * report ready to the job STM - */ - //status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,(1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize));//make sur to move the cart out - //status |= StopMotor(HARDWARE_MOTOR_TYPE__MOTO_SCREW,0); - status |= MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_SCREW, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize),numOfSteps, Winder_ScrewAtOffsetCallback); - //status |= MotorMoveSteps (HARDWARE_MOTOR_TYPE__MOTO_SCREW, numOfSteps, Winder_ScrewAtOffsetCallback); - //set motor location 0 here + + status |= MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_SCREW, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize),numOfSteps, Winder_ScrewAtOffsetCallback); + //set motor location 0 here return status; } -uint32_t WinderPresegmentReady(uint32_t deviceID, uint32_t ReadValue) +uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag) { - return PreSegmentReady(Module_Winder,ModuleDone); + SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_SCREW); //set this point as the spool home + + ScrewCurrentDirection = false; + BusyfirstCall = true; + ScrewSpeed = 0; + ScrewControlId = 0xFF; + ScrewNumberOfSteps = 0; + DirectionChangeCounter = 0; + + PrepareReady(Module_Winder, ModuleDone); + return OK; } + +/* + * this is the main operational function of the screw - run back and forth until stopped + */ +/* + +bool ScrewCurrentDirection = false; +bool BusyfirstCall = true; +double ScrewSpeed = 0; +uint32_t ScrewControlId = 0xFF; +uint32_t ScrewNumberOfSteps = 0; +uint32_t DirectionChangeCounter = 0; +*/ + uint32_t ScrewDirectionChange(uint32_t deviceID, uint32_t BusyFlag) { + if (BusyfirstCall) + { + BusyfirstCall = false; + return; + } //ScrewCurrentDirection: false moves out, true moves home if (BusyFlag == NOTBUSY) { @@ -133,11 +124,25 @@ uint32_t ScrewDirectionChange(uint32_t deviceID, uint32_t BusyFlag) { ScrewCurrentDirection = false; } - MotorMove (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewCurrentDirection,InternalWinderCfg.segmentoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep); //process: set point 0, set max speed, move to the specified length, return back. + MotorMove (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewCurrentDirection,ScrewNumberOfSteps); //process: set point 0, set max speed, move to the specified length, return back. + DirectionChangeCounter++; } +/* + * calculate new ScrewSpeed and call MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed); + * + */ + /* + * calculate cone shape according to DirectionChangeCounter and update ScrewNumberOfSteps + */ return OK; } + +uint32_t WinderPresegmentReady(uint32_t deviceID, uint32_t ReadValue) +{ + return PreSegmentReady(Module_Winder,ModuleDone); +} + uint32_t Winder_Presegment(void *JobDetails) { JobTicket* JobTicket = JobDetails; @@ -146,35 +151,26 @@ uint32_t Winder_Presegment(void *JobDetails) float RotationsPerSecond; ScrewCurrentDirection = (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize); -// double ScrewSpeed = (process_speed * MotorsCfg[SCREW_MOTOR].pulseperround * MotorsCfg[SCREW_MOTOR].microstep)/(2*PI* MotorsCfg[SCREW_MOTOR].pulleyradius); - //double ScrewSpeed = (process_speed * MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulseperround)/(2*PI* MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulleyradius); -/*typedef struct -{ - uint32_t startoffsetpulses; - uint32_t spoolbackingrate; - uint32_t segmentoffsetpulses; - uint32_t milimetersperrotation; - int32 SpoolBottomBackingRate;// the angle of the bottom of the spool - double NumberOfRotationPerPassage; // how many rotations per spool passage -}InternalWinderConfigStruc; - * */ -// * 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. + // * 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 * 3.1416); + 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. - screw_speed = screw_speed*RotationsPerSecond; + 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 - MotorMove (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewCurrentDirection,InternalWinderCfg.segmentoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep); //process: set point 0, set max speed, move to the specified length, return back. - ScrewControlId = AddControlCallback(ScrewDirectionChange, eTenMilliSecond,MotorControlGetnBusyState,(IfTypeMotors*0x100+HARDWARE_MOTOR_TYPE__MOTO_SCREW), HARDWARE_MOTOR_TYPE__MOTO_SCREW, 0); + 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; @@ -185,6 +181,7 @@ uint32_t Winder_Presegment(void *JobDetails) uint32_t Winder_End(void) { //stop screw + ScrewNumberOfSteps = 0; RemoveControlCallback(ScrewControlId,ScrewDirectionChange); return StopMotor (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz); } @@ -202,10 +199,4 @@ void Winder_ScrewOutLimitSwitchInterrupt(void) uint32_t status; status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);//make sure to move the cart out } -uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag) -{ - SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_SCREW); //set this point as the spool home - PrepareReady(Module_Winder, ModuleDone); - return OK; -} diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c index fdae44171..7b5da0e65 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -165,7 +165,7 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) totalLength+=length; } -#warning control disabled +//#warning control disabled CurrentProcessedLength+=length; PosDif[MotorDataIndex] = CurrentPosition; @@ -297,7 +297,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) avreageSampleValue += MotorSamples[index][i]; avreageSampleValue = avreageSampleValue / MotorsControl[index].pvinputfilterfactormode; //Stop Execution if the dancer moves too much -#warning need to disable this in the first second of the job execution + if ((abs(avreageSampleValue)> DancerStopActivityLimit[index])&&(JobCounter > eOneSecond)) { EndState(CurrentJob); diff --git a/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c b/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c index 09334dc8e..f62bfba26 100644 --- a/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c +++ b/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c @@ -133,30 +133,41 @@ static ReturnCode PrepareState(void *JobDetails) ReturnCode retcode; retcode = JobSuccess; //start (fast??) heating - //wait for fast heating to end - //start other peripheral systems: chiller, waist handling - //check thread type - if (Configured[Module_Winder]) - { - // PrepareWaiting[Module_Winder] = ModuleWaiting; - Winder_Prepare(); - } if (Configured[Module_IDS]) { //PrepareWaiting[Module_IDS] = ModuleWaiting; - //IDSPrepareState(JobDetails); } if (Configured[Module_Heaters]) { PrepareWaiting[Module_Heaters] = ModuleWaiting; - //heaters preparation starts on process parameters handling - //IDSPrepareState(JobDetails); } if (Configured[Module_Thread]) { PrepareWaiting[Module_Thread] = ModuleWaiting; + } + if (Configured[Module_Winder]) + { + PrepareWaiting[Module_Winder] = ModuleWaiting; + } + //wait for fast heating to end + //start other peripheral systems: chiller, waist handling + //check thread type + if (Configured[Module_IDS]) + { + //IDSPrepareState(JobDetails); + } +/* if (Configured[Module_Heaters]) + { + //heaters preparation starts on process parameters handling + }*/ + if (Configured[Module_Thread]) + { ThreadPrepareState(CurrentJob); } + if (Configured[Module_Winder]) + { + Winder_Prepare(JobDetails); + } return retcode; } @@ -255,7 +266,7 @@ void JobAbortFunc(MessageContainer* requestContainer) AbortJobRequest* request = abort_job_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data); EndState(CurrentJob); - AbortJob(); + //AbortJob(); AbortJobResponse response = ABORT_JOB_RESPONSE__INIT; responseContainer = createContainer(MESSAGE_TYPE__AbortJobResponse, JobToken, false, &response, &abort_job_response__pack, &abort_job_response__get_packed_size); container_buffer = malloc(message_container__get_packed_size(&responseContainer)); diff --git a/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.c b/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.c index c93c85df0..57f3ff5a3 100644 --- a/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.c +++ b/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.c @@ -19,6 +19,7 @@ #include "include.h" #include "./printingSTM.h" #include "modules/thread/thread_ex.h" +#include "modules/Heaters/Heaters_ex.h" #include "modules/ids/ids_ex.h" #include "PMR/Hardware/UploadHardWareConfigurationRequest.pb-c.h" #include "PMR/Hardware/HardwareMotorType.pb-c.h" @@ -173,16 +174,25 @@ static uint32_t PreSegmentState(void *JobDetails, int SegmentId) if (Configured[Module_Thread]) { PreSegmentWaiting[Module_Thread] = ModuleWaiting; - ThreadPreSegmentState(JobDetails); } if (Configured[Module_Winder]) { PreSegmentWaiting[Module_Winder] = ModuleWaiting; - Winder_Presegment(JobDetails); } if (Configured[Module_IDS]) { PreSegmentWaiting[Module_IDS] = ModuleWaiting; + } + if (Configured[Module_Thread]) + { + ThreadPreSegmentState(JobDetails); + } + if (Configured[Module_Winder]) + { + Winder_Presegment(JobDetails); + } + if (Configured[Module_IDS]) + { IDSPreSegmentState(JobDetails,SegmentId); } return OK; @@ -247,6 +257,8 @@ static uint32_t SegmentState(void *JobDetails, int SegmentId) //******************************************************************************************************************** uint32_t EndState(void *JobDetails) { + SendJobProgress(100,0,true); + if (Configured[Module_Winder]) { // EndWaiting[Module_Winder] = ModuleWaiting; -- cgit v1.3.1 From 8ea30fe7354c7d878c5ff718b7e52a04178cf55b Mon Sep 17 00:00:00 2001 From: Shlomo Hecht Date: Sun, 17 Jun 2018 14:16:18 +0300 Subject: MotorStop, improve IDS, Dispensers pressure --- .../Embedded/Drivers/ADC_Sampling/ADC.c | 1 + .../Embedded/Drivers/ADC_Sampling/ADC.h | 3 ++ .../Embedded_SW/Embedded/Drivers/Motors/Motor.c | 31 ++++++++++-------- .../Embedded_SW/Embedded/Drivers/Motors/Motor.h | 2 +- .../Embedded/Modules/Control/MillisecTask.c | 15 ++++++--- Software/Embedded_SW/Embedded/Modules/IDS/IDS.h | 1 + Software/Embedded_SW/Embedded/Modules/IDS/IDS_ex.h | 1 + .../Embedded_SW/Embedded/Modules/IDS/IDS_init.c | 17 ++++++++++ .../Embedded_SW/Embedded/Modules/IDS/IDS_print.c | 38 ++++++++++++++-------- .../Embedded/Modules/Stubs_Handler/Stub_Motor.c | 2 +- .../Embedded/Modules/Thread/Thread_Winder.c | 4 +-- .../Embedded/Modules/Thread/Thread_print.c | 8 +++-- .../Embedded/StateMachines/Printing/JobSTM.c | 2 +- .../Embedded/StateMachines/Printing/PrintingSTM.c | 6 ++-- 14 files changed, 89 insertions(+), 42 deletions(-) (limited to 'Software/Embedded_SW/Embedded/Modules/Thread') diff --git a/Software/Embedded_SW/Embedded/Drivers/ADC_Sampling/ADC.c b/Software/Embedded_SW/Embedded/Drivers/ADC_Sampling/ADC.c index f07bd790e..9659e5ff5 100644 --- a/Software/Embedded_SW/Embedded/Drivers/ADC_Sampling/ADC.c +++ b/Software/Embedded_SW/Embedded/Drivers/ADC_Sampling/ADC.c @@ -154,6 +154,7 @@ uint32_t g_pui32ADCSeq[MAX_ADC_DEVICES] = //#define NUM_ADC_CHANNELS (sizeof(g_pui32ADCSeq) / \ sizeof(g_pui32ADCSeq[0])) +int DispenserIdToPressureSensorId[MAX_DISPENSERS] = {CHAN_DISPENSE_PRESSURE_1, CHAN_DISPENSE_PRESSURE_2 , CHAN_DISPENSE_PRESSURE_3 , CHAN_DISPENSE_PRESSURE_4 , CHAN_DISPENSE_PRESSURE_5 , CHAN_DISPENSE_PRESSURE_6 , CHAN_DISPENSE_PRESSURE_7 , CHAN_DISPENSE_PRESSURE_8 }; //const uint8_t Num_ADC_Channels = ( sizeof(g_pui32ADCSeq) / sizeof(g_pui32ADCSeq[0]) ); diff --git a/Software/Embedded_SW/Embedded/Drivers/ADC_Sampling/ADC.h b/Software/Embedded_SW/Embedded/Drivers/ADC_Sampling/ADC.h index 87f776c89..b667025e5 100644 --- a/Software/Embedded_SW/Embedded/Drivers/ADC_Sampling/ADC.h +++ b/Software/Embedded_SW/Embedded/Drivers/ADC_Sampling/ADC.h @@ -12,10 +12,13 @@ //***************************************************************************** #define MAX_ADC_DEVICES 20 +#define MAX_DISPENSERS + //#define MAX_ADC_DEVICES NUM_ADC_CHANNELS #define MAX_DIGITAL_POTENTIOMETER_READ 0x660D +extern int DispenserIdToPressureSensorId[MAX_DISPENSERS]; typedef void (*ProcessCallback)(uint32_t* adcData); void ADCAcquireInit(void); diff --git a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c index eb2f25100..761e2e935 100644 --- a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c +++ b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c @@ -287,7 +287,7 @@ bool MotorControlGetnBusyState(uint32_t _motorId, uint32_t parameter) /* * Stop Command */ -uint32_t StopMotor(TimerMotors_t _motorId, STOP_TYPE_ENUM StopType) +uint32_t MotorStop(TimerMotors_t _motorId, STOP_TYPE_ENUM StopType) { #ifdef EVALUATION_BOARD Stop_Command = StopType; @@ -342,7 +342,7 @@ uint32_t MotorMoveWithCallback (TimerMotors_t _motorId,bool direction, uint32_t //SetMotorSpeed (deviceId, parameter); MotorMoveModuleCallback = callback; - StopMotor(_motorId,2 ); + MotorStop(_motorId,2 ); MotorMove(_motorId,direction,Steps ); MotorMoveControlId = AddControlCallback( MotorMoveCallBackFunction, eTenMilliSecond, MotorControlGetnBusyState,(IfTypeMotors*0x100+_motorId), _motorId, 0 ); return MotorMoveControlId; @@ -416,20 +416,23 @@ uint32_t MotorMoveToLimitSwitchCallBackFunction(uint32_t IfIndex, bool LimitSwit } MotorId = IfIndex&0xFF; - if (LimitSwitch == LIMIT) - { - //stop this control loop - RemoveControlCallback(MotorMovetoLimitSwitchControlId[MotorId], MotorMoveToLimitSwitchCallBackFunction ); - StopMotor(MotorId,Hard_Hiz); //TODO in run time limit switch just reverse direction + if (MotorMovetoLimitSwitchControlId[MotorId] == 0xFF) + return ERROR; + if (LimitSwitch == LIMIT) + { + //stop this control loop + RemoveControlCallback(MotorMovetoLimitSwitchControlId[MotorId], MotorMoveToLimitSwitchCallBackFunction ); + MotorMovetoLimitSwitchControlId[MotorId] = 0xFF; + MotorStop(MotorId,Hard_Hiz); //TODO in run time limit switch just reverse direction - //possibly: start regular control (speed etc) - //uint32_t ControlId = AddControlCallback(ControlCBFunction Callback, eOneMillisecond, (IfTypeMotors*0x100+_motorId), deviceId, Parameter ); + //possibly: start regular control (speed etc) + //uint32_t ControlId = AddControlCallback(ControlCBFunction Callback, eOneMillisecond, (IfTypeMotors*0x100+_motorId), deviceId, Parameter ); - //call the module callback - if (MotorMovetoLimitSwitchCallback) - MotorMovetoLimitSwitchCallback(MotorId,LimitSwitch); + //call the module callback + if (MotorMovetoLimitSwitchCallback) + MotorMovetoLimitSwitchCallback(MotorId,LimitSwitch); - } + } return OK; } uint32_t MotorMovetoLimitSwitch (TimerMotors_t _motorId,bool direction, uint32_t Freq, uint32_t LimitSwitchId, callback_fptr callback) //TODO @@ -442,6 +445,8 @@ uint32_t MotorMovetoLimitSwitch (TimerMotors_t _motorId,bool direction, uint32_t MotorSetDirection( _motorId, direction); MotorSetSpeed(_motorId, Freq); + if (MotorMovetoLimitSwitchControlId[_motorId] != 0xFF) + return ERROR; MotorMovetoLimitSwitchControlId[_motorId] = AddControlCallback( MotorMoveToLimitSwitchCallBackFunction, eOneMillisecond , FPGA_Read_limit_Switches,(IfTypeMotors*0x100+_motorId), LimitSwitchId, 0 ); MotorMovetoLimitSwitchCallback = callback; diff --git a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h index c59481113..b7e11d21d 100644 --- a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h +++ b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h @@ -160,7 +160,7 @@ typedef enum Soft_Hiz, }STOP_TYPE_ENUM; -uint32_t StopMotor(TimerMotors_t _motorId, STOP_TYPE_ENUM StopType); +uint32_t MotorStop(TimerMotors_t _motorId, STOP_TYPE_ENUM StopType); uint32_t MotorMove(TimerMotors_t _motorId,bool direction, uint32_t Steps); diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c index a1de29b08..312f970d6 100644 --- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c +++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c @@ -31,6 +31,7 @@ #include "drivers/FPGA/FPGA.h" #include "modules/thread/thread_ex.h" +#include "modules/ids/ids_ex.h" Task_Handle Millisecond_Task_Handle; /******************** Definitions ********************************************/ @@ -134,7 +135,7 @@ void MillisecStart(void) ADCAcquireStart(0,1); } - +uint32_t msec_millisecondCounter = 0; void OneMilliSecondMillisecInterrupt(UArg arg0) { MillisecMessageStruc Message; @@ -148,7 +149,7 @@ void OneMilliSecondMillisecInterrupt(UArg arg0) //send message to the Millisec task Message.messageId = OneMillisec; - Message.tick = UsersysTickGet(); + Message.tick = msec_millisecondCounter++;; Message.msglen = sizeof(MillisecMessageStruc); if (MillisecMsgQ != NULL) Mailbox_post(MillisecMsgQ , &Message, BIOS_NO_WAIT); @@ -226,9 +227,10 @@ uint32_t MillisecLoop(uint32_t tick) //call all modules Millisec functions //test dancers and speed encoders //check all callback units (state machine waiting for completion of a change) - bool Ten_msTick, Hundred_msTick, Onesecond_Tick; + bool Ten_msTick, Hundred_msTick, Onesecond_Tick,O900Millisecond_Tick; Ten_msTick = (tick%eTenMilliSecond == 0) ?true:false; Hundred_msTick = (tick%eHundredMillisecond == 0) ?true:false; + O900Millisecond_Tick = (tick%eOneSecond == 900) ?true:false; Onesecond_Tick = (tick%eOneSecond == 0) ?true:false; //gather Motor data from FPGA #ifndef EVALUATION_BOARD @@ -342,10 +344,15 @@ uint32_t MillisecLoop(uint32_t tick) MillisecReadFromTempSensor(Sensor_i, NULL); } - if (Onesecond_Tick) + if (O900Millisecond_Tick) { ADC_TriggerCollection(); } + if (Onesecond_Tick) + { + for (Sensor_i = 0;Sensor_i < MAX_SYSTEM_DISPENSERS;Sensor_i++) + CalculateDispenserPressure(Sensor_i); + } /* OLD CODE MotorStatus_Data[MOTOR_DRYER_DRIVING] = MotorGetStatusFromFPGA(MOTOR_DRYER_DRIVING); diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS.h b/Software/Embedded_SW/Embedded/Modules/IDS/IDS.h index 2c3910140..c3499454d 100644 --- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS.h +++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS.h @@ -6,6 +6,7 @@ #include "ids_ex.h" extern uint32_t DispenserIdToMotorId[MAX_SYSTEM_DISPENSERS]; +extern float DispenserPressure[MAX_SYSTEM_DISPENSERS]; uint32_t DispenserConfigMessage(HardwareDispenser * request); extern HardwareDispenser DispensersCfg[ MAX_SYSTEM_DISPENSERS]; diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_ex.h b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_ex.h index feb38000f..bc9e11749 100644 --- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_ex.h +++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_ex.h @@ -17,5 +17,6 @@ uint32_t IDSSegmentState(void *JobDetails, int SegmentId); uint32_t IDS_DispenserPidRequestMessage(HardwarePidControl* request); +float CalculateDispenserPressure (int DispenserId); #endif /* MODULES_IDS_IDS_EX_H_ */ diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_init.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_init.c index d53ab3d60..81efa5662 100644 --- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_init.c +++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_init.c @@ -12,6 +12,7 @@ #include "PMR/Printing/JobSpoolType.pb-c.h" #include "drivers/Motors/Motor.h" +#include "drivers/ADC_Sampling/adc.h" #define MAX_CONTROL_SAMPLES 10 @@ -19,6 +20,22 @@ HardwareDispenser DispensersCfg[ MAX_SYSTEM_DISPENSERS]; uint32_t DispenserIdToMotorId[MAX_SYSTEM_DISPENSERS] = {HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1,HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_2,HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_3,HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_4,HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_5,HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_6,HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_7,HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_8}; +float DispenserPressure[MAX_SYSTEM_DISPENSERS] = {0}; + +float CalculateDispenserPressure (int DispenserId) +{ + unsigned short data = 0; + float temp= 0.0; + + data = ADC_GetReading(DispenserIdToPressureSensorId[DispenserId]); + temp = 4096 - data; + temp *= 3; + temp *= 1000; //move to mv + temp /= 4096; + DispenserPressure[DispenserId] = 3000 - temp; + return( DispenserPressure[DispenserId]); + +} uint32_t DispenserConfigMessage(HardwareDispenser * request) diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c index abd9794b8..1c9323254 100644 --- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c +++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c @@ -159,24 +159,16 @@ uint32_t IDSPreSegmentState(void *JobDetails, int SegmentId) TimerMotors_t HW_Motor_Id; int segmentfirst_speed; int CurrentSegment = 0; - JobBrushStopId = 0; - - for (Dispenser_i = 0;Dispenser_i <= MAX_SYSTEM_DISPENSERS;Dispenser_i++) - { - HW_Motor_Id = DispenserIdToMotorId[Dispenser_i]; - //(Speed*uStep*PPR)/((2*PI*Dispenser_Radius) - segmentfirst_speed = JobTicket->segments[CurrentSegment]->brushstops[JobBrushStopId]->dispensers[Dispenser_i]->pulsepersecond; - double Dispenser_speed = (segmentfirst_speed * MotorsCfg[HW_Motor_Id].pulseperround)/(2*PI* MotorsCfg[HW_Motor_Id].pulleyradius); - //DispenserControlConfig[Dispenser_i].m_SetParam = Dispenser_speed; - OriginalDispenserSpd_2PPS[Dispenser_i] = (int)Dispenser_speed; - } - //ControlStart(); - MotorSetSpeed(HW_Motor_Id, OriginalDispenserSpd_2PPS[HW_Motor_Id]); // activate control fr all motors /* wait for all dispensers to get to the required pressure * move the presegment ready when all dispensers are ready. */ + for (Dispenser_i = 0;Dispenser_i <= MAX_SYSTEM_DISPENSERS;Dispenser_i++) + { + HW_Motor_Id = DispenserIdToMotorId[Dispenser_i]; + MotorStop(HW_Motor_Id,Hard_Stop); + } //call the job state machine when the thread system is ready PreSegmentReady(Module_IDS,ModuleDone); @@ -186,6 +178,24 @@ int CurrentSegment = 0; //******************************************************************************************************************** uint32_t IDSSegmentState(void *JobDetails, int SegmentId) { + JobTicket* JobTicket = JobDetails; + int Dispenser_i; + TimerMotors_t HW_Motor_Id; + int segmentfirst_speed; + int CurrentSegment = 0; + + JobBrushStopId = 0; + + for (Dispenser_i = 0;Dispenser_i <= MAX_SYSTEM_DISPENSERS;Dispenser_i++) + { + HW_Motor_Id = DispenserIdToMotorId[Dispenser_i]; + //(Speed*uStep*PPR)/((2*PI*Dispenser_Radius) + segmentfirst_speed = JobTicket->segments[CurrentSegment]->brushstops[JobBrushStopId]->dispensers[Dispenser_i]->pulsepersecond; +// double Dispenser_speed = (segmentfirst_speed * MotorsCfg[HW_Motor_Id].pulseperround)/(2*PI* MotorsCfg[HW_Motor_Id].pulleyradius); + //DispenserControlConfig[Dispenser_i].m_SetParam = Dispenser_speed; +// OriginalDispenserSpd_2PPS[Dispenser_i] = (int)Dispenser_speed; + MotorSetSpeed(HW_Motor_Id, segmentfirst_speed); + } return OK; } @@ -195,7 +205,7 @@ int CurrentSegment = 0; int Motor_i; for ( Motor_i = 0;Motor_i < MAX_SYSTEM_DISPENSERS;Motor_i++) { - StopMotor(DispenserIdToMotorId[Motor_i],Hard_Hiz); + MotorStop(DispenserIdToMotorId[Motor_i],Hard_Hiz); } return OK; diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.c index 01c91c72b..cef0e1270 100644 --- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.c +++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.c @@ -294,7 +294,7 @@ void Stub_MotorStopRequest(MessageContainer* requestContainer) Mot_Stop(); #else TimerMotors_t Motor_Id = (TimerMotors_t)request->motor_id; //to remove warning - StopMotor(Motor_Id, Stop_Command); + MotorStop(Motor_Id, Stop_Command); #endif diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c index f80f26e53..e99d3df0b 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c @@ -183,14 +183,14 @@ uint32_t Winder_End(void) //stop screw ScrewNumberOfSteps = 0; RemoveControlCallback(ScrewControlId,ScrewDirectionChange); - return StopMotor (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz); + return MotorStop (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz); } void Winder_ScrewHomeLimitSwitchInterrupt(void) { uint32_t status; if (Winder_ScrewHoming) { - StopMotor(HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Stop); //stop ASAP + MotorStop(HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Stop); //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 7b5da0e65..e9cc3ddde 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -37,6 +37,7 @@ HardwareDancerType ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM] = {FEEDER_DANC uint32_t ControlIdtoMotorId [MAX_THREAD_MOTORS_NUM] = {0xFF}; uint32_t SpeedControlId=0xFF; +double DancerError[NUM_OF_DANCERS] = {0.0}; int OriginalMotorSpd_2PPS[MAX_THREAD_MOTORS_NUM] = {0}; uint32_t JobCounter = 0; typedef struct @@ -304,6 +305,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) } NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index]; MotorControlConfig[index].m_mesuredParam = NormalizedError; + DancerError[DancerId] = NormalizedError; 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 @@ -544,10 +546,10 @@ uint32_t ThreadSegmentState(void *JobDetails, int SegmentId) { RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction); } - StopMotor(ThreadMotorIdToMotorId[Motor_i],Hard_Hiz); + MotorStop(ThreadMotorIdToMotorId[Motor_i],Hard_Hiz); } - StopMotor(HARDWARE_MOTOR_TYPE__MOTO_RLOADING,Hard_Hiz); - StopMotor(HARDWARE_MOTOR_TYPE__MOTO_LLOADING,Hard_Hiz); + MotorStop(HARDWARE_MOTOR_TYPE__MOTO_RLOADING,Hard_Hiz); + MotorStop(HARDWARE_MOTOR_TYPE__MOTO_LLOADING,Hard_Hiz); return OK; } diff --git a/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c b/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c index f62bfba26..1e862128f 100644 --- a/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c +++ b/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c @@ -166,7 +166,7 @@ static ReturnCode PrepareState(void *JobDetails) } if (Configured[Module_Winder]) { - Winder_Prepare(JobDetails); + Winder_Prepare(); } return retcode; diff --git a/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.c b/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.c index 57f3ff5a3..c6ea98ba3 100644 --- a/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.c +++ b/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.c @@ -267,17 +267,17 @@ uint32_t EndState(void *JobDetails) if (Configured[Module_IDS]) { //EndWaiting[Module_IDS] = ModuleWaiting; - //IDSEndState(JobDetails); + IDSEndState(JobDetails); } if (Configured[Module_Heaters]) { - EndWaiting[Module_Heaters] = ModuleWaiting; + //EndWaiting[Module_Heaters] = ModuleWaiting; //heaters preparation starts on process parameters handling HeatersEnd(); } if (Configured[Module_Thread]) { - EndWaiting[Module_Thread] = ModuleWaiting; + //EndWaiting[Module_Thread] = ModuleWaiting; ThreadEndState(CurrentJob); } -- cgit v1.3.1 From 3640afb74b3e7b3e232f56a14104866690495c9a Mon Sep 17 00:00:00 2001 From: Shlomo Hecht Date: Mon, 18 Jun 2018 12:31:20 +0300 Subject: Motor Control Fixed, new winder works, CurrentJob holds the job request message --- .../Embedded_SW/Embedded/Drivers/Motors/Motor.c | 51 +++++++----- .../Embedded/Modules/AlarmHandling/AlarmHandling.c | 94 ++++++++++++++++++++++ .../Embedded/Modules/AlarmHandling/AlarmHandling.h | 2 +- .../Embedded/Modules/Control/MillisecTask.c | 2 + .../Embedded/Modules/Thread/Thread_Winder.c | 5 +- .../Embedded/StateMachines/Printing/JobSTM.c | 21 ++++- 6 files changed, 149 insertions(+), 26 deletions(-) (limited to 'Software/Embedded_SW/Embedded/Modules/Thread') diff --git a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c index 9dce895b9..2f4afed73 100644 --- a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c +++ b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c @@ -48,21 +48,22 @@ MotorDriverRequestStruct MotorDriverRequest[NUM_OF_MOTORS]; //MotorDriverConfigStruc MotorsCfg[NUM_OF_MOTORS]; TimerMotors_t FastMotorToMotorId[4] = {HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,HARDWARE_MOTOR_TYPE__MOTO_LDRIVING,HARDWARE_MOTOR_TYPE__MOTO_WINDER}; -callback_fptr MotorMoveModuleCallback = 0; -callback_fptr MotorSetSpeedModuleCallback = 0; -callback_fptr MotorMovetoLimitSwitchCallback = 0; +callback_fptr MotorMoveModuleCallback[NUM_OF_MOTORS]; +callback_fptr MotorSetSpeedModuleCallback[NUM_OF_MOTORS]; +callback_fptr MotorMovetoLimitSwitchCallback[NUM_OF_MOTORS]; uint32_t ControlCallBackFunction(uint32_t deviceID, uint32_t ReadValue); uint32_t MotorMoveCallBackFunction(uint32_t deviceId, uint32_t BusyFlag); uint32_t MotorSetSpeedCallBackFunction(uint32_t deviceId, uint32_t BusyFlag); -uint32_t MotorMoveControlId = 0; -uint32_t MotorSetSpeedControlId = 0; -uint32_t MotorMovetoLimitSwitchControlId[NUM_OF_MOTORS] = {0}; +uint32_t MotorMoveControlId[NUM_OF_MOTORS]; +uint32_t MotorSetSpeedControlId[NUM_OF_MOTORS]; +uint32_t MotorMovetoLimitSwitchControlId[NUM_OF_MOTORS]; //initialize general motors driver database uint32_t MotorsInit(void) { + int i = 0; #ifdef EVALUATION_BOARD //MOTOR Driver L6470 SPI SPI2_Init(); @@ -76,6 +77,16 @@ uint32_t MotorsInit(void) #else FPGA_SetMotorsInit(); #endif + for (i=0;i +#include +#include +#include + +char DiagnosticsToken[36] = {0}; +bool DiagnosticsActive = false; + Task_Handle AlarmHandling_Task_Handle; Mailbox_Handle AlarmHandlingMsgQ = NULL; @@ -33,6 +41,7 @@ Mailbox_Handle AlarmHandlingMsgQ = NULL; typedef enum { AlarmHandlingTrigger, + AlarmHandlingSendDiagnostics }AlarmHandlingMessages; typedef struct AlarmHandlingMessage{ @@ -44,6 +53,42 @@ typedef struct AlarmHandlingMessage{ /******************** CODE ********************************************/ //********************************************************************** +uint32_t StartDiagnosticsRequestFunc(MessageContainer* requestContainer) +{ + uint32_t status = NOT_SUPPORTED; + //MessageContainer responseContainer; + +// ReportInitParams InitParams; + + StartDiagnosticsRequest* request = start_diagnostics_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data); + strcpy (DiagnosticsToken, requestContainer->token); + DiagnosticsActive = true; + + start_diagnostics_request__free_unpacked(request,NULL); + return status; +} +uint32_t StopDiagnosticsRequestFunc(MessageContainer* requestContainer) +{ + + MessageContainer responseContainer; + + StopDiagnosticsRequest* request = stop_diagnostics_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data); + + StopDiagnosticsResponse response = STOP_DIAGNOSTICS_RESPONSE__INIT; + + DiagnosticsActive = false; + + responseContainer = createContainer(MESSAGE_TYPE__StopDiagnosticsResponse, requestContainer->token, false, &response, &stop_diagnostics_response__pack, &stop_diagnostics_response__get_packed_size); + responseContainer.continuous = false; + uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer)); + size_t container_size = message_container__pack(&responseContainer, container_buffer); + free(responseContainer.data.data); + //USBCDCD_sendData(container_buffer, container_size,10); + SendChars(container_buffer, container_size); + //free (container_buffer); + +return OK; +} void AlarmHandlingInit(void) { @@ -72,11 +117,58 @@ void AlarmHandlingIterate(UArg arg0) return ; } +void AlarmHandler_SendDiagnostics(void) +{ + AlarmHandlingMessageStruc Message; + //send message to the Millisec task + Message.messageId = AlarmHandlingSendDiagnostics; + Message.tick = UsersysTickGet(); + Message.msglen = sizeof(AlarmHandlingMessageStruc); + if (AlarmHandlingMsgQ != NULL) + Mailbox_post(AlarmHandlingMsgQ , &Message, BIOS_NO_WAIT); + + + return ; +} uint32_t AlarmHandlingLoop(uint32_t tick) { return OK; } +void SendDiagnostics(void) +{ + MessageContainer responseContainer; + + StartDiagnosticsResponse response = START_DIAGNOSTICS_RESPONSE__INIT; + + if (DiagnosticsActive == false) + return; + + /*DiagnosticsMonitors *monitors; + size_t n_digitalpins; + DigitalPin **digitalpins; + extern bool FPGA_Gpi_Buf[MAX_GPI]; + +void Read_FPGA_GPI_Rgisters(); +bool FPGA_Read_limit_Switches(FPGA_GPI_ENUM Limit_Switch); + + size_t n_componentsstates; + ValueComponentState **componentsstates; + size_t n_events; + Event **events;*/ + + + responseContainer = createContainer(MESSAGE_TYPE__StartDiagnosticsResponse, DiagnosticsToken, false, &response, &start_diagnostics_response__pack, &start_diagnostics_response__get_packed_size); + responseContainer.continuous = true; + + uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer)); + size_t container_size = message_container__pack(&responseContainer, container_buffer); + free(responseContainer.data.data); + //USBCDCD_sendData(container_buffer, container_size,10); +// SendChars(container_buffer, container_size); + //free (container_buffer); + +} /****************************************************************************** * ======== messageTsk ======== * Task for this function is created statically. See the project's .cfg file. @@ -99,6 +191,8 @@ void AlarmHandlingTask(UArg arg0, UArg arg1) case AlarmHandlingTrigger: AlarmHandlingLoop(Message.tick); break; + case AlarmHandlingSendDiagnostics: + SendDiagnostics(); default: break; } diff --git a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.h b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.h index c5eb88660..9e1be07e2 100644 --- a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.h +++ b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.h @@ -9,7 +9,7 @@ #define MODULES_ALARMHANDLING_ALARMHANDLING_H_ - +void AlarmHandler_SendDiagnostics(void); #endif /* MODULES_ALARMHANDLING_ALARMHANDLING_H_ */ diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c index 312f970d6..67de0271f 100644 --- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c +++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c @@ -16,6 +16,7 @@ /////////////////////////////////////////////////////////////////////////////////////////// #include "include.h" #include "Modules/General/GeneralHardware.h" +#include "Modules/AlarmHandling/AlarmHandling.h" #include "MillisecTask.h" #include @@ -342,6 +343,7 @@ uint32_t MillisecLoop(uint32_t tick) { for (Sensor_i = 0;Sensor_i < MAX_TEMPERATURE_SENSOR_ID;Sensor_i++) MillisecReadFromTempSensor(Sensor_i, NULL); + AlarmHandler_SendDiagnostics(); } if (O900Millisecond_Tick) diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c index e99d3df0b..3505ff3c6 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c @@ -111,7 +111,7 @@ uint32_t ScrewDirectionChange(uint32_t deviceID, uint32_t BusyFlag) if (BusyfirstCall) { BusyfirstCall = false; - return; + return OK; } //ScrewCurrentDirection: false moves out, true moves home if (BusyFlag == NOTBUSY) @@ -182,7 +182,8 @@ uint32_t Winder_End(void) { //stop screw ScrewNumberOfSteps = 0; - RemoveControlCallback(ScrewControlId,ScrewDirectionChange); + if (ScrewControlId != 0xFF) + RemoveControlCallback(ScrewControlId,ScrewDirectionChange); return MotorStop (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz); } void Winder_ScrewHomeLimitSwitchInterrupt(void) diff --git a/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c b/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c index 1e862128f..e3405689e 100644 --- a/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c +++ b/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c @@ -54,8 +54,9 @@ Mailbox_Handle JobmsgQ = NULL; #define MAX_TICKET_SIZE 10000 -char CurrentJobBuffer[MAX_TICKET_SIZE]; -JobTicket *CurrentJob = (JobTicket *)CurrentJobBuffer; +//char CurrentJobBuffer[MAX_TICKET_SIZE]; +JobTicket *CurrentJob = NULL;//(JobTicket *)CurrentJobBuffer; +JobRequest *CurrentJobRequest = NULL; //char PreviousJobBuffer[MAX_TICKET_SIZE]; //JobTicket *PreviousJob = (JobTicket *)PreviousJobBuffer; @@ -275,6 +276,11 @@ void JobAbortFunc(MessageContainer* requestContainer) free(responseContainer.data.data); SendChars((char*)container_buffer, container_size); abort_job_request__free_unpacked(request,NULL); + //We keep the job request until it is done + job_request__free_unpacked(CurrentJobRequest,NULL); + CurrentJob = NULL; + CurrentJobRequest = NULL; + } @@ -286,6 +292,9 @@ void JobRequestFunc(MessageContainer* requestContainer) uint8_t* container_buffer; + if (CurrentJobRequest!= NULL) + job_request__free_unpacked(CurrentJobRequest,NULL); + JobRequest* request = job_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data); strcpy (JobToken, requestContainer->token); @@ -298,7 +307,10 @@ void JobRequestFunc(MessageContainer* requestContainer) } else { - memcpy(CurrentJob, Ticket,TicketSize); + //memcpy(CurrentJob, Ticket,TicketSize); + CurrentJob = Ticket; + CurrentJobRequest = request; + status = PASSED; HandleProcessParameters(Ticket->processparameters); InternalWindingConfigMessage(Ticket->spool); @@ -346,7 +358,8 @@ void JobRequestFunc(MessageContainer* requestContainer) size_t container_size = message_container__pack(&responseContainer, container_buffer); free(responseContainer.data.data); SendChars((char*)container_buffer, container_size); - job_request__free_unpacked(request,NULL); + //We keep the job request until it is done + // job_request__free_unpacked(request,NULL); //free(container_buffer); //free(requestContainer); -- cgit v1.3.1