diff options
| author | Shlomo Hecht <shlomo@twine-s.com> | 2018-06-17 14:16:18 +0300 |
|---|---|---|
| committer | Shlomo Hecht <shlomo@twine-s.com> | 2018-06-17 14:16:18 +0300 |
| commit | 8ea30fe7354c7d878c5ff718b7e52a04178cf55b (patch) | |
| tree | 8370473d57f54a6c4acac90ff46db0041ff61f74 /Software/Embedded_SW/Embedded | |
| parent | a41edd14d6d7531550ce6f967360479c05464bd7 (diff) | |
| download | Tango-8ea30fe7354c7d878c5ff718b7e52a04178cf55b.tar.gz Tango-8ea30fe7354c7d878c5ff718b7e52a04178cf55b.zip | |
MotorStop, improve IDS, Dispensers pressure
Diffstat (limited to 'Software/Embedded_SW/Embedded')
14 files changed, 89 insertions, 42 deletions
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); } |
