Commit 98c65f01 authored by Jake Read's avatar Jake Read
Browse files

fighting the bad interval

parent c058fd27
......@@ -48,7 +48,7 @@ void Step_CL::init(void){
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 ++){
for(uint8_t i = 0; i < 200; i ++){
// pt to new angle
stepper_hw->point(phase_angle, CALIB_CSCALE);
// wait to settle / go slowly
......@@ -73,9 +73,8 @@ boolean Step_CL::calibrate(void){
// rotate
phase_angle += 0.25F;
if(phase_angle >= 1.0F) phase_angle = 0.0F;
}
// debug print intervals
if(false){
} // end measurement taking
if(false){ // debug print intervals
for(uint8_t i = 0; i < 199; i ++){
sysError("int: " + String(i)
+ " " + String(calib_readings[i], 4)
......@@ -97,10 +96,12 @@ boolean Step_CL::calibrate(void){
}
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
// find the interval that spans this sample
boolean bi = false;
int16_t interval = -1;
for(uint8_t i = 0; i < 199; i ++){
if(sign){ // +ve slope readings, left < right
......@@ -126,31 +127,56 @@ boolean Step_CL::calibrate(void){
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]));
return false;
*/
}
// 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];
// 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;
}
}
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;
// find real angle offset at e
float ra = ra0 + (ra1 - ra0) * offset;
// 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;
}
// log those
sysError("ra: " + String(ra, 4));
delay(1);
if(bi){
sysError("e: " + String(e) + " ra: " + String(ra, 4) + " 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
}
\ No newline at end of file
......@@ -516,4 +516,35 @@ The maths-ey way to do this is to convert to cartesian coordinates, given the th
I guess doing this like that isn't too aweful, tho it's expensive. It would be awesome to have a faster method later on, but there probably better filters (like a kalman) make more sense than just a big ol' average.
I can imagine taking the measurement spreads: if any were larger than 20 ticks, I could sweep through that set and add the enc_count to the lo vals, then average. Or I could just do this for any reading < 31 ticks, as these are the small ones in *this particular window* but then I have that crawling window issue for measurements really ~ around 31 ticks. Might just be prettier to take a real circular average.
\ No newline at end of file
I can imagine taking the measurement spreads: if any were larger than 20 ticks, I could sweep through that set and add the enc_count to the lo vals, then average. Or I could just do this for any reading < 31 ticks, as these are the small ones in *this particular window* but then I have that crawling window issue for measurements really ~ around 31 ticks. Might just be prettier to take a real circular average.
Here's the circular average:
```cpp
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);
```
I'm getting some strange intervals back, seems like the 'bad interval' is showing up in three chunks, though it should be one span of angles. I think the best way to debug this would be to plot the whole thing out... or I *could* try straight printing it all back on the serport... maybe I'll try that before building a whole packet-transmission-of-hella-floats thing.
Yeah this looks all kinds of wrong.
Borked, indeed. Will check tomorrow. Errors just before the BI (nan, so our of index somewhere?) and the BI looks close but not quite there.
## 2020 10 08
...
\ 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