step_cl.cpp 6.68 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
37
38
39
#define CALIB_STEP_DELAY 10
#define CALIB_SAMPLE_PER_TICK 10 

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

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

48
49
50
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
51
    for(uint8_t i = 0; i < 200; i ++){ 
52
53
54
        // pt to new angle 
        stepper_hw->point(phase_angle, CALIB_CSCALE);
        // wait to settle / go slowly 
Jake Read's avatar
Jake Read committed
55
        delay(CALIB_STEP_DELAY);
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
        // 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 
            delay(1); 
        }
        // 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
76
77
    } // end measurement taking 
    if(false){ // debug print intervals 
78
79
80
81
82
83
        for(uint8_t i = 0; i < 199; i ++){
            sysError("int: " + String(i) 
                        + " " + String(calib_readings[i], 4)
                        + " " + String(calib_readings[i + 1], 4));
            delay(2);
        }
Jake Read's avatar
Jake Read committed
84
    }
85
86
87
88
89
90
91
92
93
94
95
96
97
98
    // 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
99
    // (2) build the table, walk all encoder counts... 
100
101
102
    // now to build the actual table... 
    // want to start with the 0 indice, 
    for(uint16_t e = 0; e < ENCODER_COUNTS; e ++){
Jake Read's avatar
Jake Read committed
103
104
        // find the interval that spans this sample
        boolean bi = false; 
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
        int16_t interval = -1;
        for(uint8_t i = 0; i < 199; i ++){
            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 {
            // find the opposite-sign interval 
            for(uint8_t i = 0; i < 199; i ++){
                boolean intSign = (calib_readings[i + 1] - calib_readings[i]) > 0 ? true : false;
                if(intSign != sign){
                    interval = i;
Jake Read's avatar
Jake Read committed
130
                    bi = true; // mark the bad interval
131
132
133
                    break;
                }
            }
Jake Read's avatar
Jake Read committed
134
135
136
137
138
139
            if(!bi){
                // truly strange 
                sysError("missing interval, exiting");
                return false;
            }
            /*
140
141
142
143
            sysError("bad interval at: " + String(e) 
                    + " " + String(interval)
                    + " " + String(calib_readings[interval]) 
                    + " " + String(calib_readings[interval + 1]));
Jake Read's avatar
Jake Read committed
144
            */
145
146
147
148
149
150
        }
        // find anchors 
        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 
        // check we are not abt to div / 0: this could happen if motor did not turn during measurement 
        float intSpan = calib_readings[interval - 1] - calib_readings[interval];
Jake Read's avatar
Jake Read committed
151
152
153
154
155
156
157
158
        // invent a span for the bad interval, 
        if(bi){
            if(sign){
                intSpan = calib_readings[interval - 1] - calib_readings[interval] + (float)ENCODER_COUNTS;
            } else {
                intSpan = calib_readings[interval - 1] - calib_readings[interval] - (float)ENCODER_COUNTS;
            }
        }
159
160
161
162
163
164
        if(intSpan < 0.1F && intSpan > -0.1F){
            sysError("short interval, exiting");
            return false;
        }
        // find pos. inside of interval 
        float offset =  ((float)e - calib_readings[interval]) / intSpan;
Jake Read's avatar
Jake Read committed
165
166
167
168
169
170
171
        // find real angle offset at e, modulo for the bad interval 
        float ra = (ra0 - (ra1 - ra0) * offset);
        if(ra < 0.0F){
            ra += 360.0F;
        } else if (ra > 360.0F){
            ra -= 360.0F;
        }
172
        // log those 
Jake Read's avatar
Jake Read committed
173
174
175
176
177
178
        if(bi){
            sysError("e: " + String(e) + " ra: " + String(ra, 4) + " BI");
        } else {
            sysError("e: " + String(e) + " ra: " + String(ra, 4));
        }
        delay(10);
179
    } // end sweep thru 2^14 pts 
Jake Read's avatar
Jake Read committed
180
    sysError("calib complete");
181
    return true; // went OK 
Jake Read's avatar
Jake Read committed
182
}