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

1 files changed, 55 insertions, 9 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
index bbb3e4040..1920db03a 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
@@ -151,6 +151,39 @@ uint32_t MotorPidRequestMessage(HardwarePidControl* request)
         temp = temp*3/2;
     }
     DancerStopActivityLimit[Motor_i] = temp/(2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength);
+#ifdef FOUR_WINDERS
+    if (Motor_i == WINDER_MOTOR)
+    {
+        int j,winderMotorId;
+        for (j=0;j<3;j++)
+        {
+            winderMotorId = j+WINDER_2_MOTOR;
+            memcpy (&MotorsControl[winderMotorId],request,sizeof(HardwarePidControl));
+            if (MotorsControl[winderMotorId].pvinputfilterfactormode > MAX_CONTROL_SAMPLES)
+                MotorsControl[winderMotorId].pvinputfilterfactormode = MAX_CONTROL_SAMPLES;
+        #ifdef TEST_LONGER_PID_THREAD
+            MotorsControl[Motor_i].pvinputfilterfactormode = 10; //test longer control
+        #endif
+            for (i = 0;i < (int)MotorsControl[winderMotorId].pvinputfilterfactormode; i++)
+            {
+                MotorSamples[winderMotorId][i] = 0;  //reset the samples value for control beginning
+                MotorSpeedSamples[winderMotorId][i] = 0;
+            }
+            NormalizedErrorCoEfficient[winderMotorId] = (2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength);
+            temp = 1<<(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].resolutionbits);
+            temp=(NORMAL_COEF_DIVIDER*(temp-1)*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm);
+            NormalizedErrorCoEfficient[winderMotorId] = NormalizedErrorCoEfficient[winderMotorId] / temp;
+        //    uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES];
+            temp = 1<<(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].resolutionbits);
+            temp = (temp*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm);//*3/2);
+            if (Motor_i == WINDER_MOTOR)
+            {
+                temp = temp*3/2;
+            }
+            DancerStopActivityLimit[winderMotorId] = temp/(2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength);
+        }
+    }
+#endif
     return OK;
 }
 char DancerConfigPath[50] = "0://SysInfo//DancCfg.cfg";
@@ -159,31 +192,37 @@ uint32_t StoreDancerConfigMessage()
 {
     uint32_t status = OK;
   //  FRESULT Fresult = FR_OK;
-    HardwareConfiguration DancerConfig;
+    //HardwareConfiguration DancerConfig;
     HardwareDancer Dancers[MAX_SYSTEM_DANCERS];
   //  uint8_t* response_buffer;
   //  size_t response_size = 0;
     int Dancer_i;
 
-    hardware_configuration__init(&DancerConfig);
-    DancerConfig.dancers = (HardwareDancer**)my_malloc(sizeof(HardwareDancer*)*MAX_SYSTEM_DANCERS);
+    //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];
+        //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;
-        DancerConfig.dancers[Dancer_i]->zeropoint=Control_Read_Dancer_Position(Dancer_i,0);
-        DancerConfig.n_dancers++;
-        DancersCfg[Dancer_i].zeropoint = DancerConfig.dancers[Dancer_i]->zeropoint;
+        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
+    /*
     response_buffer = my_malloc(hardware_configuration__get_packed_size(&DancerConfig));
     if (response_buffer)
     {
@@ -208,11 +247,18 @@ uint32_t LoadDancerConfigMessage()
     MCU_E2PromRead(EEPROM_STORAGE_DANCER_0,&DancersCfg[0].zeropoint);
     MCU_E2PromRead(EEPROM_STORAGE_DANCER_1,&DancersCfg[1].zeropoint);
     MCU_E2PromRead(EEPROM_STORAGE_DANCER_2,&DancersCfg[2].zeropoint);
-
+#ifdef FOUR_WINDERS
+    MCU_E2PromRead(EEPROM_STORAGE_DANCER_3,&DancersCfg[3].zeropoint);
+    MCU_E2PromRead(EEPROM_STORAGE_DANCER_4,&DancersCfg[4].zeropoint);
+#endif
 
     status |= Report("Dancer 0 Store data eeprom ",__FILE__,__LINE__,0,RpWarning,(int)DancersCfg[0].zeropoint,0);
     status |= Report("Dancer 1 Store data eeprom ",__FILE__,__LINE__,1,RpWarning,(int)DancersCfg[1].zeropoint,0);
     status |= Report("Dancer 2 Store data eeprom ",__FILE__,__LINE__,2,RpWarning,(int)DancersCfg[2].zeropoint,0);
+#ifdef FOUR_WINDERS
+    status |= Report("Dancer 3 Store data eeprom ",__FILE__,__LINE__,3,RpWarning,(int)DancersCfg[3].zeropoint,0);
+    status |= Report("Dancer 4 Store data eeprom ",__FILE__,__LINE__,4,RpWarning,(int)DancersCfg[4].zeropoint,0);
+#endif
 
     return status;