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

4 files changed, 26 insertions, 30 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
index cdb726b5e..c3f15aff8 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
@@ -258,7 +258,7 @@
         {
             CallbackCounter--;
         }
-        if ((angle>14000  )&&(BusyFlag == NOTBUSY)) // OK - take another round
+        if ((abs(angle)>14000  )&&(BusyFlag == NOTBUSY)) // OK - take another round
         {
             Report("Thread_Load_Set_Load_Arm_To_Stopper time",__FILE__,msec_millisecondCounter - UnloadingStart,msec_millisecondCounter,RpMessage,UnloadingStart,0);
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
index 07ecf5d0b..11a92ab63 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
@@ -46,7 +46,7 @@ uint32_t ThreadEndState();
 
 bool Set_Thread_Rockers_Bypass (int value);
 
-uint32_t StoreDancerConfigMessage(void);
+uint32_t StoreDancerConfigMessage(int DancerId);
 uint32_t LoadDancerConfigMessage(void);
 
 //uint32_t MotorPidRequestMessage(HardwarePidControl* request);
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
index 1920db03a..869485b71 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
@@ -188,40 +188,36 @@ uint32_t MotorPidRequestMessage(HardwarePidControl* request)
 }
 char DancerConfigPath[50] = "0://SysInfo//DancCfg.cfg";
 
-uint32_t StoreDancerConfigMessage()
+uint32_t StoreDancerConfigMessage(int DancerId)
 {
     uint32_t status = OK;
   //  FRESULT Fresult = FR_OK;
     //HardwareConfiguration DancerConfig;
     HardwareDancer Dancers[MAX_SYSTEM_DANCERS];
-  //  uint8_t* response_buffer;
+#ifdef FOUR_WINDERS
+    uint8_t DancerAddress[MAX_SYSTEM_DANCERS] = {EEPROM_STORAGE_DANCER_0,EEPROM_STORAGE_DANCER_1,EEPROM_STORAGE_DANCER_2,EEPROM_STORAGE_DANCER_3,EEPROM_STORAGE_DANCER_4};
+#else
+    uint8_t DancerAddress[MAX_SYSTEM_DANCERS] = {EEPROM_STORAGE_DANCER_0,EEPROM_STORAGE_DANCER_1,EEPROM_STORAGE_DANCER_2};
+#endif
   //  size_t response_size = 0;
-    int Dancer_i;
+    int Dancer_i = DancerId;
 
     //hardware_configuration__init(&DancerConfig);
     //DancerConfig.dancers = (HardwareDancer**)my_malloc(sizeof(HardwareDancer*)*MAX_SYSTEM_DANCERS);
-    for (Dancer_i = 0; Dancer_i < MAX_SYSTEM_DANCERS; Dancer_i++)
-    {
-        hardware_dancer__init(&Dancers[Dancer_i]);
-        //DancerConfig.dancers[Dancer_i] = &Dancers[Dancer_i];
-        Dancers[Dancer_i].has_zeropoint = true;
-        Dancers[Dancer_i].hardwaredancertype = Dancer_i;
-        Dancers[Dancer_i].has_hardwaredancertype = true;
-        Dancers[Dancer_i].zeropoint=Control_Read_Dancer_Position(Dancer_i,0);
-        //DancerConfig.n_dancers++;
-        DancersCfg[Dancer_i].zeropoint = Dancers[Dancer_i].zeropoint;
-        DancersCfg[Dancer_i].zeropoint = Control_Read_Dancer_Position(Dancer_i,0);
-    }
+    hardware_dancer__init(&Dancers[Dancer_i]);
+    //DancerConfig.dancers[Dancer_i] = &Dancers[Dancer_i];
+    Dancers[Dancer_i].has_zeropoint = true;
+    Dancers[Dancer_i].hardwaredancertype = Dancer_i;
+    Dancers[Dancer_i].has_hardwaredancertype = true;
+    Dancers[Dancer_i].zeropoint=Control_Read_Dancer_Position(Dancer_i,0);
+    //DancerConfig.n_dancers++;
+    DancersCfg[Dancer_i].zeropoint = Dancers[Dancer_i].zeropoint;
+    DancersCfg[Dancer_i].zeropoint = Control_Read_Dancer_Position(Dancer_i,0);
 
-    status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_0,DancersCfg[0].zeropoint);
-    status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_1,DancersCfg[1].zeropoint);
-    status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_2,DancersCfg[2].zeropoint);
-    Report("~~~~~~Store eeprom 0",__FILE__,DancersCfg[0].zeropoint,(int)DancersCfg[1].zeropoint,RpWarning,(int)DancersCfg[2].zeropoint,0);
-#ifdef FOUR_WINDERS
-    status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_3,DancersCfg[HARDWARE_DANCER_3].zeropoint);
-    status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_4,DancersCfg[HARDWARE_DANCER_4].zeropoint);
-    Report("~~~~~~Store eeprom 1",__FILE__,__LINE__,(int)DancersCfg[HARDWARE_DANCER_3].zeropoint,RpWarning,(int)DancersCfg[HARDWARE_DANCER_4].zeropoint,0);
-#endif
+
+
+    status |= MCU_E2PromProgram(DancerAddress[Dancer_i],DancersCfg[Dancer_i].zeropoint);
+    Report("~~~~~~Store eeprom dancer",__FILE__,__LINE__,Dancer_i,RpWarning,(int)DancersCfg[Dancer_i].zeropoint,0);
     /*
     response_buffer = my_malloc(hardware_configuration__get_packed_size(&DancerConfig));
     if (response_buffer)
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 8d41f3b9a..5fee72dc7 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -1338,22 +1338,22 @@ uint32_t UpdatePidDuringRun(HardwarePidControl *request)
         return ERROR;
 
 
-    if (request->derivativetime == true)
+    if (request->has_derivativetime == true)
     {
         MotorControlConfig[Motor_i].m_params.Kd = request->derivativetime;
         ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun Kd",__FILE__,Motor_i,(int)(request->derivativetime),RpWarning,0,0);
     }
-    if (request->proportionalgain == true)
+    if (request->has_proportionalgain == true)
     {
         MotorControlConfig[Motor_i].m_params.Kp = request->proportionalgain;
         ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun Kp",__FILE__,Motor_i,(int)(request->proportionalgain),RpWarning,0,0);
     }
-    if (request->integraltime == true)
+    if (request->has_integraltime == true)
     {
         MotorControlConfig[Motor_i].m_params.Ki = request->integraltime;
         ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun Ki",__FILE__,Motor_i,(int)(request->integraltime),RpWarning,0,0);
     }
-    if (request->epsilon == true)
+    if (request->has_epsilon == true)
     {
         MotorControlConfig[Motor_i].m_params.epsilon = request->epsilon;
         ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun epsilon",__FILE__,Motor_i,(int)(request->epsilon*10000),RpWarning,0,0);