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

1 files changed, 58 insertions, 14 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
index 4454565c1..38c0f86f1 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
@@ -17,12 +17,15 @@
 #include "Common/SWUpdate/FileSystem.h"
 #include "drivers/Flash_Memory/fatfs/ff.h"
 #include "drivers/SSI_Comm/Dancer/Dancer.h"
+#include "drivers/Flash_ram/FlashProgram.h"
+
+#include "drivers/Flash_ram/MCU_E2Prom.h"
 
 #include "thread.h"
 MotorDriverConfigStruc  MotorsCfg[NUM_OF_MOTORS]={0};
 HardwarePidControl MotorsControl[MAX_THREAD_MOTORS_NUM] = {0};
 
-int32_t MotorSpeedSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES] = {0};
+double MotorSpeedSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES] = {0};
 int MotorSpeedSamplePointer[MAX_THREAD_MOTORS_NUM] = {0};
 
 int32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES] = {0};
@@ -39,11 +42,19 @@ int32_t BreakSensordebouncetimemilli = BREAK_SENSOR_LIMIT;
 HardwarePidControlType ThreadMotorIdToControlId[MAX_THREAD_MOTORS_NUM] = {HARDWARE_PID_CONTROL_TYPE__MotorFeeder,HARDWARE_PID_CONTROL_TYPE__MotorDryer,HARDWARE_PID_CONTROL_TYPE__MotorPooler,HARDWARE_PID_CONTROL_TYPE__MotorWinder,0};
 
 //********************************************************************************************************************
-uint32_t MotorsConfigMessage(HardwareMotor * request)
+uint32_t MotorsConfigMessage(HardwareConfiguration * HWrequest)
 {
     uint32_t status = PASSED;
-    TimerMotors_t Motor_i;
-    Motor_i = request->hardwaremotortype;
+    TimerMotors_t Motor_i,MotorId;
+    HardwareMotor *request;
+    if (HWrequest == NULL)
+        return ERROR;
+    for (MotorId = 0; MotorId < HWrequest->n_motors ; MotorId++)
+    {
+        request = HWrequest->motors[MotorId];
+        if (request == NULL)
+            return ERROR;
+        Motor_i = request->hardwaremotortype;
         MotorsCfg[Motor_i].configword = request->configword;
         MotorsCfg[Motor_i].hardwaremotortype = request->hardwaremotortype;
         MotorsCfg[Motor_i].minfrequency = request->minfrequency;
@@ -56,7 +67,7 @@ uint32_t MotorsConfigMessage(HardwareMotor * request)
         MotorsCfg[Motor_i].pulseperround = request->pulseperround;
         MotorsCfg[Motor_i].pulleyradius = request->pulleyradius;
         MotorsCfg[Motor_i].configword = request->configword;
-        if(MotorDriverResponse[Motor_i].DriverType == VoltageCombinedMotDriver)
+        if(MotorDriverResponse[Motor_i].DriverType == CombinrdMotDriver)
         {
             MotorsCfg[Motor_i].directionthreadwize = !(request->directionthreadwize);//PowerSTEP01
         }
@@ -79,13 +90,21 @@ uint32_t MotorsConfigMessage(HardwareMotor * request)
         MotorsCfg[Motor_i].fnslpdec = request->fnslpdec;
         MotorsCfg[Motor_i].fsspd = request->fsspd;
 
-        status = MotorConfig( Motor_i,  &MotorsCfg[Motor_i]);
+        MotorsCfg[Motor_i].gatecfg1 = request->gatecfg1;
+        MotorsCfg[Motor_i].gatecfg2 = request->gatecfg2;
+        MotorsCfg[Motor_i].tvalhold = request->tvalhold;
+        MotorsCfg[Motor_i].tvalrun  = request->tvalrun ;
+        MotorsCfg[Motor_i].tvalacc  = request->tvalacc ;
+        MotorsCfg[Motor_i].tvaldec  = request->tvaldec ;
+        MotorsCfg[Motor_i].tfast    = request->tfast   ;
+        MotorsCfg[Motor_i].tonmin   = request->tonmin  ;
+        MotorsCfg[Motor_i].toffmin  = request->toffmin ;
+
+            status = MotorConfig( Motor_i,  &MotorsCfg[Motor_i]);
 //        if (Motor_i == MOTOR_RDRIVING)
 //            ThreadInitialTestStub(request);
-
+    }
         return status;
-//    }
-//    else return Motor_i;
 
 }
 uint32_t MotorPidRequestMessage(HardwarePidControl* request)
