cl-step-control-log.md 41.9 KB
Newer Older
Jake Read's avatar
Jake Read committed
1
2
3
4
5
6
7
## 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 
Jake Read's avatar
Jake Read committed
8
- https://www.youtube.com/watch?v=5x73LjZQ21o 
Jake Read's avatar
Jake Read committed
9

10
11
12
13
## Likely Bugs

- haven't tested with '+ve' signed calibrations, i.e. if motor += magnetic step does encoder += tick step. 

14
15
16
17
## 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? 

Jake Read's avatar
Jake Read committed
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
343
344
345
346
347
348
349
350
351
## 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.

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
519
520
521
522
523
524
525
526
527
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
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
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. 

557
## 2020 10 09
Jake Read's avatar
Jake Read committed
558

559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
Ok, updating the interpolation, seems messy but I think (?) maybe this shot will work - at least for -ve signed samples. 

I think.. also, if this doesn't work, I might tackle this differently - by knocking the tail down or up, then wrapping angles. 

Ugh, still doesn't work. Here was the attempt:

```cpp
        // (4) for the bad interval, some more work to do to modify interp. points 
        if(bi){
            // find the zero crossing within the wrapping interval 
            float zc, l0, l1;
            if(sign){
                float l0 = ENCODER_COUNTS - er0;
                float l1 = er1;
            } else {
                float l0 = er0;
                float l1 = ENCODER_COUNTS - er1;
            }
            zc = l0 / (l0 + l1);
            float raMid = (ra0 - (ra1 - ra0)) * zc;
            // modify the interval tails, depending on side of zero crossing 
            if( (sign && er0 < e) || (!sign && er0 > e) ){ 
                // this is left of zc, modify right tail 
                ra1 = raMid; // find the about-zero pt, 
                if(sign){
                    er1 = ENCODER_COUNTS;
                } else {
                    er1 = 0;
                }
            } else { 
                // this is right of zc, modify left tail 
                ra0 = raMid;
                if(sign){
                    er0 = 0;
                } else {
                    er0 = ENCODER_COUNTS;
                }
            }
        }
```

I'm going to try to get this right for the normal intervals first... 

Getting much closer, have implemented a simpler wrap system, looks like the first half works but the second is confused. 

I am still getting these *three* instances of the BI, tho I had always expected to just get two - one at the head of the thing (0 -> first good interval) and the second at the end (last gi -> 2^14). The middle instance is where this seems broken... I'll inspect around there. 

Yeah, I've these readings in the middle of things that are not appearing to live in an interval. 

Right, of course, this is the one that lives in the wrap on readings. I could stick another reading on there, or wrap the indices... should check if the mechanical wrap ~= the code wrap...    

OK - just tacked the last interval on there, so I have 201 readings (201st being == 1st), and 200 intervals. Here's the full calibration routine, w/o saving the giant array:

