step_cl.cpp 10.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
/*
osap/drivers/step_cl.cpp

stepper in closed loop mode 

Jake Read at the Center for Bits and Atoms
(c) Massachusetts Institute of Technology 2020

This work may be reproduced, modified, distributed, performed, and
displayed for any purpose, but must acknowledge the squidworks and ponyo
projects. Copyright is retained and must be preserved. The work is provided as
is; no warranty is provided, and users accept all liability.
*/

#include "step_cl.h"

Step_CL* Step_CL::instance = 0;

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

Step_CL* step_cl = Step_CL::getInstance();

// https://github.com/cmaglie/FlashStorage
// flash LUT
// FlashStorage(flash_lut, step_cl_calib_table_t);
// float __attribute__((__aligned__(256))) lut[16384];

Step_CL::Step_CL(void){}

#define CALIB_CSCALE 0.4F
36
#define CALIB_STEP_DELAY 10
37
#define CALIB_SETTLE_DELAY 1
38
39
40
#define CALIB_SAMPLE_PER_TICK 10 

#define ENCODER_COUNTS 16384
Jake Read's avatar
Jake Read committed
41
42
43
44
45
46
47
48

void Step_CL::init(void){
    stepper_hw->init(false, 0.4);
    enc_as5047->init();
    // this lut == stored lut 
    //lut = flash_lut.read();
}

49
50
51
52
// das lut / flash stuff, 
// s/o to mechaduino code for this 
#define PAGE_SIZE 512 // start_flash_write reports this bit value, use datasheet 25.8.3 to reference 
const float __attribute__((__aligned__(PAGE_SIZE))) lut[ENCODER_COUNTS] = {};
Jake Read's avatar
Jake Read committed
53
uint32_t start_addr = 0x00000000;
54
55
float temp_quad[4];
uint8_t temp_indice;
56

57
58
void flash_wait_ready(void){
    while(NVMCTRL->STATUS.bit.READY == 0);
59
60
}

61
62
// erase block at this addr 
void flash_erase_block(uint32_t *dst){
Jake Read's avatar
Jake Read committed
63
    sysError("erasing 0x" + String((uint32_t)dst, HEX));
64
65
    flash_wait_ready();
    // do 'erase row'
Jake Read's avatar
Jake Read committed
66
    NVMCTRL->ADDR.reg = (uint32_t)dst;
67
    NVMCTRL->CTRLB.reg = NVMCTRL_CTRLB_CMDEX_KEY | NVMCTRL_CTRLB_CMD_EB;
68
    flash_wait_ready();
69
70
}

71
// writes blocks of four four bit words: i.e. 4 floats per row, 4 uint32, etc 
Jake Read's avatar
Jake Read committed
72
// dst address, src address... 
73
74
#define QUAD_WORD (4 * 4)
void flash_write_words(uint32_t *dst, uint32_t *src, uint32_t n_words){
Jake Read's avatar
Jake Read committed
75
    sysError("writing quad to addr: 0x" + String((uint32_t)dst, HEX));
76
77
    // set manuel page write 
    NVMCTRL->CTRLA.bit.WMODE = NVMCTRL_CTRLA_WMODE_MAN;
78

Jake Read's avatar
Jake Read committed
79
    sysError("pbc");
80
81
    // execute page buffer clear 
    flash_wait_ready();
82
    NVMCTRL->CTRLB.reg = NVMCTRL_CTRLB_CMDEX_KEY | NVMCTRL_CTRLB_CMD_PBC;
83
84
85
    flash_wait_ready();

    // write em ?
Jake Read's avatar
Jake Read committed
86
    sysError("writing...");
87
    while(n_words > 0){
Jake Read's avatar
Jake Read committed
88
        sysError(String(n_words));
89
90
91
        // more than 4 words left? 
        uint32_t len = 4 < n_words ? 4 : n_words;
        // write one quad word into page buffer (is ... the flash address?)
Jake Read's avatar
Jake Read committed
92
93
        flash_wait_ready();
        sysError("past wait");
94
95
96
97
        for(uint32_t i = 0; i < 4; i ++){
            if(i < len){
                dst[i] = src[i];
            } else {
Jake Read's avatar
Jake Read committed
98
                //((uint32_t*)dst)[i] = 0xffffffff; // tail ends write to solid 1's 
99
100
101
            }
        }
        // trigger the write 
Jake Read's avatar
Jake Read committed
102
103
        sysError("write: 0x" + String((uint32_t)dst, HEX));
        delay(100);
104
105
106
107
108
109
        NVMCTRL->ADDR.reg = (uint32_t)dst;
        NVMCTRL->CTRLB.reg = NVMCTRL_CTRLB_CMDEX_KEY | NVMCTRL_CTRLB_CMD_WQW;
        // advance thru quad words 
        dst += len;
        src += len;
        n_words -= len;
110
    }
111
112
    sysError("quad complete");
    delay(10);
113
114
}

115
116
117
118
119
120
121
122
123
124
125
126
// ok, I think I can try this: I keep a row of vals, 4 of 'em, then on wrap, I write 
// using erase / then write, using the lut as the start, 
// I'll try to do it once, to start, then read it out... 

void flash_write_init(void){
    temp_indice = 0;
}

void flash_write_value(float val){
    sysError("flt: " + String(val));
    temp_quad[temp_indice ++] = val;
    if(temp_indice > 3){
Jake Read's avatar
Jake Read committed
127
128
        flash_erase_block(&start_addr);
        flash_write_words(&start_addr, (uint32_t*)(&temp_quad), 4);
129
130
        temp_indice = 0; 
    }
131
132
133
}

// the calib routine 
134
135
136
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;
Jake Read's avatar
Jake Read committed
137
    for(uint8_t i = 0; i < 200; i ++){ 
138
139
140
        // pt to new angle 
        stepper_hw->point(phase_angle, CALIB_CSCALE);
        // wait to settle / go slowly 
Jake Read's avatar
Jake Read committed
141
        delay(CALIB_STEP_DELAY);
142
143
144
145
146
147
148
149
150
151
152
        // 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 
153
            delay(CALIB_SETTLE_DELAY); 
154
155
156
157
158
159
160
161
        }
        // 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;
Jake Read's avatar
Jake Read committed
162
    } // end measurement taking 
163
164
    // tack end-wrap together, to easily find the wrap-at-indice interval 
    calib_readings[200] = calib_readings[0];
Jake Read's avatar
Jake Read committed
165
    if(false){ // debug print intervals 
166
        for(uint8_t i = 0; i < 200; i ++){
167
168
169
170
171
            sysError("int: " + String(i) 
                        + " " + String(calib_readings[i], 4)
                        + " " + String(calib_readings[i + 1], 4));
            delay(2);
        }
Jake Read's avatar
Jake Read committed
172
    }
173
174
175
176
177
178
179
180
181
182
183
184
185
186
    // 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));

Jake Read's avatar
Jake Read committed
187
    // (2) build the table, walk all encoder counts... 
188
189
    // now to build the actual table... 
    // want to start with the 0 indice, 
190
191
    flash_write_init();
    for(uint16_t e = 0; e < 4; e ++){
Jake Read's avatar
Jake Read committed
192
193
        // find the interval that spans this sample
        boolean bi = false; 
194
        int16_t interval = -1;
195
        for(uint8_t i = 0; i < 200; i ++){
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
            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 {
214
            // no proper interval found, must be the bi 
215
            // find the opposite-sign interval 
216
            for(uint8_t i = 0; i < 200; i ++){
217
218
219
                boolean intSign = (calib_readings[i + 1] - calib_readings[i]) > 0 ? true : false;
                if(intSign != sign){
                    interval = i;
Jake Read's avatar
Jake Read committed
220
                    bi = true; // mark the bad interval
221
222
223
                    break;
                }
            }
Jake Read's avatar
Jake Read committed
224
225
226
227
228
229
            if(!bi){
                // truly strange 
                sysError("missing interval, exiting");
                return false;
            }
            /*
230
231
232
233
            sysError("bad interval at: " + String(e) 
                    + " " + String(interval)
                    + " " + String(calib_readings[interval]) 
                    + " " + String(calib_readings[interval + 1]));
Jake Read's avatar
Jake Read committed
234
            */
235
        }
236
237
238

        // (3) have the interval (one is bad), 
        // find real angles (ra0, ra1)
239
240
        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 
241
242
243
244
245
246
        // 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;
Jake Read's avatar
Jake Read committed
247
        if(bi){
248
249
250
251
252
253
            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;
Jake Read's avatar
Jake Read committed
254
255
            }
        }
256
257
258
259
260
261

        // (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");
262
263
264
            return false;
        }
        // find pos. inside of interval 
265
        float offset = (spot - er0) / intSpan;
Jake Read's avatar
Jake Read committed
266
        // find real angle offset at e, modulo for the bad interval 
267
        float ra = (ra0 + (ra1 - ra0) * offset);
268
        // log those 
269
270
271
272
273
274
275
276
277
278
279
        if(false){
            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);            
Jake Read's avatar
Jake Read committed
280
        }
281
282
        // ok, have the real angle (ra) at the encoder tick (e), now write it 
        flash_write_value(ra); // this just happens in order, we zeroe'd out global counters at the start 
283
    } // end sweep thru 2^14 pts 
Jake Read's avatar
Jake Read committed
284
    sysError("calib complete");
285
    return true; // went OK 
286
287
288
}

void Step_CL::print_table(void){
289
    for(uint16_t e = 0; e < 4; e ++){
Jake Read's avatar
Jake Read committed
290
        float ra = ((float*)start_addr)[e];//lut[e];
291
292
293
        sysError("e: " + String(e) + " ra: " + String(ra, 4));
        delay(5);
    }
Jake Read's avatar
Jake Read committed
294
}