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

calib begins, defeat circular average problem

parent 35fe8f8a
{
"files.associations": {
"cmath": "cpp"
}
}
\ No newline at end of file
......@@ -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;
}
......
......@@ -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);
......
......@@ -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
......@@ -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 ??
......
......@@ -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
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