From c058fd272bf3a4abe47027c6a7e99fd1d986a03d Mon Sep 17 00:00:00 2001
From: Jake Read <jake.read@cba.mit.edu>
Date: Thu, 8 Oct 2020 17:32:48 -0400
Subject: [PATCH] calib begins, defeat circular average problem
---
.../cl-step-controller/.vscode/settings.json | 5 +
.../src/drivers/enc_as5047.cpp | 2 +-
.../src/drivers/step_a4950.cpp | 1 +
.../src/drivers/step_cl.cpp | 127 +++++++++++--
.../cl-step-controller/src/drivers/step_cl.h | 3 +-
log/cl-step-control-log.md | 178 +++++++++++++++++-
6 files changed, 296 insertions(+), 20 deletions(-)
create mode 100644 firmware/cl-step-controller/.vscode/settings.json
diff --git a/firmware/cl-step-controller/.vscode/settings.json b/firmware/cl-step-controller/.vscode/settings.json
new file mode 100644
index 0000000..1be854f
--- /dev/null
+++ b/firmware/cl-step-controller/.vscode/settings.json
@@ -0,0 +1,5 @@
+{
+ "files.associations": {
+ "cmath": "cpp"
+ }
+}
\ No newline at end of file
diff --git a/firmware/cl-step-controller/src/drivers/enc_as5047.cpp b/firmware/cl-step-controller/src/drivers/enc_as5047.cpp
index 0e52198..ad377c8 100644
--- a/firmware/cl-step-controller/src/drivers/enc_as5047.cpp
+++ b/firmware/cl-step-controller/src/drivers/enc_as5047.cpp
@@ -138,7 +138,7 @@ void ENC_AS5047::rxcISR(void){
inWord01 = data;
} else {
inWord02 = data;
- result = (inWord01 << 8) | inWord02;
+ result = 0b0011111111111111 & ((inWord01 << 8) | inWord02);
on_read_complete(result);
readComplete = true;
}
diff --git a/firmware/cl-step-controller/src/drivers/step_a4950.cpp b/firmware/cl-step-controller/src/drivers/step_a4950.cpp
index 8ab345e..421601f 100644
--- a/firmware/cl-step-controller/src/drivers/step_a4950.cpp
+++ b/firmware/cl-step-controller/src/drivers/step_a4950.cpp
@@ -161,6 +161,7 @@ void STEP_A4950::writePhases(void){
// magnetic angle 0-1 maps 0-2PI phase
// magnitude 0-1 of <range> board's max amp output
+// one complete magnetic period is four 'steps'
void STEP_A4950::point(float magangle, float magnitude){
// guard out of range angles & magnitudes
clamp(&magangle, 0.0F, 1.0F);
diff --git a/firmware/cl-step-controller/src/drivers/step_cl.cpp b/firmware/cl-step-controller/src/drivers/step_cl.cpp
index c5ced72..1424b55 100644
--- a/firmware/cl-step-controller/src/drivers/step_cl.cpp
+++ b/firmware/cl-step-controller/src/drivers/step_cl.cpp
@@ -33,7 +33,10 @@ Step_CL* step_cl = Step_CL::getInstance();
Step_CL::Step_CL(void){}
#define CALIB_CSCALE 0.4F
-#define CALIB_STEP_DELAY 5
+#define CALIB_STEP_DELAY 10
+#define CALIB_SAMPLE_PER_TICK 10
+
+#define ENCODER_COUNTS 16384
void Step_CL::init(void){
stepper_hw->init(false, 0.4);
@@ -42,22 +45,112 @@ void Step_CL::init(void){
//lut = flash_lut.read();
}
-void Step_CL::calibrate(void){
- // OK, first order is getting a step machine where I can point at
- // phase angles w/ current scalars on that angle
- // then I use that to step thru 0->90->180->270->0 for steps
- // then first order is just checking that if I take 200 (50) steps
- // thru that, I get a full rotation
-
- // ok, at the moment, should do 200 counts (full rev) at 200*256
- for(uint32_t i = 0; i < 50; i ++){
- stepper_hw->point(0.0F, 0.4F);
- delay(CALIB_STEP_DELAY);
- stepper_hw->point(0.25F, 0.4F);
- delay(CALIB_STEP_DELAY);
- stepper_hw->point(0.5F, 0.4F);
- delay(CALIB_STEP_DELAY);
- stepper_hw->point(0.75F, 0.4F);
+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(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;
+ }
+ // debug print intervals
+ if(false){
+ 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);
+ }
}
+ // 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));
+
+ // 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
+ 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;
+ break;
+ }
+ }
+ 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];
+ 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;
+ // log those
+ sysError("ra: " + String(ra, 4));
+ delay(1);
+ } // end sweep thru 2^14 pts
+ return true; // went OK
}
\ No newline at end of file
diff --git a/firmware/cl-step-controller/src/drivers/step_cl.h b/firmware/cl-step-controller/src/drivers/step_cl.h
index da70bc3..89979ff 100644
--- a/firmware/cl-step-controller/src/drivers/step_cl.h
+++ b/firmware/cl-step-controller/src/drivers/step_cl.h
@@ -25,12 +25,13 @@ is; no warranty is provided, and users accept all liability.
class Step_CL {
private:
static Step_CL* instance;
+ float calib_readings[200];
public:
Step_CL();
static Step_CL* getInstance(void);
void init(void);
- void calibrate(void);
+ boolean calibrate(void);
//float __attribute__((__aligned__(256))) lut[16384]; // nor does this !
//float lut[16384]; // nor does this work
//step_cl_calib_table_t lut; // not even this works ?? too big ??
diff --git a/log/cl-step-control-log.md b/log/cl-step-control-log.md
index b9afc3a..f865fcc 100644
--- a/log/cl-step-control-log.md
+++ b/log/cl-step-control-log.md
@@ -340,4 +340,180 @@ I'm just going to get into the 'magnetic angle' pointing system for the DACs now
This makes sense: so when I'm at '0 degs' my A phase is on 100%, B phase is zero. Or, the way I've my LUT written, I'll have A at 0 and B at full-width positive. If I don't like this for some reason (It'll calibrate away) I can change the LUT.
-I'm up to pointing, now I just need to deliver some power to the motor.
\ No newline at end of file
+I'm up to pointing, now I just need to deliver some power to the motor.
+
+OK, this is making sense and I can point my magnetic vector around:
+
+```cpp
+// do _aStep and _bStep integers, op electronics
+void STEP_A4950::writePhases(void){
+ // a phase,
+ if(LUT_8190[_aStep] > 4095){
+ A_UP;
+ } else if (LUT_8190[_aStep] < 4095){
+ A_DOWN;
+ } else {
+ A_OFF;
+ }
+ // a DAC
+ dacs->writeDac0(dacLUT[_aStep] * _cscale);
+
+ // b phase,
+ if(LUT_8190[_bStep] > 4095){
+ B_UP;
+ } else if (LUT_8190[_bStep] < 4095){
+ B_DOWN;
+ } else {
+ B_OFF;
+ }
+ // b DAC
+ dacs->writeDac1(dacLUT[_bStep] * _cscale);
+}
+
+// magnetic angle 0-1 maps 0-2PI phase
+// magnitude 0-1 of <range> board's max amp output
+// one complete magnetic period is four 'steps'
+void STEP_A4950::point(float magangle, float magnitude){
+ // guard out of range angles & magnitudes
+ clamp(&magangle, 0.0F, 1.0F);
+ clamp(&magnitude, 0.0F, 1.0F);
+ uint16_t magint = (uint16_t)(magangle * (float)LUT_LENGTH);
+ _aStep = magint; // a phase just right on this thing,
+ // lut_length / 4 = lut_length >> 2 (rotates phase 90 degs)
+ // x modulo y = (x & (y − 1)) (wraps phase around end of lut)
+ _bStep = (magint + ((uint16_t)LUT_LENGTH >> 2)) & (LUT_LENGTH - 1);
+ // upd8 the cscale
+ _cscale = magnitude;
+ // now we should be able to...
+ writePhases();
+}
+```
+
+One magnetic period is four full 'steps', so my LUT is 1024 positions long and I am effectively 'microstepping' 256 times (or have this amount of resolution).
+
+Now I need to write the table, so I'll take 200 samples at each full step, maybe averaging encoder readings 10x.
+
+OK, I've those samples, so I guess the next move is just to write my table. I think I want to check that all of the readings are continuous (pointing in the same direction) but this also seems a bit tricky: one will be reversed as well, for sure.
+
+Then I'll get a table that has one jump around the encoder origin. I think I want to start by rewriting the readings table to base zero around the origin, well, obviously since this is a LUT. Finding the actual 'zero' is a bit of a trick though, and requires the interpolation routine to set the first 'mag angle' at that point.
+
+Maybe this is less complicated... for each value 0-16k, I find the interval it's between in 0-199 indices from the scan. One of these is a bit tricky, otherwise it's whatever. I do the linterp and write down a float.
+
+Here's the calib so far,
+
+```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 reading = 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
+ reading += (float)(enc_as5047->get_reading());
+ // this is odd, I know, but it allows a new measurement to settle
+ // so we get a real average
+ delay(1);
+ }
+ // push reading
+ calib_readings[i] = reading / (float)CALIB_SAMPLE_PER_TICK;
+ // rotate
+ phase_angle += 0.25F;
+ if(phase_angle >= 1.0F) phase_angle = 0.0F;
+ }
+ // report readings
+ if(false){
+ for(uint8_t r = 0; r < 200; r ++){
+ sysError(String(calib_readings[r]));
+ delay(10);
+ }
+ }
+ // 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));
+
+ // 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
+ 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 {
+ sysError("bad interval at: " + String(e));
+ 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];
+ 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;
+ // log those
+ sysError("ra: " + String(ra, 4));
+ delay(1);
+ } // end sweep thru 2^14 pts
+ return true; // went OK
+}
+```
+
+This works inside of 'normal' intervals, but fails around the origin due to that wrap issue. I should detect this case...
+
+OK, new bug: when I average readings that are around the origin, I have samples like i.e.
+
+```
+16383
+1
+16384
+2
+16382
+1
+```
+
+etc, so the average of these needs to wrap as well, ya duh, tough to do with the wrap tho yeah. I can just add the total counts to each measurement and then sub that value from the total average... or from the sum.
+
+I have this problem: https://en.wikipedia.org/wiki/Mean_of_circular_quantities
+
+The maths-ey way to do this is to convert to cartesian coordinates, given the theta, average in that space and then return to r coords.
+
+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
--
GitLab