diff --git a/firmware/cl-step-controller/src/drivers/step_cl.cpp b/firmware/cl-step-controller/src/drivers/step_cl.cpp
index 383c1edaf7aa6a4aca576027566a4667e484842f..051cb3b1409e619109c0680f49a2a0963ae41ee7 100644
--- a/firmware/cl-step-controller/src/drivers/step_cl.cpp
+++ b/firmware/cl-step-controller/src/drivers/step_cl.cpp
@@ -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
diff --git a/log/2020-10-17_noisy-pos.png b/log/2020-10-17_noisy-pos.png
new file mode 100644
index 0000000000000000000000000000000000000000..a10daf3485905f6615a10e4e66d476a1cefd76fe
Binary files /dev/null and b/log/2020-10-17_noisy-pos.png differ
diff --git a/log/2020-10-17_noisy-speed.png b/log/2020-10-17_noisy-speed.png
new file mode 100644
index 0000000000000000000000000000000000000000..42e5c3521a667ce57239dcfe7b21a93ad4063f1d
Binary files /dev/null and b/log/2020-10-17_noisy-speed.png differ
diff --git a/log/cl-step-control-log.md b/log/cl-step-control-log.md
index 152c08d12e62041b968c607317900a4263497021..9c7f45298e62be775f6541bbc6b0b412d00fbda2 100644
--- a/log/cl-step-control-log.md
+++ b/log/cl-step-control-log.md
@@ -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:
+
+
+
+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:
+
+
+
+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