Control fixes (Encoders, Motors and more) and FPGA preparations

4 files changed, 89 insertions, 15 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index e0abf6c2b..4e16a5599 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -17,6 +17,7 @@
 #include "include.h"
 #include "Modules/General/GeneralHardware.h"
 
+#include "MillisecTask.h"
 #include <driverlib/timer.h>
 #include <inc/hw_ints.h>
 
@@ -31,6 +32,15 @@
 #define INVALID_MSG_ID    0xFFFF
 #define MAX_TANGO_CONTROL_DEVICES 200
 /******************** STRUCTURES AND ENUMs  ********************************************/
+typedef struct MillisecMotorData
+{
+    bool Active;
+    bool WaitForData;
+    bool DataRequired;
+    MSecFptr Callback;
+    unsigned long Data;
+    int Length;
+}MillisecMotorDataStruc;
 
 typedef enum
 {
@@ -52,6 +62,8 @@ uint32_t  MotorPosition_Data[MOTOR_SPARE1_1] = {0};
 bool      MotorBusy_Data[MOTOR_SPARE1_1] = {true};
 uint32_t  Dancer_Data[NUM_OF_DANCERS] = {0};
 
+MillisecMotorDataStruc MotorData[NUM_OF_MOTORS] = {0};
+
 /******************** GLOBAL PARAMETERS  ********************************************/
 Mailbox_Handle          MillisecMsgQ = NULL;
 bool                    MillisecRestart;
@@ -75,7 +87,11 @@ void MillisecInit(void)
 
     memset(MillisecDatalog,0,sizeof(uint32_t)*MAX_TANGO_CONTROL_DEVICES);
 
-
+    int i;
+    for (i=0;i<NUM_OF_MOTORS;i++)
+    {
+        MotorData[i].Active = false;
+    }
     gateMillisecDB = GateMutex_create(NULL, &eb);
     if (gateMillisecDB == NULL)
     {
@@ -133,9 +149,33 @@ void OneMilliSecondMillisecInterrupt(UArg arg0)
     ROM_IntMasterEnable();
     return ;
 }
-
+//typedef  uint32_t (* MSecFptr)(uint32_t deviceID, uint32_t ReadValue);
+int32_t MillisecWriteToMotor(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback)
+{
+    if (MotorId >= NUM_OF_MOTORS) return -1;
+    if (MotorData[MotorId].Active == true) return -2;
+    MotorData[MotorId].Callback = Callback;
+    MotorData[MotorId].Data = Data;
+    MotorData[MotorId].Length = Length;
+    MotorData[MotorId].Active = true;
+    MotorData[MotorId].DataRequired = false;
+    return OK;
+}
+int32_t MillisecReadFromMotor(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback)
+{
+    if (MotorId >= NUM_OF_MOTORS) return -1;
+    if (MotorData[MotorId].Active == true) return -2;
+    MotorData[MotorId].Callback = Callback;
+    MotorData[MotorId].Data = Data;
+    MotorData[MotorId].Length = Length;
+    MotorData[MotorId].Active = true;
+    MotorData[MotorId].DataRequired = true;
+    return OK;
+}
 uint32_t MillisecLoop(uint32_t tick)
 {
+    int Motor_i;
+    unsigned int MotorInfo = 0;
     //call all modules Millisec functions
     //test dancers and speed encoders
     //check all callback units (state machine waiting for completion of a change)
@@ -145,7 +185,37 @@ uint32_t MillisecLoop(uint32_t tick)
     Onesecond_Tick  =   (tick%eOneSecond == 0)           ?true:false;
 
     //gather Motor data from FPGA
-    MotorSpeed_Data[MOTOR_DRYER_DRIVING] = MotorGetSpeedFromFPGA(MOTOR_DRYER_DRIVING);
+    for (Motor_i = 0;Motor_i < NUM_OF_MOTORS;Motor_i++)
+    {
+        if (MotorData[Motor_i].WaitForData == true) //Read request sent, data is waiting
+        {
+            if (MotorGetFPGAResponse(Motor_i,&MotorInfo) == OK) //got the data from the FPGA
+            {
+                MotorData[Motor_i].WaitForData = false;
+                MotorData[Motor_i].Callback(Motor_i,MotorInfo);
+            }
+        }
+        if (MotorData[Motor_i].Active == true) //new data to send
+        {
+            if (MotorSendFPGARequest(Motor_i,MotorData[Motor_i].Data,MotorData[Motor_i].Length) == OK) //sent the data to the FPGA
+            {
+                MotorData[Motor_i].Active = false; //set the Active to false first, because the callback might send a new request immediately
+                if (MotorData[Motor_i].DataRequired == true)
+                {
+                    MotorData[Motor_i].WaitForData = true; // mark the motor for data request next round
+                }
+                else
+                {
+                    MotorData[Motor_i].Callback(Motor_i,0); // call the callback to report execution
+                }
+            }
+        }
+    }
+
+#ifndef EVALUATION_BOARD
+    /* this cannot be done within one millisecond, and not needed
+     * instead, check if there is a motor waiting with data to send or read request
+     * MotorSpeed_Data[MOTOR_DRYER_DRIVING] = MotorGetSpeedFromFPGA(MOTOR_DRYER_DRIVING);
     MotorStatus_Data[MOTOR_DRYER_DRIVING] = MotorGetStatusFromFPGA(MOTOR_DRYER_DRIVING);
     MotorSpeed_Data[MOTOR_SCREW] = MotorGetSpeedFromFPGA(MOTOR_SCREW);
     MotorStatus_Data[MOTOR_SCREW] = MotorGetStatusFromFPGA(MOTOR_SCREW);
@@ -154,11 +224,11 @@ uint32_t MillisecLoop(uint32_t tick)
     MotorSpeed_Data[MOTOR_LDRIVING] = MotorGetSpeedFromFPGA(MOTOR_LDRIVING);
     MotorStatus_Data[MOTOR_LDRIVING] = MotorGetStatusFromFPGA(MOTOR_LDRIVING);
     MotorSpeed_Data[MOTOR_RDRIVING] = MotorGetSpeedFromFPGA(MOTOR_RDRIVING);
-    MotorStatus_Data[MOTOR_RDRIVING] = MotorGetStatusFromFPGA(MOTOR_RDRIVING);
+    MotorStatus_Data[MOTOR_RDRIVING] = MotorGetStatusFromFPGA(MOTOR_RDRIVING);*/
     //gather Dancer data from FPGA
-    Dancer_Data[FEEDER_DANCER] = Read_Dancer_Position(FEEDER_DANCER);
-    //Dancer_Data[POOLER_DANCER] = Read_Dancer_Position(POOLER_DANCER);
-    //Dancer_Data[WINDER_DANCER] = Read_Dancer_Position(WINDER_DANCER);
+    Dancer_Data[FEEDER_DANCER] = Read_Dancer_Position(FEEDER_DANCER,0);
+    Dancer_Data[POOLER_DANCER] = Read_Dancer_Position(POOLER_DANCER,0);
+    Dancer_Data[WINDER_DANCER] = Read_Dancer_Position(WINDER_DANCER,0);
     //gather data from FPGA
     if (Ten_msTick)
     {
@@ -189,6 +259,7 @@ uint32_t MillisecLoop(uint32_t tick)
             }
         }
     }
