From 3f6ff04da7c8c3fb2d41ee0d5f355d9bd449492a Mon Sep 17 00:00:00 2001 From: Shlomo Hecht Date: Tue, 15 May 2018 13:00:27 +0300 Subject: FPGA beautifying. some control improvements --- .../Embedded_SW/Embedded/Common/Utilities/idle_task.c | 14 +++++++++++++- Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA.c | 8 ++++---- Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA.h | 7 ++++--- Software/Embedded_SW/Embedded/Embedded.cfg | 10 ++++++++-- Software/Embedded_SW/Embedded/Main.c | 2 +- .../Embedded/Modules/Control/MillisecTask.c | 4 +++- .../Embedded/Modules/Control/MillisecTask.h | 1 + .../Embedded_SW/Embedded/Modules/Control/control.c | 4 ++-- .../Embedded_SW/Embedded/Modules/Control/control.h | 1 + .../Modules/Stubs_Handler/Stub_FPGAReadBackReg.c | 2 +- .../Embedded_SW/Embedded/Modules/Thread/Thread_print.c | 18 +++++++++++------- 11 files changed, 49 insertions(+), 22 deletions(-) (limited to 'Software/Embedded_SW') diff --git a/Software/Embedded_SW/Embedded/Common/Utilities/idle_task.c b/Software/Embedded_SW/Embedded/Common/Utilities/idle_task.c index 5f792e73d..7bd8274b4 100644 --- a/Software/Embedded_SW/Embedded/Common/Utilities/idle_task.c +++ b/Software/Embedded_SW/Embedded/Common/Utilities/idle_task.c @@ -24,6 +24,8 @@ #include #include +#include "Modules/Control/Control.h" +#include "Modules/Control/MillisecTask.h" #define MAX_PRIORITY 10 #define SECOND_MAX_PRIORITY 9 #define MIN_PRIORITY 0 @@ -198,7 +200,17 @@ uint32_t unload_max_count=0; #endif } } - + uint32_t MillisecCounter = 0; + uint32_t ControlCounter = 0; + Void mySwitchFxn(Task_Handle from, Task_Handle to) + { + if (to == Control_Task_Handle) + ControlCounter++; + if (to == Millisecond_Task_Handle) + MillisecCounter++; + //idle_counter + // System_printf("mySwitchFxn: from = 0x%x, to = 0x%x", from, to); + } /********************************************************** Name : calculate_system_load diff --git a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA.c b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA.c index 3b9333525..290f93353 100644 --- a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA.c +++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA.c @@ -14,7 +14,7 @@ unsigned short GPO_01_Reg; -int test_FPGA() +int FPGA_Test() { GP_Out_01 Gpo_01; @@ -33,7 +33,7 @@ int test_FPGA() return 0; } -int Test_FPGA_ReadBack(unsigned char FPGA_NUM, unsigned short Value, unsigned short *ReadBackValue)// = 0x1234) +int FPGA_Test_ReadBack(unsigned char FPGA_NUM, unsigned short Value, unsigned short *ReadBackValue)// = 0x1234) { //TODO to update the deley @@ -115,7 +115,7 @@ int FPGA_ReadVersion(unsigned char FPGA_NUM, unsigned char *Version, unsigned c return PASSED; } -void Init_FPGA() +void FPGA_Init() { #ifndef EVALUATION_BOARD @@ -168,7 +168,7 @@ Limit_Switch1_REG Limit_Switch1; Limit_Switch2_REG Limit_Switch2; Limit_Switch3_REG Limit_Switch3; -void Read_limit_Switches() +void FPGA_Read_limit_Switches() { Limit_Switch1.ushort = F1_GPI_LS1_D; Limit_Switch2.ushort = F1_GPI_LS2_D; diff --git a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA.h b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA.h index c815ccda6..661ff46ca 100644 --- a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA.h +++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA.h @@ -1,10 +1,11 @@ #ifndef FPGA_H #define FPGA_H -int test_FPGA(); -int Test_FPGA_ReadBack(unsigned char FPGA_NUM, unsigned short Value, unsigned short *ReadBackValue); +int FPGA_Test(); +int FPGA_Test_ReadBack(unsigned char FPGA_NUM, unsigned short Value, unsigned short *ReadBackValue); int FPGA_ReadVersion(unsigned char FPGA_NUM, unsigned char *Version, unsigned char *Year, unsigned char *Month, unsigned char *Day); -void Init_FPGA(); +void FPGA_Init(); +void FPGA_Read_limit_Switches(void); #endif //FPGA_H diff --git a/Software/Embedded_SW/Embedded/Embedded.cfg b/Software/Embedded_SW/Embedded/Embedded.cfg index 6370847ec..28410eeac 100644 --- a/Software/Embedded_SW/Embedded/Embedded.cfg +++ b/Software/Embedded_SW/Embedded/Embedded.cfg @@ -92,6 +92,12 @@ semaphore7Params.instance.name = "ReconnectSem"; semaphore7Params.mode = Semaphore.Mode_BINARY; Program.global.ReconnectSem = Semaphore.create(null, semaphore7Params); +/*Task.addHookSet({ + // registerFxn: '&myRegisterFxn', + // createFxn: '&myCreateFxn', + // deleteFxn: '&myDeleteFxn', + switchFxn: '&mySwitchFxn' +});*/ var task0Params = new Task.Params(); task0Params.instance.name = "adcProcess"; task0Params.priority = 5; @@ -126,12 +132,12 @@ Program.global.report = Task.create("&reportService", task6Params); var task7Params = new Task.Params(); task7Params.instance.name = "control"; task7Params.priority = 11; -//Program.global.control = Task.create("&controlTask", task7Params); +Program.global.control = Task.create("&controlTask", task7Params); var task8Params = new Task.Params(); task8Params.instance.name = "MilliSecond"; task8Params.priority = 12; -//Program.global.millisec = Task.create("&MillisecTask", task8Params); +Program.global.millisec = Task.create("&MillisecTask", task8Params); var task9Params = new Task.Params(); diff --git a/Software/Embedded_SW/Embedded/Main.c b/Software/Embedded_SW/Embedded/Main.c index f54b06669..a59167c61 100644 --- a/Software/Embedded_SW/Embedded/Main.c +++ b/Software/Embedded_SW/Embedded/Main.c @@ -130,7 +130,7 @@ Init_stubs(); //ROM_GPIOPinTypeGPIOOutput(GPIO_PORTN_BASE, GPIO_PIN_1); // Green LED ROM_GPIOPinWrite(GPIO_PORTN_BASE, GPIO_PIN_1, GPIO_PIN_1); //Turn ON - Init_FPGA(); + FPGA_Init(); //ROM_GPIOPinTypeGPIOOutput(GPIO_PORTQ_BASE, GPIO_PIN_7); // set PQ7 to output (Green LED) //ROM_GPIOPinTypeGPIOOutput(GPIO_PORTQ_BASE, GPIO_PIN_4); // set PQ4 to output (Blue LED) //ROM_GPIOPinTypeGPIOOutput(GPIO_PORTN_BASE, GPIO_PIN_5); // set PN5 to output (RED LED) 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; -- cgit v1.3.1