step_cl.cpp 11.1 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
#include "../utils/clamp.h"
Jake Read's avatar
Jake Read committed
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32

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();

Step_CL::Step_CL(void){}

#define CALIB_CSCALE 0.4F
33
#define CALIB_STEP_DELAY 10
34
#define CALIB_SETTLE_DELAY 1
35
36
37
#define CALIB_SAMPLE_PER_TICK 10 

#define ENCODER_COUNTS 16384
Jake Read's avatar
Jake Read committed
38
39

void Step_CL::init(void){
Jake Read's avatar
Jake Read committed
40
    step_a4950->init(false, 0.4);
Jake Read's avatar
Jake Read committed
41
    enc_as5047->init();
Jake Read's avatar
Jake Read committed
42
    _tc = 0; // torque command -> 0; 
Jake Read's avatar
Jake Read committed
43
    is_calibrating = false;
Jake Read's avatar
Jake Read committed
44
45
}

Jake Read's avatar
Jake Read committed
46
47
48
49
50
51
52
53
54
55
56
// LUT / flash work 
/*
the D51J19 flash is organized into *pages* of 512bytes, 
and *blocks* of 16 pages (8192 bytes)
write granularity is to the page, erase granularity is per block 
we must erase flash before writing to it: this is how flash hardware works 
to do flash storage of this monster table, we declare a const LUT array
const keyword will cause the compiler to put it in flash mem, 
and so we point the helper class at that void* (the head of the array) to start, 
and increment that void* thru the array in blocks, when our buffer is full
*/
Jake Read's avatar
Jake Read committed
57

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

Jake Read's avatar
Jake Read committed
61
const float __attribute__((__aligned__(8192))) lut[32768] = {}; // the actual LUT: const means it gets allocated to flash 
Jake Read's avatar
Jake Read committed
62
63
64
FlashClass flashClass((const uint8_t*)lut); // helper class (lib) // https://github.com/cmaglie/FlashStorage
const void* block_ptr; // void* to section-of-lut for write 
static float buffer[FLOATS_PER_BLOCK]; // one full block (16 pages) of flash mem, buffered 
Jake Read's avatar
Jake Read committed
65

Jake Read's avatar
Jake Read committed
66
67
uint32_t bfi = 0; // buffer indice 
uint32_t bli = 0; // block indice 
Jake Read's avatar
Jake Read committed
68

Jake Read's avatar
Jake Read committed
69
void flash_write_init(void){
Jake Read's avatar
Jake Read committed
70
71
72
    block_ptr = (const uint8_t*) lut;
    bfi = 0;
    bli = 0;
73
74
}

Jake Read's avatar
Jake Read committed
75
void flash_write_page(void){
Jake Read's avatar
Jake Read committed
76
    sysError("erasing 0x" + String((uint32_t)block_ptr));
Jake Read's avatar
Jake Read committed
77
    flashClass.erase(block_ptr, BYTES_PER_BLOCK);
Jake Read's avatar
Jake Read committed
78
    sysError("writing 0x" + String((uint32_t)block_ptr));
Jake Read's avatar
Jake Read committed
79
    flashClass.write(block_ptr, (const uint8_t*)buffer, BYTES_PER_BLOCK);
Jake Read's avatar
Jake Read committed
80
    delay(10);
Jake Read's avatar
Jake Read committed
81
82
}

Jake Read's avatar
Jake Read committed
83
void flash_write_value(float val){
Jake Read's avatar
Jake Read committed
84
85
    buffer[bfi ++] = val;
    if(bfi >= FLOATS_PER_BLOCK){
Jake Read's avatar
Jake Read committed
86
        flash_write_page();
Jake Read's avatar
Jake Read committed
87
        bfi = 0;
Jake Read's avatar
Jake Read committed
88
        bli ++;
Jake Read's avatar
Jake Read committed
89
        block_ptr = ((const uint8_t *)(&(lut[bli * FLOATS_PER_BLOCK]))); 
Jake Read's avatar
Jake Read committed
90
    }
91
92
}

