diff options
Diffstat (limited to 'Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c')
| -rw-r--r-- | Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c | 120 |
1 files changed, 72 insertions, 48 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c index 5f14a17a7..366ce827b 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -10,6 +10,10 @@ #include "../control/pidalgo.h" #include "PMR/Hardware/HardwareMotor.pb-c.h" #include "PMR/Hardware/HardwareMotorType.pb-c.h" +#include "PMR/Printing/JobSegment.pb-c.h" +#include "PMR/Printing/JobTicket.pb-c.h" + +#include "StateMachines/Printing/PrintingSTM.h" #include "drivers/Motors/Motor.h" #include "drivers/Danser_SSI/ssi_comm.h" @@ -19,14 +23,6 @@ //the state machine operation is used to operate in runtime correct profile flow execution //by recieved esign flow of the user from the UI /////////////////////////////////////////////////////////////////////////////////////////// -typedef enum -{ - NextState = 0, - Repeat, - Inter, - Home, - Stop -} ReturnCode; uint32_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM] = {MOTOR_RDRIVING,MOTOR_DRYER_DRIVING,MOTOR_LDRIVING,MOTOR_WINDER,MOTOR_SCREW}; @@ -47,6 +43,7 @@ typedef struct MotorControlConfig_t MotorControlConfig[MAX_THREAD_MOTORS_NUM]; uint32_t DeviceId2Motor[MAX_THREAD_MOTORS_NUM]; ////////////////////////Slow Motor State//////////////////////////////////// +uint32_t ThreadPreSegmentState(void *JobDetails); //////////////////////////////////////////////////////////////////////////// @@ -55,7 +52,7 @@ uint32_t ThreadSpeedControlCBFunction(uint32_t deviceID, uint32_t ReadValue) //read value is the dancer angle int i,index=MAX_THREAD_MOTORS_NUM; for (i=0;i<MAX_THREAD_MOTORS_NUM;i++) - if (DeviceId2Motor[i] == deviceID) + if (ThreadMotorIdToDancerId[i] == deviceID) { index = i; break; @@ -88,8 +85,9 @@ uint32_t ThreadControlCBFunction(uint32_t deviceID, uint32_t ReadValue) { //read value is the dancer angle int i,index=MAX_THREAD_MOTORS_NUM; + uint32_t TranslatedReadValue = ReadValue - 8740; for (i=0;i<MAX_THREAD_MOTORS_NUM;i++) - if (DeviceId2Motor[i] == deviceID) + if (ThreadMotorIdToDancerId[i] == deviceID) { index = i; break; @@ -99,9 +97,9 @@ uint32_t ThreadControlCBFunction(uint32_t deviceID, uint32_t ReadValue) LOG_ERROR (deviceID, "No motor for device"); return 0xFFFFFFFF; } - if(MotorControlConfig[index].m_isEnabled && (MotorControlConfig[index].m_SetParam != 0)) + if(MotorControlConfig[index].m_isEnabled ) { - MotorControlConfig[index].m_mesuredParam = ReadValue; + MotorControlConfig[index].m_mesuredParam = TranslatedReadValue; MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation(MotorControlConfig[index].m_SetParam , MotorControlConfig[index].m_mesuredParam, &MotorControlConfig[index].m_params, &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral); if (MotorControlConfig[index].m_calculatedError >= MotorControlConfig[index].m_params.MAX) @@ -126,72 +124,97 @@ uint32_t ThreadControlCBFunction(uint32_t deviceID, uint32_t ReadValue) uint32_t Debug_Get_Dancer_Read(uint32_t DancerId, uint32_t Parameter1, uint32_t Parameter2) { + return (rand() % (103 + 1 + 103) - 103); } #endif +uint32_t ThreadInitialTestStub(HardwareMotor * request) +{ + + + //MotorsConfigMessage(request); + ThreadPrepareState(request); + ThreadPreSegmentState(request); + return OK; +} //******************************************************************************************************************** - ReturnCode PrepareState(void *JobDetails) + uint32_t ThreadPrepareState(void *JobDetails) { - int Motor_i; + int Motor_i, HW_Motor_Id; //start thread control for all motors for (Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++) { - MotorControlConfig[Motor_i].m_params.MAX = MotorsCfg[Motor_i].maxfreq; - MotorControlConfig[Motor_i].m_params.MIN = MotorsCfg[Motor_i].minfreq; - MotorControlConfig[Motor_i].m_params.Kd = MotorsCfg[Motor_i].kd; - MotorControlConfig[Motor_i].m_params.Kp = MotorsCfg[Motor_i].kp; - MotorControlConfig[Motor_i].m_params.Ki = MotorsCfg[Motor_i].ki; - MotorControlConfig[Motor_i].m_params.dt = eOneMillisecond; - MotorControlConfig[Motor_i].m_calculatedError = 0; - MotorControlConfig[Motor_i].m_integral = 0; - MotorControlConfig[Motor_i].m_isEnabled = true; - MotorControlConfig[Motor_i].m_isReady = true; - MotorControlConfig[Motor_i].m_mesuredParam = 0; - MotorControlConfig[Motor_i].m_preError = 0; - MotorControlConfig[Motor_i].m_SetParam = 0;//need to update SetParams on presegment stage -#ifdef DEBUG_TEST_FUNCTIONS - if (Motor_i == DRYER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled - AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,MotorGetSpeed,ThreadMotorIdToMotorId[Motor_i],0); - else - AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Debug_Get_Dancer_Read,ThreadMotorIdToDancerId[Motor_i],0); -#else - if (Motor_i == DRYER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled - continue; - //AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,MotorGetSpeed,ThreadMotorIdToMotorId[Motor_i],0); - else - AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],0); -#endif + HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i]; + MotorControlConfig[Motor_i].m_params.MAX = MotorsCfg[HW_Motor_Id].maxfreq; + MotorControlConfig[Motor_i].m_params.MIN = MotorsCfg[HW_Motor_Id].minfreq; + MotorControlConfig[Motor_i].m_params.Kd = MotorsCfg[HW_Motor_Id].kd; + MotorControlConfig[Motor_i].m_params.Kp = MotorsCfg[HW_Motor_Id].kp; + MotorControlConfig[Motor_i].m_params.Ki = MotorsCfg[HW_Motor_Id].ki; + MotorControlConfig[Motor_i].m_params.dt = eOneMillisecond; + MotorControlConfig[Motor_i].m_calculatedError = 0; + MotorControlConfig[Motor_i].m_integral = 0; + MotorControlConfig[Motor_i].m_isEnabled = true; + MotorControlConfig[Motor_i].m_isReady = true; + MotorControlConfig[Motor_i].m_mesuredParam = 0; + MotorControlConfig[Motor_i].m_preError = 0; + MotorControlConfig[Motor_i].m_SetParam = 0;//need to update SetParams on presegment stage + #ifdef DEBUG_TEST_FUNCTIONS + if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled + //AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,MotorGetSpeed,ThreadMotorIdToMotorId[Motor_i],0); + // continue; + AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i); + else if ((Motor_i == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||(Motor_i == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING)||(Motor_i == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING)) + AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i); + #else + if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled + continue; + //AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,MotorGetSpeed,ThreadMotorIdToMotorId[Motor_i],Motor_i); + else if ((Motor_i == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||(Motor_i == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING)||(Motor_i == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING)) + AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i); + #endif } - + Winder_Prepare(); //set 3 dancers to the profile positions + ControlStart(); return OK; } //******************************************************************************************************************** -ReturnCode PreSegmentState(void *JobDetails) +uint32_t ThreadPreSegmentState(void *JobDetails) { +//set the speed only before the first segment, speed is constant accros job + JobTicket* JobTicket = JobDetails; +// int process_speed = JobTicket->processparameters->dyeingspeed; + int process_speed = 500; + + int dryer_speed = process_speed * MotorsCfg[MOTOR_DRYER_DRIVING].ratio2dryerspeed; //set dryer_speed_translation_here - TimerMotors_t Motor_i; - for (Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++) - { - MotorControlConfig[Motor_i].m_SetParam = MotorGetSpeed(getMotorId(Motor_i));//need to update SetParams on presegment stage - } // set the new speed in the dryer motor to the speed of the new segment + MotorSetSpeed(MOTOR_DRYER_DRIVING, process_speed, MotorsCfg[MOTOR_DRYER_DRIVING].minmicrostep); + // activate control fr all motors //set speed for both rocker motors //wait for all motors to get to the required speed (set the target speed for the control to check) //call the job state machine when the thread system is ready + PreSegmentReady(Module_Thread,OK); + return OK; } //******************************************************************************************************************** - ReturnCode SegmentState(void *JobDetails) + uint32_t ThreadSegmentState(void *JobDetails) { return OK; } //******************************************************************************************************************** - ReturnCode EndState(void *JobDetails) + uint32_t ThreadEndState(void *JobDetails) { + int Motor_i; + for ( Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++) + { + StopMotor(ThreadMotorIdToMotorId[Motor_i],Hard_Hiz); + } + return OK; } @@ -211,3 +234,4 @@ void ThreadStopPrinting(void) { //PrintingIterate(); } + |