step_cl.cpp 9.24 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
/*
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"
Jake Read's avatar
Jake Read committed
16
#include "../utils/FlashStorage.h"
Jake Read's avatar
Jake Read committed
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36

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
37
#define CALIB_STEP_DELAY 10
38
#define CALIB_SETTLE_DELAY 1
39
40
41
#define CALIB_SAMPLE_PER_TICK 10 

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

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

Jake Read's avatar
Jake Read committed
50
51
52
// #define BYTES_PER_PAGE 512 
// #define FLOATS_PER_PAGE 128
// #define PAGES_PER_BLOCK 16 // for 8192 bytes / block 
Jake Read's avatar
Jake Read committed
53
54
55
56

//const float __attribute__((__aligned__(BYTES_PER_PAGE))) lut[LUT_SIZE] = {}; 
//const void* page_ptr; 

Jake Read's avatar
Jake Read committed
57
58
#define BYTES_PER_BLOCK 8192
#define FLOATS_PER_BLOCK 2048
Jake Read's avatar
Jake Read committed
59

Jake Read's avatar
Jake Read committed
60
61
const float __attribute__((__aligned__(8192))) lut[16834] = {};
FlashClass flashClass((const uint8_t*)lut);
Jake Read's avatar
Jake Read committed
62

Jake Read's avatar
Jake Read committed
63
64
// write mechanics
const void* block_ptr;
Jake Read's avatar
Jake Read committed
65

Jake Read's avatar
Jake Read committed
66
67
// write buffer 
static float buffer[FLOATS_PER_BLOCK];
Jake Read's avatar
Jake Read committed
68

Jake Read's avatar
Jake Read committed
69
70
uint32_t bfi = 0; // buffer indice 
uint32_t bli = 0; // block indice 
Jake Read's avatar
Jake Read committed
71

Jake Read's avatar
Jake Read committed
72
//FlashStorage(flash_storage, flts);
73

Jake Read's avatar
Jake Read committed
74
void flash_write_init(void){
Jake Read's avatar
Jake Read committed
75
76
77
    block_ptr = (const uint8_t*) lut;
    bfi = 0;
    bli = 0;
78
79
}

Jake Read's avatar
Jake Read committed
80
void flash_write_page(void){
Jake Read's avatar
Jake Read committed
81
82
    sysError("erasing");
    flashClass.erase(block_ptr, BYTES_PER_BLOCK);
Jake Read's avatar
Jake Read committed
83
    sysError("writing");
Jake Read's avatar
Jake Read committed
84
    flashClass.write(block_ptr, (const uint8_t*)buffer, BYTES_PER_BLOCK);
Jake Read's avatar
Jake Read committed
85
    delay(100);
Jake Read's avatar
Jake Read committed
86
87
}

Jake Read's avatar
Jake Read committed
88
void flash_write_value(float val){
Jake Read's avatar
Jake Read committed
89
90
    buffer[bfi ++] = val;
    if(bfi >= FLOATS_PER_BLOCK){
Jake Read's avatar
Jake Read committed
91
        flash_write_page();
Jake Read's avatar
Jake Read committed
92
        bfi = 0;
Jake Read's avatar
Jake Read committed
93
94
        bli ++;
        block_ptr = ((const uint8_t *)(&(lut[bli * FLOATS_PER_BLOCK])));
Jake Read's avatar
Jake Read committed
95
    }
96
97
}

Jake Read's avatar
Jake Read committed
98
void Step_CL::print_table(void){
Jake Read's avatar
Jake Read committed
99
100
101
102
    sysError("reading from lut");
    for(uint32_t i = 0; i < ENCODER_COUNTS; i ++){
        float ra = lut[i];
        sysError(String(ra));
Jake Read's avatar
Jake Read committed
103
        delay(5);
104
    }
105
106
107
}

// the calib routine 
108
boolean Step_CL::calibrate(void){
Jake Read's avatar
Jake Read committed
109
    flash_write_init();
Jake Read's avatar
Jake Read committed
110
    for(uint32_t i = 0; i < ENCODER_COUNTS; i ++){
Jake Read's avatar
Jake Read committed
111
112
        flash_write_value(i * 1.1F);
    }
Jake Read's avatar
Jake Read committed
113
    return true;
114
115
    // (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
116
    for(uint8_t i = 0; i < 200; i ++){ 
117
118
119
        // pt to new angle 
        stepper_hw->point(phase_angle, CALIB_CSCALE);
        // wait to settle / go slowly 
Jake Read's avatar
Jake Read committed
120
        delay(CALIB_STEP_DELAY);
121
122
123
124
125
126
127
128
129
130
131
        // 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 
132
            delay(CALIB_SETTLE_DELAY); 
133
134
135
136
137
138
139
140
        }
        // 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
141
    } // end measurement taking 
142
143
    // 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
144
    if(false){ // debug print intervals 
145
        for(uint8_t i = 0; i < 200; i ++){
146
147
148
149
150
            sysError("int: " + String(i) 
                        + " " + String(calib_readings[i], 4)
                        + " " + String(calib_readings[i + 1], 4));
            delay(2);
        }
Jake Read's avatar
Jake Read committed
151
    }
152
153
154
155
156
157
158
159
160
161
162
163
164
165
    // 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
166
    // (2) build the table, walk all encoder counts... 
167
168
    // now to build the actual table... 
    // want to start with the 0 indice, 
169
170
    flash_write_init();
    for(uint16_t e = 0; e < 4; e ++){
Jake Read's avatar
Jake Read committed
171
172
        // find the interval that spans this sample
        boolean bi = false; 
173
        int16_t interval = -1;
174
        for(uint8_t i = 0; i < 200; i ++){
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
            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 {
193
            // no proper interval found, must be the bi 
194
            // find the opposite-sign interval 
195
            for(uint8_t i = 0; i < 200; i ++){
196
197
198
                boolean intSign = (calib_readings[i + 1] - calib_readings[i]) > 0 ? true : false;
                if(intSign != sign){
                    interval = i;
Jake Read's avatar
Jake Read committed
199
                    bi = true; // mark the bad interval
200
201
202
                    break;
                }
            }
Jake Read's avatar
Jake Read committed
203
204
205
206
207
208
            if(!bi){
                // truly strange 
                sysError("missing interval, exiting");
                return false;
            }
            /*
209
210
211
212
            sysError("bad interval at: " + String(e) 
                    + " " + String(interval)
                    + " " + String(calib_readings[interval]) 
                    + " " + String(calib_readings[interval + 1]));
Jake Read's avatar
Jake Read committed
213
            */
214
        }
215
216
217

        // (3) have the interval (one is bad), 
        // find real angles (ra0, ra1)
218
219
        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 
220
221
222
223
224
225
        // 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
226
        if(bi){
227
228
229
230
231
232
            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
233
234
            }
        }
235
236
237
238
239
240

        // (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");
241
242
243
            return false;
        }
        // find pos. inside of interval 
244
        float offset = (spot - er0) / intSpan;
Jake Read's avatar
Jake Read committed
245
        // find real angle offset at e, modulo for the bad interval 
246
        float ra = (ra0 + (ra1 - ra0) * offset);
247
        // log those 
248
249
250
251
252
253
254
255
256
257
258
        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
259
        }
260
        // ok, have the real angle (ra) at the encoder tick (e), now write it 
Jake Read's avatar
Jake Read committed
261
        //flash_write_value(ra); // this just happens in order, we zeroe'd out global counters at the start 
262
    } // end sweep thru 2^14 pts 
Jake Read's avatar
Jake Read committed
263
    sysError("calib complete");
264
    return true; // went OK 
265
266
}