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

8 files changed, 29 insertions, 38 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)
     {
     }
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c
index a56457bdb..08001cf01 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c
@@ -139,7 +139,7 @@ void ControlInit(void)
     ROM_IntEnable(INT_TIMER0A);
     ROM_TimerIntEnable(Control_timerBase, TIMER_TIMA_TIMEOUT);
 
-    ADCAcquireInit();
+    //ADCAcquireInit();
 
     return;
 }
diff --git a/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c b/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
index b41eca9ac..cd7174d10 100644
--- a/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
+++ b/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
@@ -321,7 +321,7 @@ uint32_t HWConfigurationFunc(MessageContainer* requestContainer)
     UploadHardwareConfigurationResponse response = UPLOAD_HARDWARE_CONFIGURATION_RESPONSE__INIT;
 
     UploadHardwareConfigurationRequest* UploadRequest = upload_hardware_configuration_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
-    HardwareConfiguration *request = UploadRequest->hardwareconfiguration;
+    HardwareConfiguration *request =    UploadRequest->hardwareconfiguration;
     if (request->n_winders == 1)
         status += InternalWinderConfigMessage(request->winders);
     for (Motor_i = 0; Motor_i < request->n_motors ; Motor_i++)
@@ -342,7 +342,7 @@ uint32_t HWConfigurationFunc(MessageContainer* requestContainer)
         status += DispenserConfigMessage(request->dispensers[Dispenser_i]);
     //status += HeaterConfigSetSharedHeatersParams (request->outputproportionalcycletime, request->outputproportionalsinglestep);
 
-
+    ControlStart();
     ThreadInitialTestStub(request);
 
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.c
index fef551748..e7b75cc9d 100644
--- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.c
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.c
@@ -179,6 +179,8 @@ void Stub_MotorRunRequest(MessageContainer* requestContainer)
 {
     //uint32_t status = FAILED;
 
+    uint32_t read_status;
+
     MessageContainer responseContainer;
 
     StubMotorRunRequest* request = stub_motor_run_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
@@ -210,9 +212,6 @@ void Stub_MotorRunRequest(MessageContainer* requestContainer)
     response.motor_id = request->motor_id;
     response.has_motor_id = true;
 #ifdef EVALUATION_BOARD
-
-    uint32_t read_status;
-
     read_status = Get_and_Clear_Status();
 
     response.speed = (double)(CurrentSpdCalc(Get_Param(x_SPEED)));
@@ -262,7 +261,7 @@ void Stub_MotorRunRequest(MessageContainer* requestContainer)
 
 void Stub_MotorStopRequest(MessageContainer* requestContainer)
 {
-
+    uint32_t read_status;
     //uint32_t status = FAILED;
 
     MessageContainer responseContainer;
@@ -301,9 +300,6 @@ void Stub_MotorStopRequest(MessageContainer* requestContainer)
     response.motor_id = request->motor_id;
     response.has_motor_id = true;
 #ifdef EVALUATION_BOARD
-
-    uint32_t read_status;
-
     read_status = Get_and_Clear_Status();
 
     response.mot_status = ((read_status & x_STATUS_MOT_STATUS)>>5);
@@ -541,6 +537,7 @@ void Stub_MotorPositionRequest(MessageContainer* requestContainer)
 
 void Stub_MotorMovRequest(MessageContainer* requestContainer)
 {
+    uint32_t read_status;
 
     MessageContainer responseContainer;
 
@@ -563,23 +560,10 @@ void Stub_MotorMovRequest(MessageContainer* requestContainer)
     response.has_motor_id = true;
 
     #ifdef EVALUATION_BOARD
-
-        uint32_t read_status;
-
         response.position = Get_Param(x_ABS_POS);
         response.has_position = true;
 
-        //while(SSIBusy(SSI2_BASE)){};
-        uint32_t timeout = SSI_SPI_TIMEOUT;
-
-        while(SSIBusy(SSI2_BASE))
-        {
-            timeout = timeout--;
-            if(timeout == 0)
-            {
-                return ERROR;
-            }
-        }
+        while(SSIBusy(SSI2_BASE)){};
 
         read_status = Get_and_Clear_Status();
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
index 7690b131f..0bda4964b 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
@@ -15,6 +15,7 @@
 #include "drivers/Motors/Motor.h"
 #include "drivers/Danser_SSI/ssi_comm.h"
 
+#include "thread_ex.h"
 
 typedef struct
 {
@@ -37,6 +38,7 @@ typedef enum threadMotorsEnum
     SCREW_MOTOR,
     MAX_THREAD_MOTORS_NUM
 }threadMotorsEnum;
+
 #define MAX_THREAD_FEED_MOTORS WINDER_MOTOR+1
 #define MAX_SYSTEM_DANCERS HARDWARE_DANCER_TYPE__RightDancer+1
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
index 5f49c7d10..3fb78d2f0 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
@@ -1,6 +1,15 @@
 
 #ifndef MODULES_THREAD_THREAD_EX_H_
 #define MODULES_THREAD_THREAD_EX_H_
+
+typedef enum
+{
+    WINDER_DANCER,
+    POOLER_DANCER,
+    FEEDER_DANCER,
+    NUM_OF_DANCERS
+} DANCER_ENUM;
+
 uint32_t ThreadPrepareState(void *JobDetails);
 uint32_t ThreadPreSegmentState(void *JobDetails);
 uint32_t ThreadSegmentState(void *JobDetails, int SegmentId);
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
index 5a8c3df76..0e276fe92 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
@@ -51,7 +51,6 @@ uint32_t MotorsConfigMessage(HardwareMotor * request)
 {
     uint32_t status = PASSED;
     TimerMotors_t Motor_i;
-    MotorDriverConfigStruc MotorDriverConfig;
     Motor_i = request->hardwaremotortype;
     /*for (i=0;i<MAX_THREAD_MOTORS_NUM;i++)
     {
@@ -69,15 +68,8 @@ uint32_t MotorsConfigMessage(HardwareMotor * request)
     //if (Motor_i< MAX_THREAD_MOTORS_NUM)
     //{
         memcpy (&MotorsCfg[Motor_i],request,sizeof(HardwareMotor));
-        MotorDriverConfig.ACC = MotorsCfg[Motor_i].maxchangeslope;
-        MotorDriverConfig.DEC = MotorsCfg[Motor_i].maxchangeslope;
-        MotorDriverConfig.MaxSpeed = MotorsCfg[Motor_i].maxfrequency;
-        MotorDriverConfig.HasMicroStep = MotorsCfg[Motor_i].has_microstep;
-        MotorDriverConfig.Microstep = MotorsCfg[Motor_i].microstep;
-        MotorDriverConfig.HasConfigWord = MotorsCfg[Motor_i].has_configword;
-        MotorDriverConfig.ConfigWord = MotorsCfg[Motor_i].configword;
 
-        status = MotorConfig( Motor_i,  &MotorDriverConfig);
+        status = MotorConfig( Motor_i,  &MotorsCfg[Motor_i]);
 //        if (Motor_i == MOTOR_RDRIVING)
 //            ThreadInitialTestStub(request);
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 0e68a6406..434ac0f21 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -216,7 +216,6 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
             avreageSampleValue += MotorSamples[index][i];
         avreageSampleValue = avreageSampleValue / MotorsControl[index].pvinputfilterfactormode;
         NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index];
-if (index != 0) return OK;
         MotorControlConfig[index].m_mesuredParam = NormalizedError;
         MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation((float)MotorControlConfig[index].m_SetParam , (float)MotorControlConfig[index].m_mesuredParam,
                                                                               &MotorControlConfig[index].m_params,   &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral);