step_cl.cpp 11.4 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
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
// 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] = {};

static const unsigned page_size = PAGE_SIZE;    // size of each page in flash mem, read out w/ NVMCTRL->PARAM.bit.PSZ 
static const unsigned floats_per_page = page_size / sizeof(float); // count of floats that can be stored in each page 
static float page[floats_per_page]; // a temporary page-width of floats, to buffer before writing to flash mem 
static unsigned page_f; // the page[page_f - 1] == the last float we wrote 
static const void * page_ptr = (const uint8_t *) lut; // ptr to the address in flash memory where we want to write 
static unsigned num_pages_written; // counting  

static void start_flash_write(void){
    num_pages_written = 0;
    page_f = 0;
    sysError("Writing to flash 0x" + String((uintptr_t) page_ptr, HEX));
    sysError("Page size PSZ= " + String(NVMCTRL->PARAM.bit.PSZ)); // datasheet for this 
    // try with wqw, and auto update? 
    NVMCTRL->CTRLA.bit.WMODE = NVMCTRL_CTRLA_WMODE_MAN;
}

static void flash_disable_cache(void){
    NVMCTRL->CTRLA.bit.CACHEDIS0 = 1;
    NVMCTRL->CTRLA.bit.CACHEDIS1 = 1;
}

static void flash_enable_cache(void){
    NVMCTRL->CTRLA.bit.CACHEDIS0 = 0;
    NVMCTRL->CTRLA.bit.CACHEDIS1 = 0;
}

static void flash_erase_page(const volatile void *flash_ptr){
    NVMCTRL->ADDR.reg = ((uint32_t)flash_ptr);
    NVMCTRL->CTRLB.reg = NVMCTRL_CTRLB_CMDEX_KEY | NVMCTRL_CTRLB_CMD_EB;
    //while(!NVMCTRL->INTFLAG.bit.DONE);
    while(!NVMCTRL->STATUS.bit.READY);
}

static inline uint32_t read_unaligned_uint32(const void *data)
{
  union {
    uint32_t u32;
    uint8_t u8[4];
  } res;
  const uint8_t *d = (const uint8_t *)data;
  res.u8[0] = d[0];
  res.u8[1] = d[1];
  res.u8[2] = d[2];
  res.u8[3] = d[3];
  return res.u32;
}

static void flash_write_page(const volatile void *flash_ptr, const void *data, uint32_t size){
    // disable cache, 
    // sysError("disable cache");
    // flash_disable_cache();
    // erase the page, 
    sysError("erase");
    flash_erase_page(flash_ptr);
    // copy into page buffer (?) 
    // do page buffer clear 
    sysError("page buffer clear");
    NVMCTRL->ADDR.reg = ((uint32_t)flash_ptr);
    NVMCTRL->CTRLB.reg = NVMCTRL_CTRLB_CMDEX_KEY | NVMCTRL_CTRLB_CMD_PBC;
    while(!NVMCTRL->STATUS.bit.READY);
    // do page buffer write (?) 
    volatile uint32_t *dst_addr = (volatile uint32_t *)flash_ptr;
    const uint8_t *src_addr = (uint8_t*)data;
    uint32_t i;
    for (i=0; i<(PAGE_SIZE/4) && size; i++) {
      *dst_addr = read_unaligned_uint32(src_addr);
      src_addr += 4;
      dst_addr++;
      size--;
    }
    // do page write (writes to page in ADDR register, from page buffer)
    sysError("page write");
    NVMCTRL->ADDR.reg = ((uint32_t)flash_ptr);
    NVMCTRL->CTRLB.reg = NVMCTRL_CTRLB_CMDEX_KEY | NVMCTRL_CTRLB_CMD_WP;
    while(!NVMCTRL->STATUS.bit.READY);
    // re-enable cache 
    // sysError("enable cache");
    // flash_enable_cache();
}

