From 0d5c2e03b1f0e06cd307eaa2d5b7eaab894126d5 Mon Sep 17 00:00:00 2001 From: Avi Levkovich Date: Wed, 13 Jun 2018 09:45:52 +0300 Subject: update I2C --- Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'Software/Embedded_SW/Embedded/Modules/Control') diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c index a1de29b08..847b777fa 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 "Drivers/Peripheral_GPIO/GPIO.h" Task_Handle Millisecond_Task_Handle; /******************** Definitions ********************************************/ @@ -236,7 +237,6 @@ 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,6 +340,7 @@ 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) -- cgit v1.3.1 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/Communication/Container.c | 2 +- .../Embedded/Drivers/FPGA/FPGA_SPI_Comm.c | 21 ++++++- .../Embedded_SW/Embedded/Drivers/Motors/Motor.c | 7 ++- .../Embedded_SW/Embedded/Drivers/Motors/Motor.h | 2 + .../Embedded_SW/Embedded/Modules/Control/control.c | 65 +++++++++++++++++++--- .../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 ++-- .../Embedded/StateMachines/Printing/JobSTM.c | 2 + 11 files changed, 140 insertions(+), 47 deletions(-) (limited to 'Software/Embedded_SW/Embedded/Modules/Control') diff --git a/Software/Embedded_SW/Embedded/Communication/Container.c b/Software/Embedded_SW/Embedded/Communication/Container.c index 42afb05a1..c02798c80 100644 --- a/Software/Embedded_SW/Embedded/Communication/Container.c +++ b/Software/Embedded_SW/Embedded/Communication/Container.c @@ -1,5 +1,4 @@ #include -#include #include "include.h" #include "driverlib/gpio.h" @@ -11,6 +10,7 @@ #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/Drivers/FPGA/FPGA_SPI_Comm.c b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c index 2e8ed2eb0..a9cf2c785 100644 --- a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c +++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c @@ -324,6 +324,7 @@ typedef enum MOTOR_CONFIG_HARD_HIZ, MOTOR_CONFIG_ACC, MOTOR_CONFIG_DEC, + MOTOR_CONFIG_MIN_SPEED, MOTOR_CONFIG_MAX_SPEED, MOTOR_CONFIG_STEP_MODE, MOTOR_CONFIG_CONFIG, @@ -373,6 +374,18 @@ uint32_t FPGA_MotorConfig_callback(TimerMotors_t _motorId, uint32_t ReadValue) temp |= speed_calc<<8; MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback); break; + case MOTOR_CONFIG_MIN_SPEED: + ConfigStages[_motorId]++; + if(MotorsCfg[_motorId].has_lowspeedoptimization) + { + temp = x_SET_PARAM | x_MIN_SPEED; + temp = temp << 24; + speed_calc = MaxSpdCalc(MotorsCfg[_motorId].minfrequency); + speed_calc|= 0x800; //LSPD_OPT + temp |= speed_calc<<8; + MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback); + break; + } case MOTOR_CONFIG_MAX_SPEED: ConfigStages[_motorId]++; if(MotorsCfg[_motorId].has_microstep) @@ -573,7 +586,8 @@ void FPGA_SetMotPosition(TimerMotors_t _motorId)//Mov Fpga_Spi[_motorId].TX_MOSI = temp; //Fpga_Spi[_motorId].AMT_OF_Words = 4; - FPGA_SPI_Transnit(_motorId); + MillisecWriteToMotor(_motorId, temp, 4, NULL); +// FPGA_SPI_Transnit(_motorId); } void FPGA_SetGoMotHome(TimerMotors_t _motorId) @@ -601,6 +615,7 @@ void FPGA_SetMotHome(TimerMotors_t _motorId) void FPGA_SetMotStop(TimerMotors_t _motorId) { + uint32_t temp; switch(MotorDriverRequest[_motorId].Stop) { @@ -624,7 +639,9 @@ void FPGA_SetMotStop(TimerMotors_t _motorId) Fpga_Spi[_motorId].TX_MOSI = Fpga_Spi[_motorId].TX_MOSI << 24;// move the command to the MSB TODO necessary??? //Fpga_Spi[_motorId].AMT_OF_Words = 1;//TODO ? - FPGA_SPI_Transnit(_motorId); + temp = Fpga_Spi[_motorId].TX_MOSI; + 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 44b869905..f553ae7fa 100644 --- a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c +++ b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c @@ -332,9 +332,10 @@ uint32_t MotorMoveWithCallback (TimerMotors_t _motorId,bool direction, uint32_t //call driver action to device id with the parameter //SetMotorSpeed (deviceId, parameter); - MotorMove(_motorId,direction,Steps ); MotorMoveModuleCallback = callback; + StopMotor(_motorId,2 ); + MotorMove(_motorId,direction,Steps ); MotorMoveControlId = AddControlCallback( MotorMoveCallBackFunction, eTenMilliSecond, MotorControlGetnBusyState,(IfTypeMotors*0x100+_motorId), _motorId, 0 ); return MotorMoveControlId; } @@ -344,7 +345,7 @@ uint32_t MotorSetSpeedWithCallback (TimerMotors_t _motorId, uint32_t _freq, call assert (callback); MotorSetSpeedModuleCallback = callback; - MotorSetSpeedControlId = AddControlCallback( MotorMoveCallBackFunction, eTenMilliSecond, MotorControlGetnBusyState,(IfTypeMotors*0x100+_motorId), _motorId, 0 ); + MotorSetSpeedControlId = AddControlCallback( MotorSetSpeedCallBackFunction, eTenMilliSecond, MotorControlGetnBusyState,(IfTypeMotors*0x100+_motorId), _motorId, 0 ); MotorSetSpeed(_motorId, _freq); return MotorSetSpeedControlId; @@ -360,7 +361,7 @@ uint32_t MotorMoveCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag) //TODO } MotorId = IfIndex&0xFF; - if (BusyFlag == 1) + if (BusyFlag == NOTBUSY) { //stop this control loop RemoveControlCallback(MotorMoveControlId, MotorMoveCallBackFunction ); diff --git a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h index cf14f0aa1..769ece72a 100644 --- a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h +++ b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h @@ -164,6 +164,8 @@ uint32_t MotorMove(TimerMotors_t _motorId,bool direction, uint32_t Steps); uint32_t MotorMoveWithCallback (TimerMotors_t _motorId,bool direction, uint32_t Steps, callback_fptr callback); +bool MotorControlGetnBusyState(uint32_t _motorId, uint32_t parameter); + uint32_t MotorSetSpeedWithCallback (TimerMotors_t _motorId, uint32_t _freq, callback_fptr callback); uint32_t SetMotHome(TimerMotors_t _motorId); diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c index a0ba50078..8e999a668 100644 --- a/Software/Embedded_SW/Embedded/Modules/Control/control.c +++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c @@ -51,6 +51,7 @@ #include #include "drivers/adc_sampling/adc.h" +#include "Drivers/Peripheral_GPIO/GPIO.h" #include "control.h" #include "MillisecTask.h" /******************** Definitions ********************************************/ @@ -93,6 +94,8 @@ static GateMutex_Handle gateControlDB; Task_Handle Control_Task_Handle; ControlDeviceStruc ControlArray[MAX_TANGO_CONTROL_DEVICES]; uint32_t ControlDatalog[MAX_TANGO_CONTROL_DEVICES]; +uint16_t ControlBacklog[1000]={0}; +uint16_t backlogindex = 0; uint32_t Control_timerBase = TIMER0_BASE; //Timer handle /******************** Functions ********************************************/ void OneMilliSecondFunction(UArg arg0); @@ -148,6 +151,25 @@ void ControlStop(void) ControlRestart = false; ADCAcquireStop(); } +uint32_t ControlActivityLed( uint32_t Parameter1, uint32_t Parameter2) +{ + static bool flag = false; + if (flag==true) + { + COMM_RED_LED_ON; + flag = false; + } + else + { + COMM_RED_LED_OFF; + flag = true; + } +return OK; +} +uint32_t ControlEmptyCBFunction(uint32_t IfIndex, uint32_t ReadValue) +{ + return OK; +} void ControlStart(void) { @@ -159,6 +181,8 @@ void ControlStart(void) TimerEnable(Control_timerBase, TIMER_A); ADCAcquireStart(0,1); + AddControlCallback( ControlEmptyCBFunction, eHundredMillisecond, ControlActivityLed,0, 0, 0 ); + } } @@ -260,45 +284,72 @@ uint32_t ControlLoop(uint32_t tick) Hundred_msTick = (tick%eHundredMillisecond == 0) ?true:false; Onesecond_Tick = (tick%eOneSecond == 0) ?true:false; Tick98 = (tick%eHundredMillisecond == 99) ?true:false; - Tick998 = (tick%eOneSecond == 998) ?true:false; + Tick998 = (tick%eOneSecond == 996) ?true:false; for (Device_i = 0; Device_i < MAX_TANGO_CONTROL_DEVICES;Device_i++) { if (ControlArray[Device_i].ControlActive) { + ControlBacklog[backlogindex]=Device_i; + if ( ++backlogindex >= 999) + backlogindex = 0; switch (ControlArray[Device_i].ControlTiming) { case eOneMillisecond: + if(ControlArray[Device_i].ControlDataReadPtr) ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2); + else + LOG_ERROR (Device_i, "Invalid callback ptr"); + if(ControlArray[Device_i].ControlCallbackPtr) ControlArray[Device_i].ControlCallbackPtr(ControlArray[Device_i].IfIndex, ControlDatalog[Device_i]); + else + LOG_ERROR (Device_i, "Invalid callback ptr"); break; case eTenMilliSecond: if (Ten_msTick) { - ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2); - ControlArray[Device_i].ControlCallbackPtr(ControlArray[Device_i].IfIndex, ControlDatalog[Device_i]); + if(ControlArray[Device_i].ControlDataReadPtr) + ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2); + else + LOG_ERROR (Device_i, "Invalid callback ptr"); + if(ControlArray[Device_i].ControlCallbackPtr) + ControlArray[Device_i].ControlCallbackPtr(ControlArray[Device_i].IfIndex, ControlDatalog[Device_i]); + else + LOG_ERROR (Device_i, "Invalid callback ptr"); } break; case eHundredMillisecond: if (Tick98) { //there is a need to trigger the data collection from FPGA, from I2C or from ADC 1.5 milliseconds to collect the data - ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2); + if(ControlArray[Device_i].ControlDataReadPtr) + ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2); + else + LOG_ERROR (Device_i, "Invalid callback ptr"); } if (Hundred_msTick) { //there is a need to trigger the data collection from FPGA, from I2C or from ADC 1.5 milliseconds to collect the data - ControlArray[Device_i].ControlCallbackPtr(ControlArray[Device_i].IfIndex, ControlDatalog[Device_i]); + if(ControlArray[Device_i].ControlCallbackPtr) + ControlArray[Device_i].ControlCallbackPtr(ControlArray[Device_i].IfIndex, ControlDatalog[Device_i]); + else + LOG_ERROR (Device_i, "Invalid callback ptr"); } break; case eOneSecond: if (Tick998) { - ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2); + if(ControlArray[Device_i].ControlDataReadPtr) + ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2); + else + LOG_ERROR (Device_i, "Invalid callback ptr"); } if (Onesecond_Tick) { - ControlArray[Device_i].ControlCallbackPtr(ControlArray[Device_i].IfIndex, ControlDatalog[Device_i]); + if(ControlArray[Device_i].ControlCallbackPtr) + ControlArray[Device_i].ControlCallbackPtr(ControlArray[Device_i].IfIndex, ControlDatalog[Device_i]); + else + LOG_ERROR (Device_i, "Invalid callback ptr"); } break; default: 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 diff --git a/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c b/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c index 609548812..f62bfba26 100644 --- a/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c +++ b/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c @@ -301,6 +301,8 @@ void JobRequestFunc(MessageContainer* requestContainer) memcpy(CurrentJob, Ticket,TicketSize); status = PASSED; HandleProcessParameters(Ticket->processparameters); + InternalWindingConfigMessage(Ticket->spool); + // PrepareWaiting[Module_Heaters] = ModuleWaiting; } if (status == PASSED) -- cgit v1.3.1