cl-step-control-log.md 14.9 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
343
## 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.

I'm up to pointing, now I just need to deliver some power to the motor.