+#endif
     //gather data from FPGA
 
     return OK;
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
index 0c9d66201..fde3a36b3 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
@@ -13,13 +13,13 @@
 #include "drivers/Motors/Motor.h"
 #include "drivers/Heater/TemperatureSensor.h"
 
+typedef  uint32_t (* MSecFptr)(uint32_t deviceID, uint32_t ReadValue);
+int32_t MillisecWriteToMotor(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback);
+
 
 uint32_t getMotorStatusData(int MotorId);
 uint32_t getMotorSpeedData(int MotorId);
 uint32_t getTemperatureSensorData(int SensorId);
 uint32_t getADCData(int DeviceId);
 
-
-
-
 #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 bfbdec3bb..6bf238507 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c
@@ -263,27 +263,27 @@ uint32_t ControlLoop(uint32_t tick)
             switch (ControlArray[Device_i].ControlTiming)
             {
                 case eOneMillisecond:
-                        ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr(Device_i, ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
+                        ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
                         ControlArray[Device_i].ControlCallbackPtr(Device_i, ControlDatalog[Device_i]);
                     break;
                 case eTenMilliSecond:
                     if (Ten_msTick)
                     {
-                        ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr(Device_i, ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
+                        ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
                         ControlArray[Device_i].ControlCallbackPtr(Device_i, ControlDatalog[Device_i]);
                     }
                     break;
                 case eHunderdMillisecond:
                     if (Hundred_msTick)
                     {
-                        ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr(Device_i, ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
+                        ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
                         ControlArray[Device_i].ControlCallbackPtr(Device_i, ControlDatalog[Device_i]);
                     }
                     break;
                 case eOneSecond:
                     if (Onesecond_Tick)
                     {
-                        ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr(Device_i, ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
+                        ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
                         ControlArray[Device_i].ControlCallbackPtr(Device_i, ControlDatalog[Device_i]);
                     }
                     break;
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.h b/Software/Embedded_SW/Embedded/Modules/Control/control.h
index f443a10d7..23bd13231 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.h
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.h
@@ -1,6 +1,8 @@
 /************************************************************************************************************************
  * control.c
  **************************************************************************************************************************/
+#ifndef MODULES_CONTROL_CONTROL_H_
+#define MODULES_CONTROL_CONTROL_H_
 
 ////////////////////////////////State machine operation////////////////////////////////////
 //the state machine operation is used to operate in runtime correct profile flow execution
@@ -10,7 +12,7 @@
 
 //typedef  uint32_t (* DeviceDataFunction)(uint32_t deviceID, uint32_t *Value);
 typedef  uint32_t (* ControlCBFunction)(uint32_t deviceID, uint32_t ReadValue);
-typedef  uint32_t (* DataReadCBFunction)(uint32_t deviceID, uint32_t Parameter1, uint32_t Parameter2);
+typedef  uint32_t (* DataReadCBFunction)( uint32_t Parameter1, uint32_t Parameter2);
 typedef enum {
     eNoControl = 0,
     eOneMillisecond = 1,
@@ -28,3 +30,4 @@ int         RemoveControlCallback(uint32_t deviceId, ControlCBFunction Callback
 uint32_t    TemplateDataReadCBFunction (uint32_t deviceId, uint32_t Parameter1, uint32_t Parameter2);
 
 
+#endif