static void flash_write_value(float val){
    page[page_f ++] = val;
    if(page_f < floats_per_page) return;
    // page is full, commit to flash 
    sysError("will write 0x" + String((uintptr_t) page_ptr, HEX) + " " + String(num_pages_written));
    flash_write_page(page_ptr, page, sizeof(page));
    delay(10);
    // reset counters 
    // page_ptr += sizeof(page);
    page_f = 0;
    memset(page, 0, sizeof(page));
}

// the calib routine 
148
149
150
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
151
    for(uint8_t i = 0; i < 200; i ++){ 
152
153
154
        // pt to new angle 
        stepper_hw->point(phase_angle, CALIB_CSCALE);
        // wait to settle / go slowly 
Jake Read's avatar
Jake Read committed
155
        delay(CALIB_STEP_DELAY);
156
157
158
159
160
161
162
163
164
165
166
        // 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 
167
            delay(CALIB_SETTLE_DELAY); 
168
169
170
171
172
173
174
175
        }
        // 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
176
    } // end measurement taking 
177
178
    // 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
179
    if(false){ // debug print intervals 
180
        for(uint8_t i = 0; i < 200; i ++){
181
182
183
184
185
            sysError("int: " + String(i) 
                        + " " + String(calib_readings[i], 4)
                        + " " + String(calib_readings[i + 1], 4));
            delay(2);
        }
Jake Read's avatar
Jake Read committed
186
    }
187
188
189
190
191
192
193
194
195
196
197
198
199
200
    // 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
201
    // (2) build the table, walk all encoder counts... 
202
203
    // now to build the actual table... 
    // want to start with the 0 indice, 
204
    start_flash_write();
205
    for(uint16_t e = 0; e < ENCODER_COUNTS; e ++){
Jake Read's avatar
Jake Read committed
206
207
        // find the interval that spans this sample
        boolean bi = false; 
208
        int16_t interval = -1;
209
        for(uint8_t i = 0; i < 200; i ++){
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
            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 {
228
            // no proper interval found, must be the bi 
229
            // find the opposite-sign interval 
230
            for(uint8_t i = 0; i < 200; i ++){
231
232
233
                boolean intSign = (calib_readings[i + 1] - calib_readings[i]) > 0 ? true : false;
                if(intSign != sign){
                    interval = i;
Jake Read's avatar
Jake Read committed
234
                    bi = true; // mark the bad interval
235
236
237
                    break;
                }
            }
Jake Read's avatar
Jake Read committed
238
239
240
241
242
243
            if(!bi){
                // truly strange 
                sysError("missing interval, exiting");
                return false;
            }
            /*
244
245
246
247
            sysError("bad interval at: " + String(e) 
                    + " " + String(interval)
                    + " " + String(calib_readings[interval]) 
                    + " " + String(calib_readings[interval + 1]));
Jake Read's avatar
Jake Read committed
248
            */
249
        }
250
251
252

        // (3) have the interval (one is bad), 
        // find real angles (ra0, ra1)
253
254
        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 
255
256
257
258
259
260
        // 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
261
        if(bi){
262
263
264
265
266
267
            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
268
269
            }
        }
270
271
272
273
274
275

        // (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");
276
277
278
            return false;
        }
        // find pos. inside of interval 
279
        float offset = (spot - er0) / intSpan;
Jake Read's avatar
Jake Read committed
280
        // find real angle offset at e, modulo for the bad interval 
281
        float ra = (ra0 + (ra1 - ra0) * offset);
282
        // log those 
283
284
285
286
287
288
289
290
291
292
293
        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
294
        }
295
296
        // 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 
297
    } // end sweep thru 2^14 pts 
Jake Read's avatar
Jake Read committed
298
    sysError("calib complete");
299
    return true; // went OK 
300
301
302
303
304
305
306
307
}

void Step_CL::print_table(void){
    for(uint16_t e = 0; e < ENCODER_COUNTS; e ++){
        float ra = lut[e];
        sysError("e: " + String(e) + " ra: " + String(ra, 4));
        delay(5);
    }
Jake Read's avatar
Jake Read committed
308
}