cl-step-control-log.md 23.6 KB
Newer Older
Jake Read's avatar
Jake Read committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
## Closed Loop Step Control

With 2x A4950s on the DAC, AS5047P on the encoder, etc.

## Do Want 

- magnet alignment print / tool for the glue-in 

## 2020 10 06 

OK, today am just waking this thing up. Have magnet glued (curing) on the back of the motor now, and one encoder soldered up on an old module-based stepper board - code should be the same.

OK, I have the SPI set up to read from the encoder as in I can see things on the scope are working but I still need to get the complete value back in. My write op for the 16 bit word is ~ 4us, and I need two of those for a complete read (one to issue the read cmd, the next to get the data back), so I am looking at an 8us read, ~ 10, about 100kHz, which is what I was running the step ticker at. I can try pushing the clock speed - running around 124ns clock period is just north of the AS5047's limit, but it seems to behave here and the edges look pretty clean: that nets a 6us read operation for 160-ish KHz alone.

```cpp
void ENC_AS5047::init(void){
    // do pin setup 
    // chip select (not on PC, op manuel)
    ENC_CS_PORT.DIRSET.reg = ENC_CS_BM;
    ENC_CS_DESELECT;
    // clk 
    ENC_CLK_PORT.DIRSET.reg = ENC_CLK_BM;
    ENC_CLK_PORT.PINCFG[ENC_CLK_PIN].bit.PMUXEN = 1;
    if(ENC_CLK_PIN % 2){
        ENC_CLK_PORT.PMUX[ENC_CLK_PIN >> 1].reg |= PORT_PMUX_PMUXO(ENC_SER_PERIPHERAL);
    } else {
        ENC_CLK_PORT.PMUX[ENC_CLK_PIN >> 1].reg |= PORT_PMUX_PMUXE(ENC_SER_PERIPHERAL);
    }
    // mosi
    ENC_MOSI_PORT.DIRSET.reg = ENC_MOSI_BM;
    ENC_MOSI_PORT.PINCFG[ENC_MOSI_PIN].bit.PMUXEN = 1;
    if(ENC_MOSI_PIN % 2){
        ENC_MOSI_PORT.PMUX[ENC_MOSI_PIN >> 1].reg |= PORT_PMUX_PMUXO(ENC_SER_PERIPHERAL);
    } else {
        ENC_MOSI_PORT.PMUX[ENC_MOSI_PIN >> 1].reg |= PORT_PMUX_PMUXE(ENC_SER_PERIPHERAL);
    }
    // miso 
    ENC_MISO_PORT.DIRCLR.reg = ENC_MISO_BM;
    ENC_MISO_PORT.PINCFG[ENC_MISO_PIN].bit.PMUXEN = 1;
    if(ENC_MISO_PIN % 2){
        ENC_MISO_PORT.PMUX[ENC_MISO_PIN >> 1].reg |= PORT_PMUX_PMUXO(ENC_SER_PERIPHERAL);
    } else {
        ENC_MISO_PORT.PMUX[ENC_MISO_PIN >> 1].reg |= PORT_PMUX_PMUXE(ENC_SER_PERIPHERAL);
    }

    // do SPI clock setup
    MCLK->APBDMASK.bit.SERCOM4_ = 1;
    GCLK->GENCTRL[ENC_SER_GCLKNUM].reg = GCLK_GENCTRL_SRC(GCLK_GENCTRL_SRC_DFLL) | GCLK_GENCTRL_GENEN;
    while(GCLK->SYNCBUSY.reg & GCLK_SYNCBUSY_GENCTRL(ENC_SER_GCLKNUM));
    GCLK->PCHCTRL[ENC_SER_GCLK_ID_CORE].reg = GCLK_PCHCTRL_CHEN | GCLK_PCHCTRL_GEN(ENC_SER_GCLKNUM);
    
    // reset / disable SPI 
    while(ENC_SER_SPI.SYNCBUSY.bit.ENABLE);
    ENC_SER_SPI.CTRLA.bit.ENABLE = 0; // disable 
    while(ENC_SER_SPI.SYNCBUSY.bit.SWRST);
    ENC_SER_SPI.CTRLA.bit.SWRST = 1; // reset 
    while(ENC_SER_SPI.SYNCBUSY.bit.SWRST || ENC_SER_SPI.SYNCBUSY.bit.ENABLE);
    
    // configure the SPI 
    // AS5047 datasheet says CPOL = 1, CPHA = 0, msb first, and parity checks 
    // bit: func 
    // 15: parity, 14: 0/read, 1/write, 13:0 address to read or write 
    ENC_SER_SPI.CTRLA.reg = SERCOM_SPI_CTRLA_CPOL | // CPOL = 1
                            SERCOM_SPI_CTRLA_DIPO(3) | // pad 3 is data input 
                            SERCOM_SPI_CTRLA_DOPO(0) | // pad 0 is data output, 1 is clk  
                            SERCOM_SPI_CTRLA_MODE(3);  // mode 3: head operation 
    ENC_SER_SPI.CTRLB.reg = SERCOM_SPI_CTRLB_RXEN; // enable rx, char size is 8, etc 
    ENC_SER_SPI.BAUD.reg = SERCOM_SPI_BAUD_BAUD(2); // f_baud = f_ref / ((2*BAUD) + 1) 
                                                    // BAUD = 2 ~= 8MHz / 124ns clock period 
                                                    // BAUD = 3 ~= 6MHz / 164ns clock period: AS5047 min period is 100ns
    
    // enable interrupts 
    NVIC_EnableIRQ(SERCOM4_0_IRQn);
    NVIC_EnableIRQ(SERCOM4_1_IRQn);
    NVIC_EnableIRQ(SERCOM4_2_IRQn);
    NVIC_EnableIRQ(SERCOM4_3_IRQn);

    // turn it back on 
    while(ENC_SER_SPI.SYNCBUSY.bit.ENABLE);
    ENC_SER_SPI.CTRLA.bit.ENABLE = 1;
}

uint16_t ENC_AS5047::spi_interaction(uint16_t outWord){
    if(ENC_SER_SPI.INTFLAG.bit.DRE == 1){
        ENC_CS_SELECT;
        // write first half (back 8 bits) then enable tx interrupt to write second 
        // when written & cleared, write next half 
        outWord01 = (outWord >> 8);
        outWord02 = outWord & 255;
        firstWord = true;
        ENC_SER_SPI.DATA.reg = outWord01;
        ENC_SER_SPI.INTENSET.bit.TXC = 1;
    } else {
        return 0;
    }
}

void ENC_AS5047::txcISR(void){
    // always clear this flag 
    ENC_SER_SPI.INTFLAG.bit.TXC = 1;
    if(firstWord){
        ENC_SER_SPI.DATA.reg = outWord02;
        firstWord = false;
    } else {
        ENC_CS_DESELECT;
        ENC_SER_SPI.INTENCLR.bit.TXC = 1;
    }
}

// 1 handles TXC 
void SERCOM4_1_Handler(void){
    enc_as5047->txcISR();
}

uint16_t ENC_AS5047::read(void){
    spi_interaction(AS5047_SPI_READ_POS);
}
```

I could do this with a read-start then collect-results-later block. 100kHz *seems* like a lot of speed, so I can finish this as a blocking structure and then adapt it later if it seems necessary: but if I run this at 100kHz control loop and do that, I'm occupying ~ 60% of the processor clock just running the encoder read. The intelligent thing would be to trigger the read before the loop, and use it's 'tail' to do the maths. I'll be running this on a timer interrupt anyways, so might as well have that timer start the read, then come back to finish the control loop when the new value is available. 

OK, I think I'm just getting half of the bits... 

Trying this with receive complete interrupts, seems like I'm only getting two events. Weird, have to reset the rxc interrupt to be on at the beginning of every transaction.

Now it just smells like I'm not getting the MSB in the data... probably polarity? OK, per D51 this is actually CPOL = 0, CPHA = 1, flipped bit relative AS5047 datasheet. Here's the full implementation:

```cpp
#include "enc_as5047.h"
#include "../utils/clocks_d51_module.h"

ENC_AS5047* ENC_AS5047::instance = 0;

ENC_AS5047* ENC_AS5047::getInstance(void){
    if(instance == 0){
        instance = new ENC_AS5047();
    }
    return instance;
}

ENC_AS5047* enc_as5047 = ENC_AS5047::getInstance();

ENC_AS5047::ENC_AS5047(){};

void ENC_AS5047::init(void){
    // do pin setup 
    // chip select (not on PC, op manuel)
    ENC_CS_PORT.DIRSET.reg = ENC_CS_BM;
    ENC_CS_DESELECT;
    // clk 
    ENC_CLK_PORT.DIRSET.reg = ENC_CLK_BM;
    ENC_CLK_PORT.PINCFG[ENC_CLK_PIN].bit.PMUXEN = 1;
    if(ENC_CLK_PIN % 2){
        ENC_CLK_PORT.PMUX[ENC_CLK_PIN >> 1].reg |= PORT_PMUX_PMUXO(ENC_SER_PERIPHERAL);
    } else {
        ENC_CLK_PORT.PMUX[ENC_CLK_PIN >> 1].reg |= PORT_PMUX_PMUXE(ENC_SER_PERIPHERAL);
    }
    // mosi
    ENC_MOSI_PORT.DIRSET.reg = ENC_MOSI_BM;
    ENC_MOSI_PORT.PINCFG[ENC_MOSI_PIN].bit.PMUXEN = 1;
    if(ENC_MOSI_PIN % 2){
        ENC_MOSI_PORT.PMUX[ENC_MOSI_PIN >> 1].reg |= PORT_PMUX_PMUXO(ENC_SER_PERIPHERAL);
    } else {
        ENC_MOSI_PORT.PMUX[ENC_MOSI_PIN >> 1].reg |= PORT_PMUX_PMUXE(ENC_SER_PERIPHERAL);
    }
    // miso 
    ENC_MISO_PORT.DIRCLR.reg = ENC_MISO_BM;
    ENC_MISO_PORT.PINCFG[ENC_MISO_PIN].bit.PMUXEN = 1;
    if(ENC_MISO_PIN % 2){
        ENC_MISO_PORT.PMUX[ENC_MISO_PIN >> 1].reg |= PORT_PMUX_PMUXO(ENC_SER_PERIPHERAL);
    } else {
        ENC_MISO_PORT.PMUX[ENC_MISO_PIN >> 1].reg |= PORT_PMUX_PMUXE(ENC_SER_PERIPHERAL);
    }

    // do SPI clock setup
    MCLK->APBDMASK.bit.SERCOM4_ = 1;
    GCLK->GENCTRL[ENC_SER_GCLKNUM].reg = GCLK_GENCTRL_SRC(GCLK_GENCTRL_SRC_DFLL) | GCLK_GENCTRL_GENEN;
    while(GCLK->SYNCBUSY.reg & GCLK_SYNCBUSY_GENCTRL(ENC_SER_GCLKNUM));
    GCLK->PCHCTRL[ENC_SER_GCLK_ID_CORE].reg = GCLK_PCHCTRL_CHEN | GCLK_PCHCTRL_GEN(ENC_SER_GCLKNUM);
    
    // reset / disable SPI 
    while(ENC_SER_SPI.SYNCBUSY.bit.ENABLE);
    ENC_SER_SPI.CTRLA.bit.ENABLE = 0; // disable 
    while(ENC_SER_SPI.SYNCBUSY.bit.SWRST);
    ENC_SER_SPI.CTRLA.bit.SWRST = 1; // reset 
    while(ENC_SER_SPI.SYNCBUSY.bit.SWRST || ENC_SER_SPI.SYNCBUSY.bit.ENABLE);
    
    // configure the SPI 
    // AS5047 datasheet says CPOL = 1, CPHA = 0, msb first, and parity checks 
    // bit: func 
    // 15: parity, 14: 0/read, 1/write, 13:0 address to read or write 
    ENC_SER_SPI.CTRLA.reg = //SERCOM_SPI_CTRLA_CPOL | // CPOL = 1
                            SERCOM_SPI_CTRLA_CPHA | // ?
                            SERCOM_SPI_CTRLA_DIPO(3) | // pad 3 is data input 
                            SERCOM_SPI_CTRLA_DOPO(0) | // pad 0 is data output, 1 is clk  
                            SERCOM_SPI_CTRLA_MODE(3);  // mode 3: head operation 
    ENC_SER_SPI.CTRLB.reg = SERCOM_SPI_CTRLB_RXEN; // enable rx, char size is 8, etc 
    ENC_SER_SPI.BAUD.reg = SERCOM_SPI_BAUD_BAUD(2); // f_baud = f_ref / ((2*BAUD) + 1) 
                                                    // BAUD = 2 ~= 8MHz / 124ns clock period 
                                                    // BAUD = 3 ~= 6MHz / 164ns clock period: AS5047 min period is 100ns
    
    // enable interrupts 
    NVIC_EnableIRQ(SERCOM4_0_IRQn);
    NVIC_EnableIRQ(SERCOM4_1_IRQn);
    NVIC_EnableIRQ(SERCOM4_2_IRQn);
    NVIC_EnableIRQ(SERCOM4_3_IRQn);

    // turn it back on 
    while(ENC_SER_SPI.SYNCBUSY.bit.ENABLE);
    ENC_SER_SPI.CTRLA.bit.ENABLE = 1;
    // just... always listen 
    ENC_SER_SPI.INTENSET.bit.RXC = 1;
}

void ENC_AS5047::start_spi_interaction(uint16_t outWord){
    // for some reason, have to reset this to fire? 
    ENC_SER_SPI.INTENSET.bit.RXC = 1;
    if(ENC_SER_SPI.INTFLAG.bit.DRE == 1){
        ENC_CS_SELECT;
        // write first half (back 8 bits) then enable tx interrupt to write second 
        // when written & cleared, write next half 
        outWord01 = (outWord >> 8);
        outWord02 = outWord & 255;
        firstWord = true;
        ENC_SER_SPI.DATA.reg = outWord01;
        ENC_SER_SPI.INTENSET.bit.TXC = 1;
    }
}

void ENC_AS5047::txcISR(void){
    // always clear this flag 
    ENC_SER_SPI.INTFLAG.bit.TXC = 1;
    if(firstWord){
        ENC_SER_SPI.DATA.reg = outWord02;
        firstWord = false;
    } else {
        ENC_CS_DESELECT;
        ENC_SER_SPI.INTENCLR.bit.TXC = 1;
        if(firstAction){
            firstAction = false;
            start_spi_interaction(AS5047_SPI_READ_POS);
        }
    }
}

void ENC_AS5047::rxcISR(void){
    // always clear the bit, 
    uint8_t data = ENC_SER_SPI.DATA.reg;
    readComplete = true;
    if(!firstAction){
        if(firstWord){
            inWord01 = data;
        } else {
            inWord02 = data;
            result = (inWord01 << 8) | inWord02;
            on_read_complete(result);
            readComplete = true;
        }
    }
}

void SERCOM4_2_Handler(void){
    DEBUG1PIN_TOGGLE;
    enc_as5047->rxcISR();
}

// 1 handles TXC 
void SERCOM4_1_Handler(void){
    enc_as5047->txcISR();
}

void ENC_AS5047::trigger_read(void){
    firstAction = true;
    readComplete = false;
    start_spi_interaction(AS5047_SPI_READ_POS);
}

boolean ENC_AS5047::is_read_complete(void){
    return readComplete;
}

uint16_t ENC_AS5047::get_reading(void){
    return result;
}
```