```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 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(CALIB_SETTLE_DELAY); 
        }
        // 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);
        // rotate 
        phase_angle += 0.25F;
        if(phase_angle >= 1.0F) phase_angle = 0.0F;
    } // end measurement taking 
    // tack end-wrap together, to easily find the wrap-at-indice interval 
    calib_readings[200] = calib_readings[0];
    if(false){ // debug print intervals 
        for(uint8_t i = 0; i < 200; i ++){
            sysError("int: " + String(i) 
                        + " " + String(calib_readings[i], 4)
                        + " " + String(calib_readings[i + 1], 4));
            delay(2);
        }
    }
    // 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));

    // (2) build the table, walk all encoder counts... 
    // 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
        boolean bi = false; 
        int16_t interval = -1;
        for(uint8_t i = 0; i < 200; 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 {
            // no proper interval found, must be the bi 
            // find the opposite-sign interval 
            for(uint8_t i = 0; i < 200; i ++){
                boolean intSign = (calib_readings[i + 1] - calib_readings[i]) > 0 ? true : false;
                if(intSign != sign){
                    interval = i;
                    bi = true; // mark the bad interval
                    break;
                }
            }
            if(!bi){
                // truly strange 
                sysError("missing interval, exiting");
                return false;
            }
            /*
            sysError("bad interval at: " + String(e) 
                    + " " + String(interval)
                    + " " + String(calib_readings[interval]) 
                    + " " + String(calib_readings[interval + 1]));
            */
        }

        // (3) have the interval (one is bad), 
        // find real angles (ra0, ra1)
        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 
        // interval spans these readings (er0, er1)
        float er0 = calib_readings[interval];
        float er1 = calib_readings[interval + 1];

        // (4) for the bad interval, some more work to do to modify interp. points 
        float spot = e;
        if(bi){
            if(sign){ // wrap the tail *up*, do same for pts past zero crossing 
                er1 += (float)ENCODER_COUNTS;
                if(spot < er0) spot += (float)ENCODER_COUNTS;
            } else { // wrap the tail *down*, do same for pts past zero crossing 
                er1 -= (float)ENCODER_COUNTS;
                if(spot > er0) spot -= (float)ENCODER_COUNTS;
            }
        }

        // (5) continue w/ (ra0, ra1) and (er0, er1) to interpolate for spot 
        // check we are not abt to div / 0: this could happen if motor did not turn during measurement 
        float intSpan = er1 - er0;
        if(intSpan < 0.01F && intSpan > -0.01F){
            sysError("near zero interval, exiting");
            return false;
        }
        // find pos. inside of interval 
        float offset = (spot - er0) / intSpan;
        // find real angle offset at e, modulo for the bad interval 
        float ra = (ra0 + (ra1 - ra0) * offset);
        // wrap to 360 degs, 
        /*
        if(ra < 0.0F){
            ra += 360.0F;
        } else if (ra > 360.0F){
            ra -= 360.0F;
        }
        */
        // log those 
        if(bi){
            sysError("e: " + String(e) + " ra: " + String(ra, 4) + " BI");
            //     + " span: " + String(intSpan) + " offset: " + String(offset));
            // sysError("i0: " + String(interval) + " " + String(calib_readings[interval])
            //     + " i1: " + String(calib_readings[interval + 1])
            //     + " BI");
        } else {
            sysError("e: " + String(e) + " ra: " + String(ra, 4));
        }
        delay(10);
    } // end sweep thru 2^14 pts 
    sysError("calib complete");
    return true; // went OK 
}
```

771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
So next up is writing that to flash so that I can use it. 

Alright! Flash memory. 

Seems like I've most of it written - just copying from mechaduino, but after I write these out, I freeze up the micro. 

Yeah, welp, I think I might have to get into the datasheet and do this au manuel. I've learned that when I declare a const variable, it goes into the flash memory. I think my 'alignment' of that will set it up in pages, so I 'align' it to the page size, which I can read to be 512 bytes... I'll take a minute, see how this goes. Bummer it's Friday, I was pumped to get this up and running this week. 

## 2020 10 13 

Setting up the NVM Controller today to try to write pages into flash. Not sure why this was crashing before, probably something simple. 

I don't think there's a clock / init I have to do to wake up the flash controller, seems like that would be kind of crazy. Yeah, 25.6.1.1 clarifies this. 

I'm also not sure where the bootloader is in here, and seems likely that I might write it over. 

'addressable on the AHB bus' 

I think I almost get this, need to figure this note about disabling cache.. some erratta... am at 25.6.6

Trying with just doing automatic quad word writing, which I think auto increments the page-write pointer, not sure about what happens for the page buffer though. I should check via the mechaduino code and flash storage code... 

Ok this is hella opaque, 25.6.6.2 ... `procedure for manual page writes` 
    - the block to be written must be erased before the write command is given 
    - write to the page buffer by addressing the nvm main address space directly (crazy) 
    - cmd = wp to write the full contents of the page buffer into the nvm at the page pointed to by addr 

