Merge branch 'master' of https://twinetfs.visualstudio.com/_git/Tango

2 files changed, 45 insertions, 20 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index 732bcaa96..8243c5faa 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -12,7 +12,7 @@
 
 ////////////////////////////////State machine operation////////////////////////////////////
 //the state machine operation is used to operate in runtime correct profile flow execution
-//by recieved esign flow of the user from the UI
+//by recieved design flow of the user from the UI
 ///////////////////////////////////////////////////////////////////////////////////////////
 #include "include.h"
 #include "Modules/General/GeneralHardware.h"
@@ -63,13 +63,14 @@ typedef struct MillisecMessage{
     uint8_t messageData[20];
 }MillisecMessageStruc;
 
-uint32_t  ADC_Data[MAX_ADC_DEVICES] = {0};
-uint32_t  TemperatureSensor_Data[MAX_TEMPERATURE_SENSOR_ID] = {0};
-uint32_t  MotorSpeed_Data[NUM_OF_MOTORS] = {0};
-uint32_t  MotorStatus_Data[NUM_OF_MOTORS] = {0};
-uint32_t  MotorPosition_Data[NUM_OF_MOTORS] = {0};
-bool      MotorBusy_Data[NUM_OF_MOTORS] = {true};
+//uint32_t  ADC_Data[MAX_ADC_DEVICES] = {0};
+//uint32_t  TemperatureSensor_Data[MAX_TEMPERATURE_SENSOR_ID] = {0};
+//uint32_t  MotorSpeed_Data[NUM_OF_MOTORS] = {0};
+//uint32_t  MotorStatus_Data[NUM_OF_MOTORS] = {0};
+//uint32_t  MotorPosition_Data[NUM_OF_MOTORS] = {0};
+//bool      MotorBusy_Data[NUM_OF_MOTORS] = {true};
 uint32_t  Dancer_Data[NUM_OF_DANCERS] = {0};
+uint32_t  Speed_Data = 0;
 
 MillisecMotorDataStruc MotorData[NUM_OF_MOTORS] = {0};
 MillisecMotorDataStruc SpeedSetPending[NUM_OF_MOTORS] = {0};
@@ -225,7 +226,7 @@ int32_t MillisecWriteToMotor(TimerMotors_t MotorId, unsigned long Data, int Leng
     MSBacklog[MsecLogindex]=Data;
     Motor_Id[MsecLogindex]=MotorId;
     MsecLogindex++;
-    if (MsecLogindex>=999)
+    if (MsecLogindex>=199)
         MsecLogindex = 0;
     //==========================
     if (MotorId >= NUM_OF_MOTORS) return -1;
@@ -246,7 +247,7 @@ int32_t MillisecReadFromMotor(TimerMotors_t MotorId, unsigned long Data, int Len
     MSBacklog[MsecLogindex]=Data;
     Motor_Id[MsecLogindex]=MotorId;
     MsecLogindex++;
-    if (MsecLogindex>=999)
+    if (MsecLogindex>=199)
         MsecLogindex = 0;
     //==========================
     MotorActivity++;
@@ -268,18 +269,19 @@ 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,O900Millisecond_Tick;
+    bool Ten_msTick, Hundred_msTick, Onesecond_Tick,O900Millisecond_Tick,Tick98;
     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;
+    Tick98          =   (tick%eHundredMillisecond == 99) ?true:false;
     //gather Motor data from FPGA
     //ROM_IntMasterDisable();
 
 #ifndef EVALUATION_BOARD
         FPGA_GetBusy();  //load the busy motor information to all motors
         FPGA_Read_limit_Switches_Registers();
-        Read_FPGA_GPI_Rgisters();//FPGA_Read_limit_Switches();
+        //Read_FPGA_GPI_Rgisters();//FPGA_Read_limit_Switches();
 #endif
 
         temp += MotorActivity;
@@ -381,10 +383,27 @@ uint32_t MillisecLoop(uint32_t tick)
     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);
+    if (Ten_msTick)
+    {
+        Speed_Data = Read_Speed_Sensor();
+    }
+    if (Tick98)
+    {
+        MotorGetSpeedFromFPGA(HARDWARE_MOTOR_TYPE__MOTO_RDRIVING);
+        MotorGetSpeedFromFPGA(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING);
+        MotorGetSpeedFromFPGA(HARDWARE_MOTOR_TYPE__MOTO_LDRIVING);
+        MotorGetSpeedFromFPGA(HARDWARE_MOTOR_TYPE__MOTO_WINDER);
+        MotorGetSpeedFromFPGA(HARDWARE_MOTOR_TYPE__MOTO_SCREW);
+    }
     if (Hundred_msTick)
     {
         for (Sensor_i = 0;Sensor_i < MAX_TEMPERATURE_SENSOR_ID;Sensor_i++)
             MillisecReadFromTempSensor(Sensor_i, NULL);
+        MotorGetSpeedFromFPGA_Res(HARDWARE_MOTOR_TYPE__MOTO_RDRIVING);
+        MotorGetSpeedFromFPGA_Res(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING);
+        MotorGetSpeedFromFPGA_Res(HARDWARE_MOTOR_TYPE__MOTO_LDRIVING);
+        MotorGetSpeedFromFPGA_Res(HARDWARE_MOTOR_TYPE__MOTO_WINDER);
+        MotorGetSpeedFromFPGA_Res(HARDWARE_MOTOR_TYPE__MOTO_SCREW);
     }
 
     if (O900Millisecond_Tick)
@@ -432,27 +451,32 @@ void MillisecTask(UArg arg0, UArg arg1)
 }
 
 
-uint32_t getMotorStatusData(int MotorId)
+/*uint32_t getMotorStatusData(int MotorId)
 {
     assert (MotorId < NUM_OF_MOTORS);
     return MotorStatus_Data[MotorId];
-}
-uint32_t getMotorSpeedData(int MotorId)
+}*/
+/*uint32_t getMotorSpeedData(int MotorId)
 {
     assert (MotorId < NUM_OF_MOTORS);
     return MotorSpeed_Data[MotorId];
 }
-
-uint32_t getTemperatureSensorData(int SensorId)
+*/
+/*uint32_t getTemperatureSensorData(int SensorId)
 {
     assert (SensorId < MAX_TEMPERATURE_SENSOR_ID);
     return TemperatureSensor_Data[SensorId];
-}
+}*/
 
-uint32_t getADCData(int DeviceId)
+/*uint32_t getADCData(int DeviceId)
 {
     assert (DeviceId < MAX_ADC_DEVICES);
     return ADC_Data[DeviceId];
+}*/
+uint32_t getSensorSpeedData(void)
+{
+    return Speed_Data;
 }
 
 
+
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
index 3f0b5f1f9..ef9a9d21b 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
@@ -21,12 +21,13 @@ int MillisecFlushMsgQ(TimerMotors_t MotorId);
 
 int32_t MillisecReadFromTempSensor(uint32_t TempSensorId, MSecFptr Callback);
 int MillisecFlushMsgQ(TimerMotors_t MotorId);
-
+/*
 uint32_t getMotorStatusData(int MotorId);
 uint32_t getMotorSpeedData(int MotorId);
 uint32_t getTemperatureSensorData(int SensorId);
 uint32_t getADCData(int DeviceId);
-
+*/
+uint32_t getSensorSpeedData(void);
 void MillisecInit(void);
 void MillisecStop(void);
 void MillisecStart(void);