```cpp
void loop() {
  osap->loop();
  stepper_hw->dacRefresh();

  tick++;
  if(tick > 1000){
    tick = 0;
    enc_as5047->trigger_read();
    while(!enc_as5047->is_read_complete());
    uint16_t reading = 0b0011111111111111 & enc_as5047->get_reading();
    sysError(String(reading));
  }
}
```

So, next (tomorrow?) I want to outfit this to build calib. tables... starting at phase 0 / step x, building a table through the sweep. Then I should just be able to apply torque by writing 90 deg phase offsets based on that table, i.e. just pointing through table offsets, no maths. Nice. Then a speed test is to see how fast I can op the stepper just by doing that. 

10pm now, do I want to do this tonight? 

Looking forward, I want to write the table into EEPROM (equivalent) on the motor itself, so writing a routine to do it via VM doesn't make a lot of sense... I should get my spreadsheet out and decide what a reasonable size table is / scheme for this thing. 

## 2020 10 07 

OK, so I need to consider how this is actually going to work.

My control loop will have some set command (torque), and I'll start by reading the encoder. I want a table that relates my encoder reading to (1) the 'actual angle' and (2) the magnetic angle of the rotor w/r/t the stator. Since the whole premise is that we use the stepper's high internal accuracy w/r/t its magnetic angles (its... steps), these should be the same LUT... and since there is a strong relationship between angular position & magnetic position, I am really just going to do some maths later to get the phase advance right, and for the time being can just generate the table from encoder posns to 'real angular' posns. 