I fully don't understand this, but it seems like it should be straightforward. Likely next step is to burn it all down and try again from scratch. I should see if I can write *anything* and move away from this calib-then-write situation, which is probably complicating things. 

#### Aboot Flash

Have to erase before write, to set back to 1's: flash memory writes by pulling zeroes low, and has to operate in blocks. 

I can also check the UF2 bootloader, which uses flash:

806
807
808
809
810
811
812
813
https://github.com/microsoft/uf2-samdx1/blob/master/src/flash_samd51.c

This code seems to have a much better handle on things in any case. I should try something straightforward to start. 

Well, I think I might be writing into memory, but I've no way of reading it out - or I crash when I try to dereference the address. Will try making that const... Same. 

I suspect I am writing into some bad spaces. This code (which *is* better) is expecting a pointer to a uint32, not the actual 'raw' uint32 addr... and I was previously trying to write to 0xF0000000 which is well out of range, I meant to do 0x10000000... 

Jake Read's avatar
Jake Read committed
814
815
816
817
818
819
820
821
822
823
824
825
To clarify, I'll write the base fns with real void* rs... or in uint32_t addresses, as uint32_t s... 

Have isolated down to this `((uint32_t*)dst)[i] = ((uint32_t*)src)[i];` call... I suspect I'm pointing at something poorly. And I still get the sense I'm writing into the bootloader block. 

OK, I can read out of the start_addr, but after I've written to it, cannot do this anymore - processor hangs. Altho:

`// bossac writes at 0x20005000`

so that should be OK, I'm writing at 0x1... tho this program uses 42kb, but indeed once I've done the flash write, the bootloader fails. Tried writing at the lut address, fails also. 

I don't know what is real anymore, the libraries failed me, the datasheet is opaque AF, I am writing somewhere I shouldn't, etc. Might be time to bring out the atmel studio guns. 

Jake Read's avatar
Jake Read committed
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
Or the small guns: I'm going to try to get a minimum viable something up with this library. 

Works like this:

```cpp
FlashStorage(flash_store, float);

void flash_write_init(void){

}

void flash_write_value(float val){
    flash_store.write(val);
}

void Step_CL::print_table(void){
    for(uint16_t e = 0; e < 4; e ++){
        float ra = flash_store.read();
        sysError("e: " + String(e) + " ra: " + String(ra, 4));
        delay(5);
    }
}
```

So I can try doing batches of these... Seems like a full 'page' works, I wonder if anything > 512 bytes does as well, or crashes... breaks at 256 floats, but not at 129... 

So it seems that it should work, if I can get into the lower levels of this thing and increment pointers around. I'll try to do that first with just the 128 bytes that I know can work, and I'm pretty sure that's inside of one page. 

OK, trips at the erase. Takes *forever* to reload code once I cripple it like this. 

Oy, seems like I wrote it... then it dissappears after a reset, wth? That was to do with a volatile flag. 

I think this has to do with erase block / instead of erase page. I should learn more about that in the data sheet. 

Blocks are 16 pages: erase granularity is per block, write granularity is per page. One guess is that if the block / page are not aligned to a block, the EB command fails. Altho from 25.6.6.6 (fitting), it notes that any address within the block is valid for an erase block command. 

OK, trying to unlock the block beforehand. 

SO! Definitely hung on trying to erase a block. However, have had some evidence that this worked previously. 

It's 1230 - I'll think about this tomorrow. Maybe try taking the above single-float working example apart, see if you can code it up yourself, to understand what's up. 

## 2020 10 14 

OK, today will bring this library into the local build and see about figuring what the heck it does, build the minimum example from scratch, and then see if I can scale that up to multiple blocks / tables. 

- pages are 512 bytes 
- blocks are 16 pages 

