FPGA beautifying. some control improvements

11 files changed, 49 insertions, 22 deletions

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 <xdc/runtime/Error.h>
 #include <xdc/runtime/System.h>
 
+#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;