From 98c65f01a7d2d37ba5a42601dfec6db49dda7f6f Mon Sep 17 00:00:00 2001
From: Jake Read <jake.read@cba.mit.edu>
Date: Thu, 8 Oct 2020 22:37:10 -0400
Subject: [PATCH] fighting the bad interval
---
.../src/drivers/step_cl.cpp | 46 +++++++++++++++----
log/cl-step-control-log.md | 33 ++++++++++++-
2 files changed, 68 insertions(+), 11 deletions(-)
diff --git a/firmware/cl-step-controller/src/drivers/step_cl.cpp b/firmware/cl-step-controller/src/drivers/step_cl.cpp
index 1424b55..3fd9979 100644
--- a/firmware/cl-step-controller/src/drivers/step_cl.cpp
+++ b/firmware/cl-step-controller/src/drivers/step_cl.cpp
@@ -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
diff --git a/log/cl-step-control-log.md b/log/cl-step-control-log.md
index f865fcc..a5c2b52 100644
--- a/log/cl-step-control-log.md
+++ b/log/cl-step-control-log.md
@@ -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
--
GitLab