Jake Read's avatar
Jake Read committed
93
void Step_CL::print_table(void){
Jake Read's avatar
Jake Read committed
94
    sysError("reading from lut");
Jake Read's avatar
Jake Read committed
95
96
97
98
    for(uint32_t i = 0; i < ENCODER_COUNTS * 2; i ++){
        float ra = lut[i * 2];
        float pa = lut[i * 2 + 1];
        sysError("real angle at enc " + String(i) + ": " + String(ra) + "phase angle: " + String(pa));
Jake Read's avatar
Jake Read committed
99
        delay(5);
100
    }
101
102
}

Jake Read's avatar
Jake Read committed
103
104
105
106
107
108
109
// set twerks 
// tc: -1 : 1
void Step_CL::set_torque(float tc){
    clamp(&tc, -1.0F, 1.0F);
    _tc = tc;
}

Jake Read's avatar
Jake Read committed
110
111
112
113
float Step_CL::get_torque(void){
    return _tc;
}

Jake Read's avatar
Jake Read committed
114
115
// the control loop 
void Step_CL::run_torque_loop(void){
Jake Read's avatar
Jake Read committed
116
    if(is_calibrating) return;
Jake Read's avatar
Jake Read committed
117
118
    // mark time 
    DEBUG1PIN_ON;
Jake Read's avatar
Jake Read committed
119
120
121
122
123
    // ok, first we read the encoder 
    enc_as5047->trigger_read();
    // this kicks off the party, proceeds below
}

Jake Read's avatar
Jake Read committed
124
#define MAP_7p2_TO_1 (1.0F / 7.2F)
Jake Read's avatar
Jake Read committed
125
126
volatile float _ra;
volatile float _pa;
Jake Read's avatar
Jake Read committed
127

Jake Read's avatar
Jake Read committed
128
void ENC_AS5047::on_read_complete(uint16_t result){
Jake Read's avatar
Jake Read committed
129
    if(step_cl->is_calibrating) return;
Jake Read's avatar
Jake Read committed
130
131
    _ra = lut[result * 2];          // the real angle (position 0-360)
    _pa = lut[result * 2 + 1];      // the phase angle (0 - 1 in a sweep of 4 steps)
Jake Read's avatar
Jake Read committed
132
133
    // this is the phase angle we want to apply, 90 degs off & wrap't to 1 
    if(step_cl->get_torque() < 0){
Jake Read's avatar
Jake Read committed
134
135
136
        _pa -= 0.25; // 90* phase swop 
        if(_pa < 0){
            _pa += 1.0F;
Jake Read's avatar
Jake Read committed
137
138
        }
    } else {
Jake Read's avatar
Jake Read committed
139
140
141
        _pa += 0.25;
        if(_pa > 1){
            _pa -= 1.0F;
Jake Read's avatar
Jake Read committed
142
143
144
145
        }
    }
    // now we ask our voltage modulation machine to put this on the coils 
    // with the *amount* commanded by our _tc torque ask 
Jake Read's avatar
Jake Read committed
146
    step_a4950->point(_pa, abs(step_cl->get_torque()));
Jake Read's avatar
Jake Read committed
147
148
149
150
    // debug loop completion 
    DEBUG1PIN_OFF;
}

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

        // (3) have the interval (one is bad), 
        // find real angles (ra0, ra1)
259
260
        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 
261
262
263
264
265
266
        // 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
267
        if(bi){
268
269
270
271
272
273
            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
274
275
            }
        }
276
277
278
279
280
281

        // (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");
282
283
284
            return false;
        }
        // find pos. inside of interval 
285
        float offset = (spot - er0) / intSpan;
Jake Read's avatar
Jake Read committed
286
        // find real angle offset at e, modulo for the bad interval 
287
        float ra = (ra0 + (ra1 - ra0) * offset);
288
        // log those 
289
290
291
292
293
294
295
296
297
298
299
        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
300
        }
301
        // ok, have the real angle (ra) at the encoder tick (e), now write it 
Jake Read's avatar
Jake Read committed
302
303
304
305
306
307
308
        flash_write_value(ra); // log the real angle here 
        float pa = ra;
        while(pa > 7.2F){
            pa -= 7.2F;
        }
        pa = pa * MAP_7p2_TO_1;
        flash_write_value(pa); // log the phase angle beside it 
309
    } // end sweep thru 2^14 pts 
Jake Read's avatar
Jake Read committed
310
    sysError("calib complete");
Jake Read's avatar
Jake Read committed
311
    is_calibrating = false;
312
    return true; // went OK 
313
314
}