875
876
This means a block is 8192, makes sense this is where flashstorage aligns itself. I need 8 blocks total for the full LUT. 

Jake Read's avatar
Jake Read committed
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
Interesting, now that I fixed this error which appeared when it was compiling locally, I can get past 512 bytes with my example. Could try the big one... 

OK, but doesn't work past 8192 bytes - I'll try just over the boundary to check again. At the full spec, it indeed doesn't even start up. Can get to 4096 floats, but 8192 breaks things. 

Last check for easy answers, I'll see if I can just tag 4x4096 FlashStorage instances together. OK, looks like I can get away with this. Damn, that was promising but it locks up on block #3. It doesn't matter which one I try to write first: i.e. it's not that prior writes fill up space for the latter, even if I try to write to the 3rd brick at the start, it hangs. OK very strange: can write all bricks except for the 3rd... 

Eh, IDK anymore, I can't write more than twice regardless of which brick I'm writing to. Big shrug, going to try with bricks of 2048 size. Need 8 of those for the same total-bounds errors... OK, having the same problem here. These should be blocks. I can try once more, with 1024 floats each, just to be sure. 

Nope, errors around the same spot. So I should check in the code to see if I can figure out where this is hanging up... At this point I suspect I am having some low level error from an NVMCTRL command, or, somehow the ptrs are pointing into bad spaces. 

Trying:
- no write (fails block 3)
- no erase (fails block 3)
- no invalidate cache (same)
- *no* write fn (same)
- *no* erase fn 

Suspect ROW_SIZE something is up, 
- no while(>size) in erase 
- no ptr op in erase fn 

OK this is crazy, I have every fn in the class commented out, and the thing still locks up. 

Jake Read's avatar
Jake Read committed
900
901
So, tried my way around that, I have honestly no idea what is going on here. I'm going to try writing from the base class again. 

Jake Read's avatar
Jake Read committed
902
OK, did that and seems like I'm writing into the thing. Crazy. Here's the code:
Jake Read's avatar
Jake Read committed
903

Jake Read's avatar
Jake Read committed
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
```cpp
#define BYTES_PER_BLOCK 8192
#define FLOATS_PER_BLOCK 2048

const float __attribute__((__aligned__(8192))) lut[16834] = {};
FlashClass flashClass((const uint8_t*)lut);

// write mechanics
const void* block_ptr;

// write buffer 
static float buffer[FLOATS_PER_BLOCK];

uint32_t bfi = 0; // buffer indice 
uint32_t bli = 0; // block indice 

//FlashStorage(flash_storage, flts);

void flash_write_init(void){
    block_ptr = (const uint8_t*) lut;
    bfi = 0;
    bli = 0;
}

void flash_write_page(void){
    sysError("erasing");
    flashClass.erase(block_ptr, BYTES_PER_BLOCK);
    sysError("writing");
    flashClass.write(block_ptr, (const uint8_t*)buffer, BYTES_PER_BLOCK);
    delay(100);
}

void flash_write_value(float val){
    buffer[bfi ++] = val;
    if(bfi >= FLOATS_PER_BLOCK){
        flash_write_page();
        bfi = 0;
        bli ++;
        block_ptr = ((const uint8_t *)(&(lut[bli * FLOATS_PER_BLOCK])));
    }
}

void Step_CL::print_table(void){
    sysError("reading from lut");
    for(uint32_t i = 0; i < ENCODER_COUNTS; i ++){
        float ra = lut[i];
        sysError(String(ra));
        delay(5);
    }
}

// the calib routine 
boolean Step_CL::calibrate(void){
    flash_write_init();
    for(uint32_t i = 0; i < ENCODER_COUNTS; i ++){
        flash_write_value(i * 1.1F);
    }
    return true;
}
```

So I think now I'll carry on actually running the calibration, then see if I can send a 'torque' command down / operate a control loop. 

Great, this calibrates, so now I can roll a tiny control loop, yeah?