@@ -106,18 +125,18 @@ uint32_t MotorPidRequestMessage(HardwarePidControl* request)
 #ifdef TEST_LONGER_PID_THREAD
     MotorsControl[Motor_i].pvinputfilterfactormode = 10; //test longer control
 #endif
-    for (i = 0;i < MotorsControl[Motor_i].pvinputfilterfactormode; i++)
+    for (i = 0;i < (int)MotorsControl[Motor_i].pvinputfilterfactormode; i++)
     {
         MotorSamples[Motor_i][i] = 0;  //reset the samples value for control beginning
         MotorSpeedSamples[Motor_i][i] = 0;
     }
     NormalizedErrorCoEfficient[Motor_i] = (2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength);
     temp = 1<<(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].resolutionbits);
-    temp=(10*(temp-1)*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm);
+    temp=(NORMAL_COEF_DIVIDER*(temp-1)*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm);
     NormalizedErrorCoEfficient[Motor_i] = NormalizedErrorCoEfficient[Motor_i] / 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*2);
+    temp = (temp*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm*3/2);
     DancerStopActivityLimit[Motor_i] = temp/(2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength);
     return OK;
 }
@@ -145,12 +164,24 @@ uint32_t StoreDancerConfigMessage()
         DancerConfig.n_dancers++;
         DancersCfg[Dancer_i].zeropoint = DancerConfig.dancers[Dancer_i]->zeropoint;
     }
+    MCU_E2PromProgram(EEPROM_STORAGE_DANCER_0,DancersCfg[0].zeropoint);
+    MCU_E2PromProgram(EEPROM_STORAGE_DANCER_1,DancersCfg[1].zeropoint);
+    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);
+
     response_buffer = my_malloc(hardware_configuration__get_packed_size(&DancerConfig));
     if (response_buffer)
     {
         response_size = hardware_configuration__pack(&DancerConfig, response_buffer);
     }
-    Fresult = FileWrite(response_buffer,response_size,DancerConfigPath);
+    Fresult = FileWrite(response_buffer,response_size,DancerConfigPath,BIOS_WAIT_FOREVER);
+    EraseFlashSection(DANCERS_MAP_IN_FLASH,1024);
+    for (Dancer_i = 0; Dancer_i < MAX_SYSTEM_DANCERS; Dancer_i++)
+    {
+        ReadAppAndProgram(DANCERS_MAP_IN_FLASH, sizeof(Dancers), Dancers);
+    }
+
+
     my_free(response_buffer);
 
     return Fresult;
@@ -163,6 +194,10 @@ uint32_t LoadDancerConfigMessage()
 
     HardwareConfiguration *DancerConfig;
     int Dancer_i;
+    HardwareDancer DancersCfg1[MAX_SYSTEM_DANCERS] = {0};
+    HardwareDancer DancersCfg2[MAX_SYSTEM_DANCERS] = {0};
+
+    memcpy(DancersCfg1,(void *)DANCERS_MAP_IN_FLASH,sizeof(DancersCfg1));
 
     Fresult = FileRead(DancerConfigPath, &Bytes, &buffer);
     if (Fresult == FR_OK)
@@ -170,11 +205,20 @@ uint32_t LoadDancerConfigMessage()
         DancerConfig = hardware_configuration__unpack(NULL, Bytes, buffer);
         for (Dancer_i = 0; Dancer_i < DancerConfig->n_dancers ; Dancer_i++)
         {
-            DancersCfg[Dancer_i].zeropoint = DancerConfig->dancers[Dancer_i]->zeropoint;
+            DancersCfg2[Dancer_i].zeropoint = DancerConfig->dancers[Dancer_i]->zeropoint;
         }
         hardware_configuration__free_unpacked(DancerConfig,NULL);
         free (buffer);
     }
+    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);
+
+
+    Report("Dancer 0 Store data flash, internal flash, eeprom ",__FILE__,DancersCfg[0].zeropoint,(int)DancersCfg1[0].zeropoint,RpWarning,(int)DancersCfg2[0].zeropoint,0);
+    Report("Dancer 1 Store data flash, internal flash, eeprom ",__FILE__,DancersCfg[1].zeropoint,(int)DancersCfg1[1].zeropoint,RpWarning,(int)DancersCfg2[1].zeropoint,0);
+    Report("Dancer 2 Store data flash, internal flash, eeprom ",__FILE__,DancersCfg[2].zeropoint,(int)DancersCfg1[2].zeropoint,RpWarning,(int)DancersCfg2[2].zeropoint,0);
+
     return Fresult;
 
 }