step_cl.cpp 8.12 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();
}

Jake Read's avatar
Jake Read committed
49
FlashStorage(flash_store, float);
50

Jake Read's avatar
Jake Read committed
51
void flash_write_init(void){
52

53
54
}

Jake Read's avatar
Jake Read committed
55
56
void flash_write_value(float val){
    flash_store.write(val);
57
58
}

Jake Read's avatar
Jake Read committed
59
60
61
62
63
void Step_CL::print_table(void){
    for(uint16_t e = 0; e < 4; e ++){
        float ra = flash_store.read();
        sysError("e: " + String(e) + " ra: " + String(ra, 4));
        delay(5);
64
    }
65
66
67
}

// the calib routine 
68
boolean Step_CL::calibrate(void){
Jake Read's avatar
Jake Read committed
69
70
    flash_write_value(100.0F);
    return true;
71
72
    // (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
73
    for(uint8_t i = 0; i < 200; i ++){ 
74
75
76
        // pt to new angle 
        stepper_hw->point(phase_angle, CALIB_CSCALE);
        // wait to settle / go slowly 
Jake Read's avatar
Jake Read committed
77
        delay(CALIB_STEP_DELAY);
78
79
80
81
82
83
84
85
86
87
88
        // 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 
89
            delay(CALIB_SETTLE_DELAY); 
90
91
92
93
94
95
96
97
        }
        // 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
98
    } // end measurement taking 
99
100
    // 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
101
    if(false){ // debug print intervals 
102
        for(uint8_t i = 0; i < 200; i ++){
103
104
105
106
107
            sysError("int: " + String(i) 
                        + " " + String(calib_readings[i], 4)
                        + " " + String(calib_readings[i + 1], 4));
            delay(2);
        }
Jake Read's avatar
Jake Read committed
108
    }
109
110
111
112
113
114
115
116
117
118
119
120
121
122
    // 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
123
    // (2) build the table, walk all encoder counts... 
124
125
    // now to build the actual table... 
    // want to start with the 0 indice, 
126
127
    flash_write_init();
    for(uint16_t e = 0; e < 4; e ++){
Jake Read's avatar
Jake Read committed
128
129
        // find the interval that spans this sample
        boolean bi = false; 
130
        int16_t interval = -1;
131
        for(uint8_t i = 0; i < 200; i ++){
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
            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 {
150
            // no proper interval found, must be the bi 
151
            // find the opposite-sign interval 
152
            for(uint8_t i = 0; i < 200; i ++){
153
154
155
                boolean intSign = (calib_readings[i + 1] - calib_readings[i]) > 0 ? true : false;
                if(intSign != sign){
                    interval = i;
Jake Read's avatar
Jake Read committed
156
                    bi = true; // mark the bad interval
157
158
159
                    break;
                }
            }
Jake Read's avatar
Jake Read committed
160
161
162
163
164
165
            if(!bi){
                // truly strange 
                sysError("missing interval, exiting");
                return false;
            }
            /*
166
167
168
169
            sysError("bad interval at: " + String(e) 
                    + " " + String(interval)
                    + " " + String(calib_readings[interval]) 
                    + " " + String(calib_readings[interval + 1]));
Jake Read's avatar
Jake Read committed
170
            */
171
        }
172
173
174

        // (3) have the interval (one is bad), 
        // find real angles (ra0, ra1)
175
176
        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 
177
178
179
180
181
182
        // 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
183
        if(bi){
184
185
186
187
188
189
            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
190
191
            }
        }
192
193
194
195
196
197

        // (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");
198
199
200
            return false;
        }
        // find pos. inside of interval 
201
        float offset = (spot - er0) / intSpan;
Jake Read's avatar
Jake Read committed
202
        // find real angle offset at e, modulo for the bad interval 
203
        float ra = (ra0 + (ra1 - ra0) * offset);
204
        // log those 
205
206
207
208
209
210
211
212
213
214
215
        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
216
        }
217
218
        // 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 
219
    } // end sweep thru 2^14 pts 
Jake Read's avatar
Jake Read committed
220
    sysError("calib complete");
221
    return true; // went OK 
222
223
}