Commit 651d8815 authored by Jake Read's avatar Jake Read
Browse files

find position noise, end notes, fix later

parent 86c2b1f1
......@@ -77,7 +77,7 @@ void flash_write_page(void){
flashClass.erase(block_ptr, BYTES_PER_BLOCK);
sysError("writing 0x" + String((uint32_t)block_ptr));
flashClass.write(block_ptr, (const uint8_t*)buffer, BYTES_PER_BLOCK);
delay(10);
delay(1);
}
void flash_write_value(float val){
......@@ -124,10 +124,15 @@ void Step_CL::run_torque_loop(void){
#define MAP_7p2_TO_1 (1.0F / 7.2F)
#define TICKS_PER_SEC 50000.0F
#define SECS_PER_TICK 1.0F / TICKS_PER_SEC
volatile float _ra;
volatile float _ra_last;
volatile float _deg_s; // real angle / sec
volatile float _pa;
volatile float q = 0.05F;//0.125F; // process noise covariance
volatile float r = 1.0F; // measurement noise covariance
volatile float measurement; // the measurement...
volatile float a = 0.0F; // the estimate on a
volatile float p = 100.0F; // estimation error covariance
volatile float k; // kalman gain
volatile float _deg_s; // real angle / sec
volatile float _pa; // phase angle
float Step_CL::get_deg_sec(void){
return _deg_s;
......@@ -135,12 +140,20 @@ float Step_CL::get_deg_sec(void){
void ENC_AS5047::on_read_complete(uint16_t result){
if(step_cl->is_calibrating) return;
_ra = lut[result * 2]; // the real angle (position 0-360)
// (1) get the new measurement,
measurement = lut[result * 2];
p = p + q;
k = p / (p + r);
a = a + k * (measurement - a);
p = (1 - k) * p;
// write a 'real' speed value
_deg_s = measurement;
_pa = lut[result * 2 + 1]; // the phase angle (0 - 1 in a sweep of 4 steps)
// want to calculate speeds,
// just this is not going to be good enough: have to wrap that angle
_deg_s = (_ra - _ra_last) * TICKS_PER_SEC;
_ra_last = _ra; // upd8 for next tick
// this is the phase angle we want to apply, 90 degs off & wrap't to 1
if(step_cl->get_torque() < 0){
_pa -= 0.25; // 90* phase swop
......
......@@ -1116,12 +1116,73 @@ Discrete deeeeeerivatiiiives
I think this is probably not hard, however, doing it fast is maybe more challenging. I also need to set up some infrastructure I think to do it well: this js looping situation. Maybe that first.
- phase advance can be 'roughly' to do 0.25 + min(speed * x, 0.25) where x is set such that speed * x goes to 0.25 around 2000rpm (by intuition) but really I should make a spreadsheet to figure where (if current writing is direct) the pole-rotation time starts to sync with the 50khz, you know what I mean?
- also read ur datasheets abt current chopping time ?
- in the end, phase advance probably just tuned
- do manuel-set phase advance parameter,
- do js interface for tc down, degs/sec up, plot on loop
- see about sweeping parameter space, learning real torque
- measure against real values, from, what?
- to learn PID, command PID params down, return accumulated error over set evaluation period, do learning over sample gcodes ? means integration into bus, etc
- seems likely that PID learning is related to acceleration setting in smoothie, maybe somehow do multiple accel vals? also return w/ pid an average effort used, and a maximum? ideally tunes motor PID, and per-axis accel all together
\ No newline at end of file
OK, have this basically up and - yes - there's lots of noise in the measurement, and the wrap is a struggle. I am tempted to plot a little chart so that I can visualize this more-better as opposed to just watching values flash across the page.
Yeah, starting that up, now have this very noisy signal:
![noise](2020-10-17_noisy-speed.png)
I think first order is eliminating those spikes that (?) I would guess are during wraps, and then doing better by smoothing over readings. Although I am getting some readings north of 16 million degs / sec: those would be the wraps (360 * 50k), most of the noise is around 60k degs / sec, where it appears as if things have rotated ~ 1 full degree each tick. Those seem odd: one encoder tick is only 0.022 degrees, so that would indicate over 50 ticks of noise, which seems like a tonne more than I was observing.
Tried the cos/sin/atan2 wrap method: way too slow.
I suspect it will eventually be very useful to bring the whole calibration table back and plot it, and I wonder if I should just get to filtering things to see if I can eliminate these 1 degree noisy events.
A simple / dummy filter *sort of works*
```cpp
// want to calculate speeds, need to wrap the pair
_ra_l0 = 0;
_ra_l1 = 0;
// to filter...
for(uint8_t i = 0; i < 6; i ++){
_ra_l0 += _ra_last[i];
_ra_l1 += _ra_last[i + 6];
}
_diff = (_ra_l1 - _ra_l0) / 6;
_deg_s = _diff * TICKS_PER_SEC;
// upd8
for(uint8_t i = 0; i < 11; i ++){
_ra_last[i] = _ra_last[i + 1];
}
_ra_last[11] = _ra;
```
But not really. I think the real answer is a kalman filter, so I should give this a go.
http://robotsforroboticists.com/kalman-filtering/
https://github.com/hmartiro/kalman-cpp
This would be a real piece of maths homework for me. A more likely approach would be to fit a linear segment to a series of measurements, and use the endpoint of that segment as the 'predicted state' ...
I also have this wrapping-around problem no matter how I kick it. For a last attempt at a hacked filter, I might try writing down deltas and wrapping deltas, then filtering on those (instead of absolute angles).
... state estimate is x and xdot
... state estimator is x = x + xdot
... new estimate for xdot is the
... (old estimate for xdot * alpha) + (measurement * (1 - alpha))
... alpha is an expecation of how smooth
... new xdot uses old estimate of x, and new measurement
OK, sam gave me some beta on how to think about this in simpler terms. A 1D Kalman filter is e-z, seems like, it's kind of a smoothing filter.
Oh jeez, I don't think I've done this right: or the wrapping situation is really messing things up.
I've implemented [this simple 1d kalman](https://github.com/bachagas/Kalman/blob/master/Kalman.h) though I don't think I understand it... it does do stuff. But does it do more than a simple smoothing filter?
In any case, I gave up tuning and just piped position back, here's what noise there looks like:
![noise](2020-10-17_noisy-pos.png)
Alltogether too much, I think I should read in the whole calibration table and plot it, to see what that looks like... I suspect there are some stray jumps there: maybe at interval boundaries or something like that.
So next up, I'll write a little routine that sequentially queries for LUT entries, in blocks, and plots those. Once I squash that problem (ominous foreshadowing) I think I'll have a much better time filtering. X first, then Xdot, yeah.
So - going to put this down for a minute.
**when we return**
- find the jumps in position readings: pull the LUT up and plot it: check RA and PA.
- the current track is just trying to get a decent, non-noisy position and velocity estimate(s) from the thing. then you can use the vel estimate to write phase advance term per your notes above, and also to measure accel (xddot) = f, for your 2D torque-estimate lut.
- *then* you want to do both
- measuring torque guess success: use calibrated loadcell or somesuch?
- (or) simply go forwards, towards using this in a machine: build a PID position controller, and tune it, maybe using a JS setPID() command, and remotely evaluate some set of moves, to tune. an auto-cal / auto-tune routine, etc. likely via the bus & with smoothie dishing moves, maybe just over usb, who knows
\ 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