diff --git a/controller/client/clStepClient.js b/controller/client/clStepClient.js
index 2228e09ff57516577a486499fe977daf9827417f..1e994e4104f8075dae90486592b2a22788295f6e 100644
--- a/controller/client/clStepClient.js
+++ b/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()
diff --git a/controller/client/clStepVM.js b/controller/client/clStepVM.js
index 776d79c93ab23b2f0d5f35f8e4b81808dda41846..55a20658808dffef9e99d107332f776c9bac8f69 100644
--- a/controller/client/clStepVM.js
+++ b/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) })
diff --git a/firmware/cl-step-controller/src/drivers/step_cl.cpp b/firmware/cl-step-controller/src/drivers/step_cl.cpp
index 71775db682a264890b307c119710c92be00f51f5..b05ac2652326d00393a2926ecf917eeebfd0288d 100644
--- a/firmware/cl-step-controller/src/drivers/step_cl.cpp
+++ b/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,
diff --git a/firmware/cl-step-controller/src/drivers/step_cl.h b/firmware/cl-step-controller/src/drivers/step_cl.h
index 3f74c90b2569a7cdc2b0eb65ab535d8eeb283aae..13c6893ae23277f3b9e8b64750b1ceba6cb9b31e 100644
--- a/firmware/cl-step-controller/src/drivers/step_cl.h
+++ b/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);
diff --git a/firmware/cl-step-controller/src/main.cpp b/firmware/cl-step-controller/src/main.cpp
index 638685d0cb26c0fc60da08edf258bb3cdcd7c613..2cacee8d2e46a215382e189da5335f7416e458f6 100644
--- a/firmware/cl-step-controller/src/main.cpp
+++ b/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;
}
diff --git a/log/2021-02-17_tuning-01.png b/log/2021-02-17_tuning-01.png
new file mode 100644
index 0000000000000000000000000000000000000000..7ee7bc78de3099aea7f6c76fe4d0691036a5f451
Binary files /dev/null and b/log/2021-02-17_tuning-01.png differ
diff --git a/log/2021-02-17_tuning-02.png b/log/2021-02-17_tuning-02.png
new file mode 100644
index 0000000000000000000000000000000000000000..29972fba3b8b75218d6add06fe715e3755c1a429
Binary files /dev/null and b/log/2021-02-17_tuning-02.png differ
diff --git a/log/cl-step-control-log.md b/log/cl-step-control-log.md
index b9757787ee7e146fe7e53504c5c1ee52e04eff99..9d68d21f15c5bda447454ec4f77131069359ac45 100644
--- a/log/cl-step-control-log.md
+++ b/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 ?
\ No newline at end of file
+## 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:
+
+
+
+```
+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.
+
+
+
+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