change motors config, improve initialization and control, first control run

1 files changed, 9 insertions, 4 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index 12603b5ee..05d582eac 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -29,6 +29,8 @@
 #include "drivers/Heater/TemperatureSensor.h"
 #include "drivers/FPGA/FPGA_SPI_Comm.h"
 
+#include "modules/thread/thread_ex.h"
+
 /******************** Definitions  ********************************************/
 #define INVALID_MSG_ID    0xFFFF
 #define MAX_TANGO_CONTROL_DEVICES 200
@@ -89,7 +91,7 @@ void MillisecInit(void)
     MillisecMsgQ = Mailbox_create(sizeof(MillisecMessageStruc), 20, NULL,&eb);
     for (i=0;i<NUM_OF_MOTORS;i++)
     {
-        MotorsMsgQ[i] = Mailbox_create(sizeof(MillisecMotorDataStruc), 6, NULL,&eb);
+        MotorsMsgQ[i] = Mailbox_create(sizeof(MillisecMotorDataStruc), 20, NULL,&eb);
     }
 
     MillisecRestart = false;
@@ -123,6 +125,9 @@ void MillisecStart(void)
 {
     MillisecRestart = true;
     ROM_TimerLoadSet(Millisec_timerBase, TIMER_A,120000/*one millisecond*/);
+    ROM_TimerEnable(Millisec_timerBase, TIMER_A);
+    ROM_IntEnable(INT_TIMER1A);
+    ROM_TimerIntEnable(Millisec_timerBase, TIMER_TIMA_TIMEOUT);
     ADCAcquireStart(0,1);
 }
 
@@ -291,9 +296,9 @@ 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);
+    Dancer_Data[FEEDER_DANCER] = Read_Dancer_Position(ROTENC_RDANCER);
+    Dancer_Data[POOLER_DANCER] = Read_Dancer_Position(ROTENC_LDANCER1);
+    Dancer_Data[WINDER_DANCER] = Read_Dancer_Position(ROTENC_LDANCER2);
     if (Hundred_msTick)
     {
     }