diff options
Diffstat (limited to 'Software/Embedded_SW/Embedded/Modules')
6 files changed, 19 insertions, 11 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c index eaf7e81d8..2fe813395 100644 --- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c +++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c @@ -31,6 +31,7 @@ #include "modules/thread/thread_ex.h" +Task_Handle Millisecond_Task_Handle; /******************** Definitions ********************************************/ #define INVALID_MSG_ID 0xFFFF #define MAX_TANGO_CONTROL_DEVICES 200 @@ -231,6 +232,7 @@ uint32_t MillisecLoop(uint32_t tick) //gather Motor data from FPGA #ifndef EVALUATION_BOARD FPGA_GetBusy(); //load the busy motor information to all motors + FPGA_Read_limit_Switches(); #endif for (Motor_i = 0;Motor_i < NUM_OF_MOTORS;Motor_i++) @@ -370,7 +372,7 @@ void MillisecTask(UArg arg0, UArg arg1) //Clock_setTimeout(HostKAClock, 1000); //Clock_start(HostKAClock); MillisecInit(); - + Millisecond_Task_Handle = Task_self(); while(1) { Mailbox_pend(MillisecMsgQ , &Message, BIOS_WAIT_FOREVER); diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h index f0009b43b..6949a4abe 100644 --- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h +++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h @@ -30,4 +30,5 @@ void MillisecInit(void); void MillisecStop(void); void MillisecStart(void); +extern Task_Handle Millisecond_Task_Handle; #endif /* MODULES_CONTROL_MILLISECTASK_H_ */ diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c index 08001cf01..40eee95c8 100644 --- a/Software/Embedded_SW/Embedded/Modules/Control/control.c +++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c @@ -90,7 +90,7 @@ int ControlPhaseDelay = 300; //the control task enters only after data gathering Mailbox_Handle ControlMsgQ = NULL; bool ControlRestart; static GateMutex_Handle gateControlDB; - +Task_Handle Control_Task_Handle; ControlDeviceStruc ControlArray[MAX_TANGO_CONTROL_DEVICES]; uint32_t ControlDatalog[MAX_TANGO_CONTROL_DEVICES]; uint32_t Control_timerBase = TIMER0_BASE; //Timer handle @@ -321,7 +321,7 @@ void controlTask(UArg arg0, UArg arg1) //uint16_t length; //Clock_setTimeout(HostKAClock, 1000); //Clock_start(HostKAClock); - + Control_Task_Handle = Task_self(); while(1) { diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.h b/Software/Embedded_SW/Embedded/Modules/Control/control.h index 76e90ed2e..853393739 100644 --- a/Software/Embedded_SW/Embedded/Modules/Control/control.h +++ b/Software/Embedded_SW/Embedded/Modules/Control/control.h @@ -40,5 +40,6 @@ uint32_t AddControlCallback( ControlCBFunction Callback, CTRL_TIMING_ENUM Ctr int RemoveControlCallback(uint32_t deviceId, ControlCBFunction Callback ); uint32_t TemplateDataReadCBFunction (uint32_t deviceId, uint32_t Parameter1, uint32_t Parameter2); +extern Task_Handle Control_Task_Handle; #endif diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_FPGAReadBackReg.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_FPGAReadBackReg.c index 8b9dc8103..eef70ee37 100644 --- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_FPGAReadBackReg.c +++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_FPGAReadBackReg.c @@ -36,7 +36,7 @@ void Stub_FPGAReadBackRegRequest(MessageContainer* requestContainer) writeString(", "); writeFloat(request->value); - status = Test_FPGA_ReadBack((unsigned char) request->fpgaid, (unsigned short) request->value, &ReadBack_Value); + status = FPGA_Test_ReadBack((unsigned char) request->fpgaid, (unsigned short) request->value, &ReadBack_Value); StubFPGAReadBackRegResponse response = STUB_FPGAREAD_BACK_REG_RESPONSE__INIT; diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c index 4477835d0..67f2227d2 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -26,6 +26,7 @@ //by recieved esign flow of the user from the UI /////////////////////////////////////////////////////////////////////////////////////////// +uint32_t CurrentControlledSpeed[MAX_THREAD_MOTORS_NUM] = {0}; TimerMotors_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM] = {HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,HARDWARE_MOTOR_TYPE__MOTO_LDRIVING,HARDWARE_MOTOR_TYPE__MOTO_WINDER,HARDWARE_MOTOR_TYPE__MOTO_SCREW}; HardwareDancerType ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM] = {FEEDER_DANCER,NUM_OF_DANCERS,POOLER_DANCER,WINDER_DANCER,NUM_OF_DANCERS}; @@ -230,9 +231,12 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) { MotorControlConfig[index].m_calculatedError = MotorControlConfig[index].m_params.MIN; }*/ - - MotorSetSpeed(ThreadMotorIdToMotorId[index], (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index], MotorsCfg[ThreadMotorIdToMotorId[index]].microstep); - //SetMotorFreq (index, MotorControlConfig[index].m_calculatedError); + uint32_t calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index]; + if (abs(calculated_speed-CurrentControlledSpeed[index])>5) + { + CurrentControlledSpeed[index] = calculated_speed; + MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed, MotorsCfg[ThreadMotorIdToMotorId[index]].microstep); + } } return OK; @@ -273,7 +277,7 @@ bool InitialProcess = false; MotorControlConfig[Motor_i].m_params.Kp = MotorsControl[Pid_Id].proportionalgain; MotorControlConfig[Motor_i].m_params.Ki = MotorsControl[Pid_Id].integraltime; MotorControlConfig[Motor_i].m_params.epsilon = 0.01; - MotorControlConfig[Motor_i].m_params.dt = eOneMillisecond; + MotorControlConfig[Motor_i].m_params.dt = 50; MotorControlConfig[Motor_i].m_calculatedError = 0; MotorControlConfig[Motor_i].m_integral = 0; MotorControlConfig[Motor_i].m_isEnabled = true; @@ -286,11 +290,11 @@ bool InitialProcess = false; #ifdef DEBUG_TEST_FUNCTIONS if (Motor_i == FEEDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled - ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadLengthCBFunction, eOneMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); + ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); if (Motor_i == FEEDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled - ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eHundredMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); + ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); if (Motor_i == POOLER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled - ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eHundredMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); + ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); /*if (HW_Motor_Id == 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,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],0); // continue; |