tuned in, ready to wrap

controller/client/clStepClient.js

@@ -167,16 +167,18 @@ stopBtn.onClick(() => {
 
 // -------------------------------------------------------- PID Values
 
-let pVal = new TextInput(10, 190, 100, 20, '-0.002')
-let iVal = new TextInput(10, 220, 100, 20, '0.0')
-let dVal = new TextInput(10, 250, 100, 20, '0.0')
-let aVal = new TextInput(10, 280, 100, 20, '0.01')
-let aDotVal = new TextInput(10, 310, 100, 20, '0.1')
-
-let pidSetBtn = new Button(10, 340, 94, 14, 'set PID / a')
+let pVal = new TextInput(10, 190, 100, 20, '-0.09')
+let iVal = new TextInput(10, 220, 100, 20, '-0.002')
+let iLimVal = new TextInput(10, 250, 100, 20, '0.8')
+let dVal = new TextInput(10, 280, 100, 20, '-0.0003')
+let aVal = new TextInput(10, 310, 100, 20, '0.2')
+let aDotVal = new TextInput(10, 340, 100, 20, '0.05')
+
+let pidSetBtn = new Button(10, 370, 94, 14, 'set PID / a')
 pidSetBtn.onClick(() => {
   let p = parseFloat(pVal.value)
   let i = parseFloat(iVal.value)
+  let ilim = parseFloat(iLimVal.value)
   let d = parseFloat(dVal.value)
   let a = parseFloat(aVal.value)
   let ad = parseFloat(aDotVal.value)
@@ -184,7 +186,7 @@ pidSetBtn.onClick(() => {
     pidSetBtn.bad("bad parse", 1000)
     return
   }
-  vm.setPIDTerms([p, i, d]).then(() => {
+  vm.setPIDTerms([p, i, ilim, d]).then(() => {
     return vm.setAlphas([a, ad])
   }).then(() => {
     pidSetBtn.good("ok", 500)
@@ -194,7 +196,7 @@ pidSetBtn.onClick(() => {
   })
 })
 
-let angleRandoBtn = new Button(10, 370, 94, 14, 'targ')
+let angleRandoBtn = new Button(10, 400, 94, 14, 'targ')
 angleRandoBtn.onClick(() => {
   let targ = Math.random() * 360 
   vm.setTarget(targ).then(() => {
@@ -205,6 +207,16 @@ angleRandoBtn.onClick(() => {
   })
 })
 
+let angleWrapBtn = new Button(10, 430, 94, 14, 'wrap')
+angleWrapBtn.onClick(() => {
+  vm.setTarget(360.0).then(() => {
+    angleWrapBtn.good("ok", 500)
+  }).catch((err) => {
+    console.error(err)
+    angleWrapBtn.bad("err", 500)
+  })
+})
+
 // -------------------------------------------------------- STARTUP LOCAL
 
 let wscVPort = osap.vPort()

controller/client/clStepVM.js

@@ -123,10 +123,11 @@ export default function ClStepVM(osap, route) {
   pidEP.addRoute(route, TS.endpoint(0, 6), 512)
   this.setPIDTerms = (vals) => {
     return new Promise((resolve, reject) => {
-      let datagram = new Uint8Array(12)
+      let datagram = new Uint8Array(16)
       TS.write('float32', vals[0], datagram, 0, true)
       TS.write('float32', vals[1], datagram, 4, true)
       TS.write('float32', vals[2], datagram, 8, true)
+      TS.write('float32', vals[3], datagram, 12, true)
       pidEP.write(datagram).then(() => {
         resolve()
       }).catch((err) => { reject(err) })

firmware/cl-step-controller/src/drivers/step_cl.cpp

@@ -147,19 +147,21 @@ float Step_CL::get_angle_target(void){
 
 // PIDs
 
-volatile float pTerm = -0.004F;
+volatile float pTerm = -0.01F;
 volatile float iTerm = 0.0F;
+volatile float iLimit = 0.01F;
 volatile float dTerm = -0.001F;
 volatile float effort = 0.0F;
 
-void Step_CL::set_pid_values(float p, float i, float d){
+void Step_CL::set_pid_values(float p, float i, float ilim, float d){
     pTerm = p;
     iTerm = i;
+    iLimit = ilim;
     dTerm = d;
 }
 
-volatile float a_alpha = 0.01F;       // trust in measurement 
-volatile float a_dot_alpha = 0.1F;
+volatile float a_alpha = 0.2F;       // trust in measurement 
+volatile float a_dot_alpha = 0.05F;
 
 void Step_CL::set_alpha_values(float a, float adot){
     a_alpha = a;
@@ -187,6 +189,7 @@ volatile float a_est = 0.0F;
 volatile float a_last_est = 0.0F;
 volatile float a_dot = 0.0f;
 volatile float a_dot_last = 0.0f;
+volatile float integral = 0.0f;
 
 volatile float _pa;         // phase angle 
 
@@ -226,9 +229,16 @@ void ENC_AS5047::on_read_complete(uint16_t result){
     // set history 
     a_last_est = a_est;
     a_dot_last = a_dot;
+    // calc integral
+    integral += iTerm * (a_est - _ta);  // properly there would be this term: * (US_PER_LOOP / 1000000);
+    if(integral > iLimit){              // to make the 'integral' meaningful w/r/t time, however, this causes 
+        integral = iLimit;              // what I expect are numerical instabilities or something 
+    } else if (integral < (-iLimit)){   // as we end up doing like 10000 * 0.0001 * err ... idk 
+        integral = -iLimit;
+    }
 
     // calculate output, 
-    float output = pTerm * (a_est - _ta) + dTerm * a_dot;
+    float output = pTerm * (a_est - _ta) + dTerm * a_dot + integral;
     effort = output;
     clamp(&output, -1.0F, 1.0F);
     // _tc (nominally torque output / effort) is a local variable to this file scope, 

firmware/cl-step-controller/src/drivers/step_cl.h

@@ -37,7 +37,7 @@ class Step_CL {
         float get_angle_target(void);
         float get_output_effort(void);
         // pid set 
-        void set_pid_values(float p, float i, float d);
+        void set_pid_values(float p, float i, float ilim, float d);
         void set_alpha_values(float a, float adot);
         // read stat 
         uint16_t get_last_enc_reading(void);

firmware/cl-step-controller/src/main.cpp

@@ -125,8 +125,9 @@ boolean onPIDPut(uint8_t* data, uint16_t len){
   uint16_t ptr = 0;
   chunk_float32 pc = { .bytes = { data[ptr ++], data[ptr ++], data[ptr ++], data[ptr ++] }};
   chunk_float32 ic = { .bytes = { data[ptr ++], data[ptr ++], data[ptr ++], data[ptr ++] }};
+  chunk_float32 ilc = { .bytes = { data[ptr ++], data[ptr ++], data[ptr ++], data[ptr ++] }};
   chunk_float32 dc = { .bytes = { data[ptr ++], data[ptr ++], data[ptr ++], data[ptr ++] }};
-  step_cl->set_pid_values(pc.f, ic.f, dc.f);
+  step_cl->set_pid_values(pc.f, ic.f, ilc.f, dc.f);
   return true;
 }
 

log/cl-step-control-log.md

@@ -11,6 +11,11 @@ With 2x A4950s on the DAC, AS5047P on the encoder, etc.
 
 - haven't tested with '+ve' signed calibrations, i.e. if motor += magnetic step does encoder += tick step. 
 
+## Likely Improvements
+
+- encoder value filtering is a PITA, see note from 2021-02-17 about filtering and time constant. change loop structure so that we (1) sample the encoder as fast as possible, always, maybe 50kHz, and then (2) filter that with a heavier first exponential filter (keeping some small time constant as a target), and (3) run the actual control loop with this filtered value 
+- even better, run an [alpha beta filter](https://en.wikipedia.org/wiki/Alpha_beta_filter) underneath the control loop, as fast as possible, always. 
+
 ## Evaluation
 
 - static holding, watch encoder wobble when torque applied (can see ticks moving around even w/o motor torque being overcome), measure again with closed loop: can we hold *better* than static pointing? 
@@ -1283,8 +1288,68 @@ It occurs to me also that this particular case (motor on desk, nothing else) is
 
 Yeah, tuning will be hard, have the subsystems now though. I am suspicious of the heavy filtering - especially as I think it might be shifting the d-term signal phase to be 180' or so out of phase, making for oscillations. In any case, tomorrow, a serious tuning / pid-mastery-attempt session. 
 
-So, next would do:
-- PID remote-set
-- target-pos remote-set 
-- tuning attempts 
-- auto-tune ? 
+## 2021 02 17
+
+So, got to design a little rotary drive last night, that should help establish a realistic baseline for 'the plant' so to speak. 
+
+Currently:
\ No newline at end of file
+
+![tn](2021-02-17_tuning-01.png)
+
+```
+P   -0.09
+I   0
+D   -0.0003
+
+A   0.2
+A*  0.05
+```
+
+This feels... OK. I need an I term, and an I limit... which I figure should be (?) ~ 0.2, to start. I can end up adding oscillation and is a PITA to tune IIRC. In a lot of ways, it's another 'p' term. 
+
+![tn](2021-02-17_tuning-01.png)
+
+I figured at the start that a relatively small iLimit would be the move, but a larger limit feels better to help small disturbances... i.e. it pins the torque to the max when there's a resting force on the rotor (as in machining / etc). 
+
+My current winners are:
+
+```
+P       -0.09
+I       -0.002
+Ilim    0.8
+D       -0.0003
+
+A       0.2
+A*      0.05
+```
+
+OK, I am satisfied with these for the time being, time now is to apply it to a real system. I have also a few things I want to know:
+
+### Time Constant of the Exponential Filter?
+
+I was suspecting earlier that my filters were too aggressive (alpha 0.01), causing effectively a signal phase delay that was driving oscillations. This was confirmed (psuedo-scientific / intuitive sense) when turning the filter values 'down' (to 0.2) caused oscillations to stop, and allowed me to tune my P values back up without bringing on more oscillation. 
+
+So, what's the real maths here? Shouldn't be that hard to find on the internet. Yeah, from [the wiki](https://en.wikipedia.org/wiki/Exponential_smoothing) it looks like (if the sampling interval `delT` is fast relative the time constant)
+
+`tc = delT / alpha`
+
+So for an alpha around 0.01, with our sampling rate of 10kHz or 0.0001 the time constant was ~ 0.1 seconds, or 100ms, which is a whole lot in these terms - and the oscillations I observed seemed around 10Hz (maybe faster) so, yes, ok, we cannot just filter this into oblivion. 
+
+However, we could sample the encoder more often (I think 50kHz was the hardware limit) and filter that, then run the PID loop on a pre-filtered value. I'm going to keep a list of 'likely improvements' here, that being one of them. 
+
+### The Alpha Beta Filter?
+
+This also seems primo, the [alpha beta filter](https://en.wikipedia.org/wiki/Alpha_beta_filter) is basically a 2d kalman filter, IIRC from conversations with Sam. This is basically what we do when we know that i.e. our previous speed estimate is likely to give us a good expected value for our new position measurement. 
+
+Seems a bit tricky algorithmically because it is easy to become confused about time-steps / previous values / etc - and besides, we are using the position measurements to take derivatives that we then use to improve our position measurements, seems a bit cyclical. 
+
+In any case, I am not going to go down this rabbit hole now, but it's there for interest. 
+
+### Unrolling the Angles
+
+So, then, next is having target set-points that are not directly related to the motor. I wonder if actually the way to do this is actually to add another layer that runs on top of the servo controller and, given a set-point with some 'RPU' (rotations per unit), calculates the rotations to get there, and keeps feeding a new target to the lower level hardware... nah, this won't work because eventually we will have some points that are at ~ 359.9 degs or so and it needs to intelligently servo from 0.1 degs to that, not around the other side.
+
+So! There must be an established way to do this... 
+
+- wrap around 360, test with 'targ 360' key 
+- supervisor for offsets / etc ? 
\ No newline at end of file