Initial Motors control testing

1 files changed, 62 insertions, 47 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
index 7119238de..c9876f89c 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
@@ -11,10 +11,16 @@
 #include "PMR/common/MessageContainer.pb-c.h"
 
 #include "thread.h"
-#define MAX_SYSTEM_DANCERS HARDWARE_DANCER_TYPE__RightDancer+1
-MotorConfigStruc  MotorsCfg[MAX_THREAD_MOTORS_NUM]={0};
+HardwareMotor  MotorsCfg[NUM_OF_MOTORS]={0};
+HardwarePidControl MotorsControl[MAX_THREAD_MOTORS_NUM] = {0};
+
+uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES] = {0};
+int MotorSamplePointer[MAX_THREAD_MOTORS_NUM] = {0};
+double NormalizedErrorCoEfficient[MAX_THREAD_MOTORS_NUM] = {0};
 InternalWinderConfigStruc InternalWinderCfg = {0};
-DancerConfigStruc DancersCfg[MAX_SYSTEM_DANCERS] = {0};
+HardwareDancer DancersCfg[MAX_SYSTEM_DANCERS] = {0};
+
+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};
 
 
 
@@ -33,8 +39,8 @@ uint32_t InternalWindingConfigMessage(JobSpool* request)
     InternalWinderCfg.segmentoffsetpulses = request->segmentoffsetpulses;
     InternalWinderCfg.spoolbackingrate = request->backingrate;
     InternalWinderCfg.startoffsetpulses = request->startoffsetpulses;
-    InternalWinderCfg.SpoolBottomBackingRate =  request->spoolbottombackingrate;
-    InternalWinderCfg.NumberOfRotationPerPassage = request->numberofrotationperpassage;
+    InternalWinderCfg.SpoolBottomBackingRate =  request->bottombackingrate;
+    InternalWinderCfg.NumberOfRotationPerPassage = request->rotationsperpassage;
     InternalWinderCfg.diameter = request->diameter;
 
     return status;
@@ -47,50 +53,63 @@ uint32_t MotorsConfigMessage(HardwareMotor * request)
     int Motor_i;
     MotorDriverConfigStruc MotorDriverConfig;
     Motor_i = request->hardwaremotortype;
-    if (Motor_i< MAX_THREAD_MOTORS_NUM)
+    /*for (i=0;i<MAX_THREAD_MOTORS_NUM;i++)
     {
-        MotorsCfg[Motor_i].id               = request->hardwaremotortype;
-        MotorsCfg[Motor_i].minfreq          = request->minfrequency;
-        MotorsCfg[Motor_i].maxfreq          = request->maxfrequency;
-        MotorsCfg[Motor_i].minmicrostep     = request->minmicrostep;
-        MotorsCfg[Motor_i].maxmicrostep     = request->maxmicrostep;
-        MotorsCfg[Motor_i].linearratio      = request->linearratio;
-        MotorsCfg[Motor_i].medianposition   = request->medianposition;
-        MotorsCfg[Motor_i].correctiongain   = request->correctiongain;
-        MotorsCfg[Motor_i].ratio2dryerspeed  = request->ratiotodryerspeed;
-        MotorsCfg[Motor_i].kp               = request->kp;
-        MotorsCfg[Motor_i].ki               = request->ki;
-        MotorsCfg[Motor_i].kd               = request->kd;
-        MotorsCfg[Motor_i].changeslope      = request->changeslope;
-        MotorsCfg[Motor_i].hightimeoutusec  = request->highlengthmicrosecond;
-        if (Motor_i == MOTOR_LDRIVING)
-            ThreadInitialTestStub(request);
-        /*
-        typedef struct
+        if (ThreadMotorIdToControlId[i] == request->hardwarepidcontroltype)
         {
-              uint32_t ACC; //set
-              uint32_t DEC;  //set
-              uint32_t MaxSpeed; //set
-              bool HasMicroStep;
-              uint32_t Microstep; //set
-              bool HasConfigWord;
-              uint16_t ConfigWord; //set
-        }MotorDriverConfigStruc;
-        */
-        MotorDriverConfig.ACC = MotorsCfg[Motor_i].changeslope;
-        MotorDriverConfig.DEC = MotorsCfg[Motor_i].changeslope;
-        MotorDriverConfig.MaxSpeed = MotorsCfg[Motor_i].maxfreq;
-        MotorDriverConfig.HasMicroStep = true;
-        MotorDriverConfig.Microstep = MotorsCfg[Motor_i].minmicrostep;
-        MotorDriverConfig.HasConfigWord = false;
-        MotorDriverConfig.ConfigWord = 0;
+            Motor_i = i;
+            break;
+        }
+    }
+    if (Motor_i == 0)
+    {
+        LOG_ERROR(request->hardwarepidcontroltype,"ERROR Control Id");
+        return ERROR;
+    }*/
+    //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,  &MotorDriverConfig);
+//        if (Motor_i == MOTOR_RDRIVING)
+//            ThreadInitialTestStub(request);
 
         return status;
+//    }
+//    else return Motor_i;
+
+}
+uint32_t MotorPidRequestMessage(HardwarePidControl* request)
+{
+    int Motor_i,i;
+    int temp;
+    for (i=0;i<MAX_THREAD_MOTORS_NUM;i++)
+    {
+        if (ThreadMotorIdToControlId[i] == request->hardwarepidcontroltype)
+        {
+            Motor_i = i;
+            break;
+        }
     }
-    else return Motor_i;
+    memcpy (&MotorsControl[Motor_i],request,sizeof(HardwarePidControl));
+    if (MotorsControl[Motor_i].pvinputfilterfactormode > MAX_CONTROL_SAMPLES)
+        MotorsControl[Motor_i].pvinputfilterfactormode = MAX_CONTROL_SAMPLES;
+    for (i = 0;i < MotorsControl[Motor_i].pvinputfilterfactormode; i++)
+        MotorSamples[Motor_i][i] = 0;  //reset the samples value for control beginning
+    NormalizedErrorCoEfficient[Motor_i] = (2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength);
+    temp = 1<<(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].resolutionbits);
+    temp=(100*(temp-1)*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm);
+    NormalizedErrorCoEfficient[Motor_i] = NormalizedErrorCoEfficient[Motor_i] / temp;
+//    uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES];
 
+    return OK;
 }
 uint32_t DancerConfigMessage(HardwareDancer * request)
 {
@@ -100,11 +119,7 @@ uint32_t DancerConfigMessage(HardwareDancer * request)
     Dancer_i = request->hardwaredancertype;
     if (Dancer_i<MAX_SYSTEM_DANCERS )
     {
-        DancersCfg[Dancer_i].id = Dancer_i;
-        DancersCfg[Dancer_i].fixorgradual = request->gradual;
-        DancersCfg[Dancer_i].k = request->k;
-        DancersCfg[Dancer_i].x = request->x;
-        DancersCfg[Dancer_i].pulsepermmspring = request->pulsepermmspring;
+        memcpy (&DancersCfg[Dancer_i],request,sizeof(HardwareDancer));
         return status;
     }
     else