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(() => {
// -------------------------------------------------------- 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()
......
......@@ -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) })
......
......@@ -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,
......
......@@ -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);
......
......@@ -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;
}
......
......@@ -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:
![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
Markdown is supported
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