Commit b6003cb0 authored by Jake Read's avatar Jake Read
Browse files

thru calibration stage, now to write flash

parent 98c65f01
......@@ -34,6 +34,7 @@ Step_CL::Step_CL(void){}
#define CALIB_CSCALE 0.4F
#define CALIB_STEP_DELAY 10
#define CALIB_SETTLE_DELAY 1
#define CALIB_SAMPLE_PER_TICK 10
#define ENCODER_COUNTS 16384
......@@ -64,7 +65,7 @@ boolean Step_CL::calibrate(void){
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);
delay(CALIB_SETTLE_DELAY);
}
// push reading, average removes the wraps added to readings.
calib_readings[i] = atan2(y, x);//(reading / (float)CALIB_SAMPLE_PER_TICK) - ENCODER_COUNTS;
......@@ -74,8 +75,10 @@ boolean Step_CL::calibrate(void){
phase_angle += 0.25F;
if(phase_angle >= 1.0F) phase_angle = 0.0F;
} // end measurement taking
// tack end-wrap together, to easily find the wrap-at-indice interval
calib_readings[200] = calib_readings[0];
if(false){ // debug print intervals
for(uint8_t i = 0; i < 199; i ++){
for(uint8_t i = 0; i < 200; i ++){
sysError("int: " + String(i)
+ " " + String(calib_readings[i], 4)
+ " " + String(calib_readings[i + 1], 4));
......@@ -103,7 +106,7 @@ boolean Step_CL::calibrate(void){
// find the interval that spans this sample
boolean bi = false;
int16_t interval = -1;
for(uint8_t i = 0; i < 199; i ++){
for(uint8_t i = 0; i < 200; i ++){
if(sign){ // +ve slope readings, left < right
if(calib_readings[i] < e && e <= calib_readings[i + 1]){
interval = i;
......@@ -122,8 +125,9 @@ boolean Step_CL::calibrate(void){
// + " " + String(calib_readings[interval])
// + " " + String(calib_readings[interval + 1]));
} else {
// no proper interval found, must be the bi
// find the opposite-sign interval
for(uint8_t i = 0; i < 199; i ++){
for(uint8_t i = 0; i < 200; i ++){
boolean intSign = (calib_readings[i + 1] - calib_readings[i]) > 0 ? true : false;
if(intSign != sign){
interval = i;
......@@ -143,35 +147,53 @@ boolean Step_CL::calibrate(void){
+ " " + String(calib_readings[interval + 1]));
*/
}
// find anchors
// (3) have the interval (one is bad),
// find real angles (ra0, ra1)
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];
// invent a span for the bad interval,
// 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;
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;
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;
}
}
if(intSpan < 0.1F && intSpan > -0.1F){
sysError("short interval, exiting");
// (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");
return false;
}
// find pos. inside of interval
float offset = ((float)e - calib_readings[interval]) / intSpan;
float offset = (spot - er0) / intSpan;
// find real angle offset at e, modulo for the bad interval
float ra = (ra0 - (ra1 - ra0) * offset);
float ra = (ra0 + (ra1 - ra0) * offset);
// wrap to 360 degs,
/*
if(ra < 0.0F){
ra += 360.0F;
} else if (ra > 360.0F){
ra -= 360.0F;
}
*/
// log those
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));
}
......
......@@ -25,7 +25,7 @@ is; no warranty is provided, and users accept all liability.
class Step_CL {
private:
static Step_CL* instance;
float calib_readings[200];
float calib_readings[201];
public:
Step_CL();
......
......@@ -6,6 +6,10 @@ With 2x A4950s on the DAC, AS5047P on the encoder, etc.
- magnet alignment print / tool for the glue-in
## Likely Bugs
- haven't tested with '+ve' signed calibrations, i.e. if motor += magnetic step does encoder += tick step.
## 2020 10 06
OK, today am just waking this thing up. Have magnet glued (curing) on the back of the motor now, and one encoder soldered up on an old module-based stepper board - code should be the same.
......@@ -547,4 +551,216 @@ Borked, indeed. Will check tomorrow. Errors just before the BI (nan, so our of i
## 2020 10 08
...
\ No newline at end of file
Ok, updating the interpolation, seems messy but I think (?) maybe this shot will work - at least for -ve signed samples.
I think.. also, if this doesn't work, I might tackle this differently - by knocking the tail down or up, then wrapping angles.
Ugh, still doesn't work. Here was the attempt:
```cpp
// (4) for the bad interval, some more work to do to modify interp. points
if(bi){
// find the zero crossing within the wrapping interval
float zc, l0, l1;
if(sign){
float l0 = ENCODER_COUNTS - er0;
float l1 = er1;
} else {
float l0 = er0;
float l1 = ENCODER_COUNTS - er1;
}
zc = l0 / (l0 + l1);
float raMid = (ra0 - (ra1 - ra0)) * zc;
// modify the interval tails, depending on side of zero crossing
if( (sign && er0 < e) || (!sign && er0 > e) ){
// this is left of zc, modify right tail
ra1 = raMid; // find the about-zero pt,
if(sign){
er1 = ENCODER_COUNTS;
} else {
er1 = 0;
}
} else {
// this is right of zc, modify left tail
ra0 = raMid;
if(sign){
er0 = 0;
} else {
er0 = ENCODER_COUNTS;
}
}
}
```
I'm going to try to get this right for the normal intervals first...
Getting much closer, have implemented a simpler wrap system, looks like the first half works but the second is confused.
I am still getting these *three* instances of the BI, tho I had always expected to just get two - one at the head of the thing (0 -> first good interval) and the second at the end (last gi -> 2^14). The middle instance is where this seems broken... I'll inspect around there.
Yeah, I've these readings in the middle of things that are not appearing to live in an interval.
Right, of course, this is the one that lives in the wrap on readings. I could stick another reading on there, or wrap the indices... should check if the mechanical wrap ~= the code wrap...
OK - just tacked the last interval on there, so I have 201 readings (201st being == 1st), and 200 intervals. Here's the full calibration routine, w/o saving the giant array:
```cpp
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;
for(uint8_t i = 0; i < 200; i ++){
// pt to new angle
stepper_hw->point(phase_angle, CALIB_CSCALE);
// wait to settle / go slowly
delay(CALIB_STEP_DELAY);
// 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(CALIB_SETTLE_DELAY);
}
// 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;
} // end measurement taking
// tack end-wrap together, to easily find the wrap-at-indice interval
calib_readings[200] = calib_readings[0];
if(false){ // debug print intervals
for(uint8_t i = 0; i < 200; i ++){
sysError("int: " + String(i)
+ " " + String(calib_readings[i], 4)
+ " " + String(calib_readings[i + 1], 4));
delay(2);
}
}
// 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));
// (2) build the table, walk all encoder counts...
// now to build the actual table...
// want to start with the 0 indice,
for(uint16_t e = 0; e < ENCODER_COUNTS; e ++){
// find the interval that spans this sample
boolean bi = false;
int16_t interval = -1;
for(uint8_t i = 0; i < 200; 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 {
// no proper interval found, must be the bi
// find the opposite-sign interval
for(uint8_t i = 0; i < 200; i ++){
boolean intSign = (calib_readings[i + 1] - calib_readings[i]) > 0 ? true : false;
if(intSign != sign){
interval = i;
bi = true; // mark the bad interval
break;
}
}
if(!bi){
// truly strange
sysError("missing interval, exiting");
return false;
}
/*
sysError("bad interval at: " + String(e)
+ " " + String(interval)
+ " " + String(calib_readings[interval])
+ " " + String(calib_readings[interval + 1]));
*/
}
// (3) have the interval (one is bad),
// find real angles (ra0, ra1)
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
// 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;
if(bi){
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;
}
}
// (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");
return false;
}
// find pos. inside of interval
float offset = (spot - er0) / intSpan;
// find real angle offset at e, modulo for the bad interval
float ra = (ra0 + (ra1 - ra0) * offset);
// wrap to 360 degs,
/*
if(ra < 0.0F){
ra += 360.0F;
} else if (ra > 360.0F){
ra -= 360.0F;
}
*/
// log those
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);
} // end sweep thru 2^14 pts
sysError("calib complete");
return true; // went OK
}
```
So next up is writing that to flash so that I can use it.
\ No newline at end of file
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment