wip

python/MXTuning.py

@@ -238,21 +238,14 @@ class MXTuning(Tuning):
       s='  //input values\n'
       for i in range(len(u)):
         s+='  double {u}=Mptr->{u};\n'.format(u=u[i])
-      prog+=s+'''\n
+      prog+=s+'''
   if (Mptr->ClosedLoop)
   {
-    return iqCmd;
-  }
-  else
-  {
-    Mptr->Servo.Integrator=0.0;
-    return 0.0;
-  }
 '''
 
-      s='  //x[n+1]=A*x[n]+B*u;\n'
+      s='    //x[n+1]=A*x[n]+B*u;\n'
       for i in range(A.shape[0]):
-        s+='  x[%d]='%i
+        s+='    x[%d]='%i
         for j in range(A.shape[1]):
           s+='%+28.22g*_x[%d]'%(A[i,j],j)
         for j in range(B.shape[0]):
@@ -260,29 +253,37 @@ class MXTuning(Tuning):
         s+=';\n'
       prog+=s+'\n'
 
-      s='  //y=C*x[n]+D*x[n];\n'
+      s='    //y=C*x[n]+D*x[n];\n'
       for i in range(C.shape[0]):
-        s+='  %s='%y[i]
+        s+='    %s='%y[i]
         for j in range(C.shape[1]):
           s+='%+28.22g*_x[%d]'%(C[i,j],j)
         s+=';\n'
       prog+=s+'\n'
 
 
-      prog+='''  iqCmd=DesPos*{V}-{y};
-  if (iqCmd>maxDac)
-  {{
-    iqCmd=maxDac;
+      prog+='''    iqCmd=DesPos*{V}-{y};
+    if (iqCmd>maxDac)
+    {{
+      iqCmd=maxDac;
+    }}
+    else
+    {{
+      if (iqCmd<-maxDac)
+      {{
+        iqCmd=-maxDac;
+      }}
+    }}
+    //return iqCmd;
+    pshm->P[200{motid}]=iqCmd;  //lowpass of Position error
+    return pshm->ServoCtrl(Mptr);
   }}
   else
   {{
-    if (iqCmd<-maxDac)
-    {{
-      iqCmd=-maxDac;
-    }}
+    Mptr->Servo.Integrator=0.0;
+    return 0.0;
   }}
-  return iqCmd;
-}}'''.format(V=V[0,0],y=y[0])
+}}'''.format(V=V[0,0],y=y[0],motid=motid)
 
       hdr='''double obsvr_servo_ctrl_{motid}(MotorData *Mptr);
 EXPORT_SYMBOL(obsvr_servo_ctrl_{motid});'''.format(motid=motid)
@@ -336,7 +337,7 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
 
     #plt.ion()
     #args.host='MOTTEST-CPPM-CRM0573'
-    #args.host=None
+    args.host=None
     if args.host is None:
       comm=gt=None
     else: