From 9a8b53c838134dca38816aac28b119a14dae05bd Mon Sep 17 00:00:00 2001 From: Shlomo Hecht Date: Sun, 10 Jun 2018 08:46:52 +0300 Subject: First print and some winder improvements --- .../Embedded_SW/Embedded/Modules/Thread/Thread.h | 3 - .../Embedded/Modules/Thread/Thread_Winder.c | 67 +++++++++++++++++++--- .../Embedded/Modules/Thread/Thread_ex.h | 2 +- .../Embedded/Modules/Thread/Thread_init.c | 2 +- .../Embedded/Modules/Thread/Thread_print.c | 16 ++++-- 5 files changed, 73 insertions(+), 17 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 db3bfc46e..ba6597449 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h @@ -65,8 +65,5 @@ uint32_t MotorPidRequestMessage(HardwarePidControl* request); uint32_t DancerConfigMessage(HardwareDancer * request); -uint32_t ThreadPrepareState(void *JobDetails); -uint32_t Winder_Prepare(void); - #endif //MODULES_THREAD_THREAD_H_ diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c index b61247fe4..17d683444 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c @@ -9,9 +9,14 @@ #include "Drivers/Peripheral_GPIO/GPIO.h" #include "drivers/Motors/Motor.h" #include "StateMachines/Printing/PrintingSTM.h" + +#include "drivers/FPGA/FPGA.h" + +#include "drivers/FPGA/FPGA_SPI_Comm.h" + bool Winder_ScrewHoming = false; uint32_t Winder_ScrewAtOffsetCallback(uint32_t NumberOfSteps); -uint32_t Winder_PrepareStage2(void); +uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue); double ScrewSpeed = 0; @@ -19,11 +24,57 @@ uint32_t Winder_Init(void) { return OK; } -uint32_t Winder_Prepare(void) +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, GPI112_LS_SCREW_LEFT, Winder_Test2); + //status |= MotorMoveSteps (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, GPI113_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 status = 0; + 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 = (50 * MotorsCfg[SCREW_MOTOR].pulseperround)/(2*PI* MotorsCfg[SCREW_MOTOR].pulleyradius); + double ScrewSpeed = (process_speed * MotorsCfg[SCREW_MOTOR].pulseperround)/(2*PI* MotorsCfg[SCREW_MOTOR].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. @@ -33,17 +84,17 @@ uint32_t Winder_Prepare(void) if (PollGPIO(GPI113_LS_SCREW_RIGHT)) { //go to prepare stage 2 - Winder_PrepareStage2(); + Winder_PrepareStage2(0,0); } else { Winder_ScrewHoming = true; - status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,1);//make sur to move the cart home - status |= MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_SCREW, ScrewSpeed); +// status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed, GPI113_LS_SCREW_RIGHT, Winder_PrepareStage2); + status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed/10, GPI113_LS_SCREW_RIGHT, Winder_Test1); } return status; } -uint32_t Winder_PrepareStage2(void) +uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue) { uint32_t status; uint32_t numOfSteps = InternalWinderCfg.segmentoffsetpulses; @@ -52,7 +103,7 @@ uint32_t Winder_PrepareStage2(void) * 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,0);//make sur to move the cart out + status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,(1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize));//make sur to move the cart out //status |= MotorMoveSteps (HARDWARE_MOTOR_TYPE__MOTO_SCREW, numOfSteps, Winder_ScrewAtOffsetCallback); //set motor location 0 here return status; diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h index ea75bffa3..28a2346c1 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h @@ -18,7 +18,7 @@ uint32_t ThreadInitialTestStub(); //uint32_t MotorPidRequestMessage(HardwarePidControl* request); uint32_t Winder_Init(void); -uint32_t Winder_Prepare(void); +uint32_t Winder_Prepare(void *JobDetails); uint32_t Winder_Presegment(void *JobDetails); uint32_t Winder_End(void); diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c index 0e276fe92..d27b4f94e 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c @@ -97,7 +97,7 @@ uint32_t MotorPidRequestMessage(HardwarePidControl* request) MotorSamples[Motor_i][i] = 0; //reset the samples value for control beginning NormalizedErrorCoEfficient[Motor_i] = (2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength); temp = 1<<(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].resolutionbits); - temp=(100*(temp-1)*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm); + 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]; diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c index 92bfa9c92..3ebb0304b 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -22,6 +22,7 @@ #include "drivers/Heater/TemperatureSensor.h" #include "drivers/Heater/Heater.h" #include "drivers/Motors/Motor.h" +#include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h" #include "modules/heaters/heaters.h" ////////////////////////////////State machine operation//////////////////////////////////// @@ -66,6 +67,8 @@ ProcessedLengthFunc ProcessedLengthFuncPtr = NULL; void ThreadSegmentEnded(void); void ThreadInterSegmentEnded(void); void ThreadDistanceToSpoolEnded(void); + +double KeepNormalizedError = 0; ////////////////////////Slow Motor State//////////////////////////////////// //uint32_t ThreadPreSegmentState(void *JobDetails); @@ -176,7 +179,9 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) pooler_counter++; if (pooler_counter%10 == 0) { - SendJobProgress(CurrentProcessedLength/CurrentRequestedLength,CurrentSegmentId,false); + //SendJobProgress(CurrentProcessedLength/CurrentRequestedLength,CurrentSegmentId,false); + SendJobProgress(/*KeepNormalizedError*/MotorControlConfig[index].m_calculatedError,CurrentSegmentId,false); + } if (pooler_counter>=100) { @@ -280,7 +285,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) { DancerId = ThreadMotorIdToDancerId[index]; if (ReadValue < 10) - return; + return OK; TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint; if (index == POOLER_MOTOR) //pooler dancer is right sided: data is opposite TranslatedReadValue = (-1*TranslatedReadValue); @@ -295,8 +300,10 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) MotorControlConfig[index].m_mesuredParam = 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 + if (index != FEEDER_MOTOR) //feeder unit handles errors opposite to left unit MotorControlConfig[index].m_calculatedError = (-1*MotorControlConfig[index].m_calculatedError); + else + KeepNormalizedError = NormalizedError; calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index]; if (abs(calculated_speed-CurrentControlledSpeed[index])>5) { @@ -324,7 +331,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) }*/ //HeatingTestSendResonse(0, false,true,true, MotorDriverRequest[22].Speed,MotorDriverRequest[18].Speed,MotorDriverRequest[15].Speed,MotorDriverRequest[3].Speed, "MotorSpeed"); - HeatingTestSendResonse(0, false,true,true, /*OriginalMotorSpd_2PPS[index]*/_speed,OriginalMotorSpd_2PPS[index]/*(int)error_integered*/,calculated_speed,ReadValue, "FeederSpeed"); + HeatingTestSendResonse(0, false,true,true, /*OriginalMotorSpd_2PPS[index]*/_speed,OriginalMotorSpd_2PPS[index]/*(int)error_integered*/,MotorControlConfig[index].m_calculatedError,ReadValue, "FeederSpeed"); pooler_counter = 0; } } @@ -475,6 +482,7 @@ uint32_t ThreadPreSegmentState(void *JobDetails) MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 10); //#warning rocker disabled +// MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize, 0, GPI_LS_RLOADMOTOR_UP, EndState); //TODO // activate control fr all motors //set speed for both rocker motors -- cgit v1.3.1 From 51d7c41cbbec95d2428a270b3f585011aa9babf3 Mon Sep 17 00:00:00 2001 From: Shlomo Hecht Date: Mon, 11 Jun 2018 11:26:29 +0300 Subject: Winder changes, Job stop on dancer extreme move --- .../Embedded/Drivers/FPGA/FPGA_SPI_Comm.c | 2 +- .../Embedded_SW/Embedded/Drivers/Motors/Motor.c | 2 +- .../Embedded/Modules/AlarmHandling/AlarmHandling.c | 8 ----- .../Embedded/Modules/Heaters/Heaters_print.c | 2 +- .../Embedded_SW/Embedded/Modules/IDS/IDS_print.c | 41 ++++++++++++++++------ .../Embedded_SW/Embedded/Modules/Thread/Thread.h | 1 + .../Embedded/Modules/Thread/Thread_Winder.c | 22 ++++++------ .../Embedded/Modules/Thread/Thread_init.c | 4 ++- .../Embedded/Modules/Thread/Thread_print.c | 23 ++++++++---- .../Embedded/StateMachines/Printing/JobSTM.c | 10 +++--- .../Embedded/StateMachines/Printing/PrintingSTM.c | 3 ++ 11 files changed, 75 insertions(+), 43 deletions(-) (limited to 'Software/Embedded_SW/Embedded/Modules/Thread') 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 255b33672..2e8ed2eb0 100644 --- a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c +++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c @@ -594,7 +594,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/Motors/Motor.c b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c index 5ad3e38c4..44b869905 100644 --- a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c +++ b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c @@ -399,7 +399,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"); 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/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;iprocessparameters->dyeingspeed; // double ScrewSpeed = (50 * MotorsCfg[SCREW_MOTOR].pulseperround * MotorsCfg[SCREW_MOTOR].microstep)/(2*PI* MotorsCfg[SCREW_MOTOR].pulleyradius); - double ScrewSpeed = (process_speed * MotorsCfg[SCREW_MOTOR].pulseperround)/(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); /* * 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 */ - if (PollGPIO(GPI113_LS_SCREW_RIGHT)) + if (FPGA_Read_limit_Switches(GPI_LS_SCREW_RIGHT)==LIMIT) { //go to prepare stage 2 Winder_PrepareStage2(0,0); @@ -89,8 +90,8 @@ 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, GPI113_LS_SCREW_RIGHT, Winder_PrepareStage2); - status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed/10, GPI113_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); + status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed/10, GPI_LS_SCREW_RIGHT, Winder_Test1); } return status; } @@ -104,6 +105,7 @@ uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue) * 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 |= MotorMoveSteps (HARDWARE_MOTOR_TYPE__MOTO_SCREW, numOfSteps, Winder_ScrewAtOffsetCallback); //set motor location 0 here return status; @@ -122,7 +124,7 @@ uint32_t Winder_Presegment(void *JobDetails) float RotationsPerSecond; // double ScrewSpeed = (process_speed * MotorsCfg[SCREW_MOTOR].pulseperround * MotorsCfg[SCREW_MOTOR].microstep)/(2*PI* MotorsCfg[SCREW_MOTOR].pulleyradius); - double ScrewSpeed = (process_speed * MotorsCfg[SCREW_MOTOR].pulseperround)/(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 { @@ -167,12 +169,12 @@ void Winder_ScrewHomeLimitSwitchInterrupt(void) { StopMotor(HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Stop); //stop ASAP } - status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[SCREW_MOTOR].directionthreadwize);//make sure to move the cart out + status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);//make sure to move the cart out } void Winder_ScrewOutLimitSwitchInterrupt(void) { uint32_t status; - status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,1-MotorsCfg[SCREW_MOTOR].directionthreadwize);//make sure to move the cart out + 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) { diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c index d27b4f94e..e2d492b47 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c @@ -17,6 +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}; InternalWinderConfigStruc InternalWinderCfg = {0}; HardwareDancer DancersCfg[MAX_SYSTEM_DANCERS] = {0}; @@ -100,7 +101,8 @@ 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); 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 3ebb0304b..56e1bf844 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -38,7 +38,7 @@ uint32_t ControlIdtoMotorId [MAX_THREAD_MOTORS_NUM] = {0xFF}; uint32_t SpeedControlId=0xFF; int OriginalMotorSpd_2PPS[MAX_THREAD_MOTORS_NUM] = {0}; - +uint32_t JobCounter = 0; typedef struct { bool m_isEnabled; @@ -168,7 +168,7 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) #warning control disabled CurrentProcessedLength+=length; -#warning control disabled + PosDif[MotorDataIndex] = CurrentPosition; //PosDif[MotorDataIndex] = positionDiff; MotorSentData[MotorDataIndex] = length; @@ -192,8 +192,6 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) } if (CurrentProcessedLength>=CurrentRequestedLength ) { - SendJobProgress(100,0,true); - #warning handle job wit several segments!!! // segment/intersegment/distance to spool finished if (ProcessedLengthFuncPtr) ProcessedLengthFuncPtr(); @@ -287,8 +285,12 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) if (ReadValue < 10) return OK; TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint; - if (index == POOLER_MOTOR) //pooler dancer is right sided: data is opposite + if (index == POOLER_MOTOR) + { + //pooler dancer is right sided: data is opposite TranslatedReadValue = (-1*TranslatedReadValue); + JobCounter++; + } //TranslatedReadValue = 0;//test MotorSamples[index][MotorSamplePointer[index]] = TranslatedReadValue;//(-1 * TranslatedReadValue); MotorSamplePointer[index]++; @@ -296,6 +298,12 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) for (i=0;i DancerStopActivityLimit[index])&&(JobCounter > eOneSecond)) + { + EndState(CurrentJob); + } NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index]; MotorControlConfig[index].m_mesuredParam = NormalizedError; MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation((float)MotorControlConfig[index].m_SetParam , (float)MotorControlConfig[index].m_mesuredParam, @@ -316,7 +324,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) pooler_counter++; if (pooler_counter>=1000) { - float error_integered = MotorControlConfig[index].m_calculatedError*1000; + //float error_integered = MotorControlConfig[index].m_calculatedError*1000; /*{ "HeaterGroupId": 0, "Zone1Temp": 80, @@ -357,6 +365,8 @@ bool InitialProcess = false; { int Motor_i, HW_Motor_Id, Pid_Id; CurrentSegmentId = 0; + + JobCounter = 0; //start thread control for all motors for (Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++) { @@ -494,6 +504,7 @@ uint32_t ThreadPreSegmentState(void *JobDetails) } else { + ThreadUpdateProcessLength (0,(void *)NULL); PreSegmentReady(Module_Thread,ModuleDone); InitialProcess = false; } diff --git a/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c b/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c index e5efaf66c..c708a1804 100644 --- a/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c +++ b/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c @@ -136,11 +136,6 @@ static ReturnCode PrepareState(void *JobDetails) //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(JobDetails); - } if (Configured[Module_IDS]) { //PrepareWaiting[Module_IDS] = ModuleWaiting; @@ -157,6 +152,11 @@ static ReturnCode PrepareState(void *JobDetails) PrepareWaiting[Module_Thread] = ModuleWaiting; ThreadPrepareState(CurrentJob); } + if (Configured[Module_Winder]) + { + // PrepareWaiting[Module_Winder] = ModuleWaiting; + Winder_Prepare(JobDetails); + } return retcode; } diff --git a/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.c b/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.c index c93c85df0..1ad3b530c 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" @@ -247,6 +248,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