[Mechaduino](https://github.com/jcchurch13/Mechaduino-Firmware/blob/master/Mechaduino/Mechaduino/Utils.cpp) does this by generating a LUT where each index is the encoder reading, containing a floating point angular value. Does this by single-stepping through one full revolution 0-360 and recording encoder values (averaged) at each location, then reversing through 0-2^16 to linterp one angular value for each possible encoder uint16_t result, then it can just do:

`float pos = LUT[reading]`

And I need to store these in flash, or / reply to the browser w/ them... will do ahn 'runcalib' and 'readcalib' codes from the browser. 

This table is ~ 65kbytes by the way! 

OK, also I need to make my dac-writing / step-hardware code machine a bit better. First, I think that my LUT crosses 4 'steps' in its full width table, so while I thought I've been microstepping at 256 I've just been doing 64, or something similar. I need this to be a vector machine ish thing, where I can write a phase angle into it, or ->step(micro_counts), which would similarely advance the phase angle from its last position. I would eventually assume I would want something like ->point(phase_angle). 

OK, damn, before I get to that I need to wake up flash and it's a bit more complicated than this library initially makes it seem. 

I also can't seem to even store this big ol' LUT in RAM in the step_cl class, idk what is up. Going to eat dinner. 

`const float __attribute__((__aligned__(256))) lookup[16384] = {`

OK, IDK, odd things here - I can globally define the above LUT and the program does not crash, but when I make it a class member it breaks. Also doesn't seem to make any change to the program allocation size in either of those cases, which is spooky. Ghost memory. 

This seems like a whole thing I'll want to dig into later. I think for now I'm just going to see if I can generate & output the table, I can dig through flash memory later. 

## 2020 10 08 

I'm just going to get into the 'magnetic angle' pointing system for the DACs now, and just wanted to confirm for a minute that these phases *are* 90 deg out of phase with eachother. 

![phase](2020-10-08_90degs.png)

This makes sense: so when I'm at '0 degs' my A phase is on 100%, B phase is zero. Or, the way I've my LUT written, I'll have A at 0 and B at full-width positive. If I don't like this for some reason (It'll calibrate away) I can change the LUT.

343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
I'm up to pointing, now I just need to deliver some power to the motor. 

OK, this is making sense and I can point my magnetic vector around:

```cpp
// do _aStep and _bStep integers, op electronics 
void STEP_A4950::writePhases(void){
    // a phase, 
    if(LUT_8190[_aStep] > 4095){
        A_UP;
    } else if (LUT_8190[_aStep] < 4095){
        A_DOWN;
    } else {
        A_OFF;
    }
    // a DAC 
    dacs->writeDac0(dacLUT[_aStep] * _cscale);

    // b phase, 
    if(LUT_8190[_bStep] > 4095){
        B_UP;
    } else if (LUT_8190[_bStep] < 4095){
        B_DOWN;
    } else {
        B_OFF;
    }
    // b DAC
    dacs->writeDac1(dacLUT[_bStep] * _cscale);
}

// magnetic angle 0-1 maps 0-2PI phase  
// magnitude 0-1 of <range> board's max amp output 
// one complete magnetic period is four 'steps' 
void STEP_A4950::point(float magangle, float magnitude){
    // guard out of range angles & magnitudes 
    clamp(&magangle, 0.0F, 1.0F);
    clamp(&magnitude, 0.0F, 1.0F);
    uint16_t magint = (uint16_t)(magangle * (float)LUT_LENGTH);
    _aStep = magint; // a phase just right on this thing, 
    // lut_length / 4 = lut_length >> 2 (rotates phase 90 degs)
    // x modulo y = (x & (y − 1))       (wraps phase around end of lut)
    _bStep = (magint + ((uint16_t)LUT_LENGTH >> 2)) & (LUT_LENGTH - 1);
    // upd8 the cscale 
    _cscale = magnitude;
    // now we should be able to... 
    writePhases(); 
}
```

One magnetic period is four full 'steps', so my LUT is 1024 positions long and I am effectively 'microstepping' 256 times (or have this amount of resolution). 

Now I need to write the table, so I'll take 200 samples at each full step, maybe averaging encoder readings 10x. 

OK, I've those samples, so I guess the next move is just to write my table. I think I want to check that all of the readings are continuous (pointing in the same direction) but this also seems a bit tricky: one will be reversed as well, for sure. 

Then I'll get a table that has one jump around the encoder origin. I think I want to start by rewriting the readings table to base zero around the origin, well, obviously since this is a LUT. Finding the actual 'zero' is a bit of a trick though, and requires the interpolation routine to set the first 'mag angle' at that point. 

Maybe this is less complicated... for each value 0-16k, I find the interval it's between in 0-199 indices from the scan. One of these is a bit tricky, otherwise it's whatever. I do the linterp and write down a float. 

Here's the calib so far,

```cpp
boolean Step_CL::calibrate(void){
    // (1) first, build a table for 200 full steps w/ encoder averaged values at each step 
    float phase_angle = 0.0F;
    for(uint8_t i = 0; i < 200; i ++){
        // pt to new angle 
        stepper_hw->point(phase_angle, CALIB_CSCALE);
        // wait to settle / go slowly 
        delay(CALIB_STEP_DELAY);
        // do readings 
        float reading = 0.0F;
        for(uint8_t s = 0; s < CALIB_SAMPLE_PER_TICK; s ++){
            enc_as5047->trigger_read();
            while(!enc_as5047->is_read_complete()); // do this synchronously 
            reading += (float)(enc_as5047->get_reading());
            // this is odd, I know, but it allows a new measurement to settle
            // so we get a real average 
            delay(1); 
        }
        // push reading 
        calib_readings[i] = reading / (float)CALIB_SAMPLE_PER_TICK;
        // rotate 
        phase_angle += 0.25F;
        if(phase_angle >= 1.0F) phase_angle = 0.0F;
    }
    // report readings
    if(false){
        for(uint8_t r = 0; r < 200; r ++){
            sysError(String(calib_readings[r]));
            delay(10);
        }
    }
    // check sign of readings 
    // the sign will help identify the wrapping interval
    // might get unlucky and find the wrap, so take majority vote of three 
    boolean s1 = (calib_readings[1] - calib_readings[0]) > 0 ? true : false;
    boolean s2 = (calib_readings[2] - calib_readings[1]) > 0 ? true : false;
    boolean s3 = (calib_readings[3] - calib_readings[2]) > 0 ? true : false;
    boolean sign = false;
    if((s1 && s2) || (s2 && s3) || (s1 && s3)){
        sign = true;
    } else {
        sign = false;
    }
    sysError("calib sign: " + String(sign));

    // now to build the actual table... 
    // want to start with the 0 indice, 
    for(uint16_t e = 0; e < ENCODER_COUNTS; e ++){
        // find the interval that spans this sample 
        int16_t interval = -1;
        for(uint8_t i = 0; i < 199; i ++){
            if(sign){ // +ve slope readings, left < right 
                if(calib_readings[i] < e && e <= calib_readings[i + 1]){
                    interval = i;
                    break;
                }
            } else { // -ve slope readings, left > right 
                if(calib_readings[i] > e && e >= calib_readings[i + 1]){
                    interval = i;
                    break;
                }
            }
        }
        // log intervals 
        if(interval >= 0){
            // sysError(String(e) + " inter: " + String(interval) 
            //                 + " " + String(calib_readings[interval]) 
            //                 + " " + String(calib_readings[interval + 1]));
        } else {
            sysError("bad interval at: " + String(e));
            return false;
        }
        // find anchors 
        float ra0 = 360.0F * ((float)interval / 200);          // real angle at left of interval 
        float ra1 = 360.0F * ((float)(interval + 1) / 200);    // real angle at right of interval 
        // check we are not abt to div / 0: this could happen if motor did not turn during measurement 
        float intSpan = calib_readings[interval - 1] - calib_readings[interval];
        if(intSpan < 0.1F && intSpan > -0.1F){
            sysError("short interval, exiting");
            return false;
        }
        // find pos. inside of interval 
        float offset =  ((float)e - calib_readings[interval]) / intSpan;
        // find real angle offset at e 
        float ra = ra0 + (ra1 - ra0) * offset;
        // log those 
        sysError("ra: " + String(ra, 4));
        delay(1);
    } // end sweep thru 2^14 pts 
    return true; // went OK 
}
```

This works inside of 'normal' intervals, but fails around the origin due to that wrap issue. I should detect this case... 

OK, new bug: when I average readings that are around the origin, I have samples like i.e. 

```
16383
1
16384
2
16382
1
```

etc, so the average of these needs to wrap as well, ya duh, tough to do with the wrap tho yeah. I can just add the total counts to each measurement and then sub that value from the total average... or from the sum. 

I have this problem: https://en.wikipedia.org/wiki/Mean_of_circular_quantities 

The maths-ey way to do this is to convert to cartesian coordinates, given the theta, average in that space and then return to r coords. 

I guess doing this like that isn't too aweful, tho it's expensive. It would be awesome to have a faster method later on, but there probably better filters (like a kalman) make more sense than just a big ol' average. 

Jake Read's avatar
Jake Read committed
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
I can imagine taking the measurement spreads: if any were larger than 20 ticks, I could sweep through that set and add the enc_count to the lo vals, then average. Or I could just do this for any reading < 31 ticks, as these are the small ones in *this particular window* but then I have that crawling window issue for measurements really ~ around 31 ticks. Might just be prettier to take a real circular average. 

Here's the circular average:

```cpp
float x = 0.0F;
float y = 0.0F;
for(uint8_t s = 0; s < CALIB_SAMPLE_PER_TICK; s ++){
    enc_as5047->trigger_read();
    while(!enc_as5047->is_read_complete()); // do this synchronously 
    float reading = enc_as5047->get_reading();
    x += cos((reading / (float)(ENCODER_COUNTS)) * 2 * PI);
    y += sin((reading / (float)(ENCODER_COUNTS)) * 2 * PI);
    // this is odd, I know, but it allows a new measurement to settle
    // so we get a real average 
    delay(1); 
}
// push reading, average removes the wraps added to readings. 
calib_readings[i] = atan2(y, x);//(reading / (float)CALIB_SAMPLE_PER_TICK) - ENCODER_COUNTS;
if(calib_readings[i] < 0) calib_readings[i] = 2 * PI + calib_readings[i]; // wrap the circle 
calib_readings[i] = (calib_readings[i] * ENCODER_COUNTS) / (2 * PI);
```

I'm getting some strange intervals back, seems like the 'bad interval' is showing up in three chunks, though it should be one span of angles. I think the best way to debug this would be to plot the whole thing out... or I *could* try straight printing it all back on the serport... maybe I'll try that before building a whole packet-transmission-of-hella-floats thing. 

Yeah this looks all kinds of wrong. 

Borked, indeed. Will check tomorrow. Errors just before the BI (nan, so our of index somewhere?) and the BI looks close but not quite there. 

## 2020 10 08 

...