Commit d8f98081 authored by Jake Read's avatar Jake Read
Browse files

tuned in, ready to wrap

parent da0fc1ee
...@@ -167,16 +167,18 @@ stopBtn.onClick(() => { ...@@ -167,16 +167,18 @@ stopBtn.onClick(() => {
// -------------------------------------------------------- PID Values // -------------------------------------------------------- PID Values
let pVal = new TextInput(10, 190, 100, 20, '-0.002') let pVal = new TextInput(10, 190, 100, 20, '-0.09')
let iVal = new TextInput(10, 220, 100, 20, '0.0') let iVal = new TextInput(10, 220, 100, 20, '-0.002')
let dVal = new TextInput(10, 250, 100, 20, '0.0') let iLimVal = new TextInput(10, 250, 100, 20, '0.8')
let aVal = new TextInput(10, 280, 100, 20, '0.01') let dVal = new TextInput(10, 280, 100, 20, '-0.0003')
let aDotVal = new TextInput(10, 310, 100, 20, '0.1') 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, 340, 94, 14, 'set PID / a')
let pidSetBtn = new Button(10, 370, 94, 14, 'set PID / a')
pidSetBtn.onClick(() => { pidSetBtn.onClick(() => {
let p = parseFloat(pVal.value) let p = parseFloat(pVal.value)
let i = parseFloat(iVal.value) let i = parseFloat(iVal.value)
let ilim = parseFloat(iLimVal.value)
let d = parseFloat(dVal.value) let d = parseFloat(dVal.value)
let a = parseFloat(aVal.value) let a = parseFloat(aVal.value)
let ad = parseFloat(aDotVal.value) let ad = parseFloat(aDotVal.value)
...@@ -184,7 +186,7 @@ pidSetBtn.onClick(() => { ...@@ -184,7 +186,7 @@ pidSetBtn.onClick(() => {
pidSetBtn.bad("bad parse", 1000) pidSetBtn.bad("bad parse", 1000)
return return
} }
vm.setPIDTerms([p, i, d]).then(() => { vm.setPIDTerms([p, i, ilim, d]).then(() => {
return vm.setAlphas([a, ad]) return vm.setAlphas([a, ad])
}).then(() => { }).then(() => {
pidSetBtn.good("ok", 500) pidSetBtn.good("ok", 500)
...@@ -194,7 +196,7 @@ pidSetBtn.onClick(() => { ...@@ -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(() => { angleRandoBtn.onClick(() => {
let targ = Math.random() * 360 let targ = Math.random() * 360
vm.setTarget(targ).then(() => { vm.setTarget(targ).then(() => {
...@@ -205,6 +207,16 @@ angleRandoBtn.onClick(() => { ...@@ -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 // -------------------------------------------------------- STARTUP LOCAL
let wscVPort = osap.vPort() let wscVPort = osap.vPort()
......
...@@ -123,10 +123,11 @@ export default function ClStepVM(osap, route) { ...@@ -123,10 +123,11 @@ export default function ClStepVM(osap, route) {
pidEP.addRoute(route, TS.endpoint(0, 6), 512) pidEP.addRoute(route, TS.endpoint(0, 6), 512)
this.setPIDTerms = (vals) => { this.setPIDTerms = (vals) => {
return new Promise((resolve, reject) => { 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[0], datagram, 0, true)
TS.write('float32', vals[1], datagram, 4, true) TS.write('float32', vals[1], datagram, 4, true)
TS.write('float32', vals[2], datagram, 8, true) TS.write('float32', vals[2], datagram, 8, true)
TS.write('float32', vals[3], datagram, 12, true)
pidEP.write(datagram).then(() => { pidEP.write(datagram).then(() => {
resolve() resolve()
}).catch((err) => { reject(err) }) }).catch((err) => { reject(err) })
......
...@@ -147,19 +147,21 @@ float Step_CL::get_angle_target(void){ ...@@ -147,19 +147,21 @@ float Step_CL::get_angle_target(void){
// PIDs // PIDs
volatile float pTerm = -0.004F; volatile float pTerm = -0.01F;
volatile float iTerm = 0.0F; volatile float iTerm = 0.0F;
volatile float iLimit = 0.01F;
volatile float dTerm = -0.001F; volatile float dTerm = -0.001F;
volatile float effort = 0.0F; 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; pTerm = p;
iTerm = i; iTerm = i;
iLimit = ilim;
dTerm = d; dTerm = d;
} }
volatile float a_alpha = 0.01F; // trust in measurement volatile float a_alpha = 0.2F; // trust in measurement
volatile float a_dot_alpha = 0.1F; volatile float a_dot_alpha = 0.05F;
void Step_CL::set_alpha_values(float a, float adot){ void Step_CL::set_alpha_values(float a, float adot){
a_alpha = a; a_alpha = a;
...@@ -187,6 +189,7 @@ volatile float a_est = 0.0F; ...@@ -187,6 +189,7 @@ volatile float a_est = 0.0F;
volatile float a_last_est = 0.0F; volatile float a_last_est = 0.0F;
volatile float a_dot = 0.0f; volatile float a_dot = 0.0f;
volatile float a_dot_last = 0.0f; volatile float a_dot_last = 0.0f;
volatile float integral = 0.0f;
volatile float _pa; // phase angle volatile float _pa; // phase angle
...@@ -226,9 +229,16 @@ void ENC_AS5047::on_read_complete(uint16_t result){ ...@@ -226,9 +229,16 @@ void ENC_AS5047::on_read_complete(uint16_t result){
// set history // set history
a_last_est = a_est; a_last_est = a_est;
a_dot_last = a_dot; 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, // calculate output,
float output = pTerm * (a_est - _ta) + dTerm * a_dot; float output = pTerm * (a_est - _ta) + dTerm * a_dot + integral;
effort = output; effort = output;
clamp(&output, -1.0F, 1.0F); clamp(&output, -1.0F, 1.0F);
// _tc (nominally torque output / effort) is a local variable to this file scope, // _tc (nominally torque output / effort) is a local variable to this file scope,
......
...@@ -37,7 +37,7 @@ class Step_CL { ...@@ -37,7 +37,7 @@ class Step_CL {
float get_angle_target(void); float get_angle_target(void);
float get_output_effort(void); float get_output_effort(void);
// pid set // 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); void set_alpha_values(float a, float adot);
// read stat // read stat
uint16_t get_last_enc_reading(void); uint16_t get_last_enc_reading(void);
......
...@@ -125,8 +125,9 @@ boolean onPIDPut(uint8_t* data, uint16_t len){ ...@@ -125,8 +125,9 @@ boolean onPIDPut(uint8_t* data, uint16_t len){
uint16_t ptr = 0; uint16_t ptr = 0;
chunk_float32 pc = { .bytes = { data[ptr ++], data[ptr ++], data[ptr ++], data[ptr ++] }}; 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 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 ++] }}; 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; return true;
} }
......
...@@ -11,6 +11,11 @@ With 2x A4950s on the DAC, AS5047P on the encoder, etc. ...@@ -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. - 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 ## 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? - 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 ...@@ -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. 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: ## 2021 02 17
- PID remote-set
- target-pos remote-set So, got to design a little rotary drive last night, that should help establish a realistic baseline for 'the plant' so to speak.
- tuning attempts
- auto-tune ? 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
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment