Trikarus/firmware_smartstepper_trikarus/stepper_nano_zero/calibration.cpp

600 lines
14 KiB
C++
Raw Normal View History

2021-08-08 14:43:38 +02:00
/**********************************************************************
Copyright (C) 2018 MisfitTech LLC, All rights reserved.
MisfitTech uses a dual license model that allows the software to be used under
a standard GPL open source license, or a commercial license. The standard GPL
license requires that all software statically linked with MisfitTec Code is
also distributed under the same GPL V2 license terms. Details of both license
options follow:
- Open source licensing -
MisfitTech is a free download and may be used, modified, evaluated and
distributed without charge provided the user adheres to version two of the GNU
General Public License (GPL) and does not remove the copyright notice or this
text. The GPL V2 text is available on the gnu.org web site
- Commercial licensing -
Businesses and individuals that for commercial or other reasons cannot comply
with the terms of the GPL V2 license must obtain a low cost commercial license
before incorporating MisfitTech code into proprietary software for distribution in
any form. Commercial licenses can be purchased from www.misfittech.net
and do not require any source files to be changed.
This code is distributed in the hope that it will be useful. You cannot
use MisfitTech's code unless you agree that you use the software 'as is'.
MisfitTech's code is provided WITHOUT ANY WARRANTY; without even the implied
warranties of NON-INFRINGEMENT, MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE. MisfitTech LLC disclaims all conditions and terms, be they
implied, expressed, or statutory.
Written by Trampas Stern for MisfitTech.
Misfit Tech invests time and resources providing this open source code,
please support MisfitTech and open-source hardware by purchasing
products from MisfitTech, www.misifittech.net!
*********************************************************************/
#include "calibration.h"
#include "Flash.h"
#include "nonvolatile.h"
#include "board.h" //for divide with rounding macro
#include "utils.h"
static uint16_t getTableIndex(uint16_t value)
{
int32_t x;
x=((int32_t)value*CALIBRATION_TABLE_SIZE)/CALIBRATION_STEPS; //the divide is a floor not a round which is what we want
return (uint16_t)x;
}
static uint16_t interp(Angle x1, Angle y1, Angle x2, Angle y2, Angle x)
{
int32_t dx,dy,dx2,y;
dx=x2-x1;
dy=y2-y1;
dx2=x-x1;
y=(int32_t)y1+DIVIDE_WITH_ROUND((dx2*dy),dx);
if (y<0)
{
y=y+CALIBRATION_STEPS;
}
return (uint16_t)y;
}
static void printData(int32_t *data, int32_t n)
{
int32_t i;
Serial.print("\n\r");
for (i=0; i<n; i++)
{
Serial.print(data[i]);
if (i!=(n-1))
{
Serial.print(",");
}
}
Serial.print("\n\r");
}
bool CalibrationTable::updateTableValue(int32_t index, int32_t value)
{
table[index].value=value;
table[index].error=CALIBRATION_STEPS/CALIBRATION_TABLE_SIZE; //or error is roughly like variance, so set it to span to next calibration value.
return true;
}
void CalibrationTable::printCalTable(void)
{
int i;
Serial.print("\n\r");
for (i=0; i<CALIBRATION_TABLE_SIZE; i++)
{
Serial.print((uint16_t)table[i].value);
Serial.print(",");
}
Serial.print("\n\r");
}
Angle CalibrationTable::fastReverseLookup(Angle encoderAngle)
{
#ifdef NZS_FAST_CAL
//assume calibration is good
if (fastCalVaild)
{
uint16_t x;
x=((uint16_t)encoderAngle)/4; //we only have 16384 values in table
return (Angle)NVM->FastCal.angle[x];
}else
{
return reverseLookup(encoderAngle);
}
#else
return reverseLookup(encoderAngle)
#endif
}
Angle CalibrationTable::reverseLookup(Angle encoderAngle)
{
int32_t i=0;
int32_t a1,a2;
int32_t x;
int16_t y;
int32_t min,max;
min=(uint16_t)table[0].value;
max=min;
for (i=0; i<CALIBRATION_TABLE_SIZE; i++)
{
x=(uint16_t)table[i].value;
if (x<min)
{
min=x;
}
if (x>max)
{
max=x;
}
}
x=(uint16_t)encoderAngle;
if (x<min)
{
x=x+CALIBRATION_STEPS;
}
i=0;
while (i<CALIBRATION_TABLE_SIZE)
{
a1=(uint16_t)table[i].value;
//handle index wrap around
if (i==(CALIBRATION_TABLE_SIZE-1))
{
a2=(uint16_t)table[0].value;
}else
{
a2=(uint16_t)table[i+1].value;
}
//wrap
if (abs(a1-a2)>CALIBRATION_STEPS/2)
{
if (a1<a2)
{
a1=a1+CALIBRATION_STEPS;
}else
{
a2=a2+CALIBRATION_STEPS;
}
//LOG("xxxx %d %d %d",a1,a2,x);
}
//finding matching location
if ( (x>=a1 && x<=a2) ||
(x>=a2 && x<=a1) )
{
//LOG("%d", i);
// inerpolate results and return
//LOG("%d %d %d",a1,a2,x);
//LOG("%d,%d",(i*CALIBRATION_MAX)/CALIBRATION_TABLE_SIZE,((i+2)*CALIBRATION_MAX)/CALIBRATION_TABLE_SIZE);
y=interp(a1, DIVIDE_WITH_ROUND((i*CALIBRATION_STEPS),CALIBRATION_TABLE_SIZE), a2, DIVIDE_WITH_ROUND( ((i+1)*CALIBRATION_STEPS),CALIBRATION_TABLE_SIZE), x);
return y;
}
i++;
}
ERROR("WE did some thing wrong");
}
void CalibrationTable::smoothTable(void)
{
uint16_t b[]={1,2,4,5,4,2,1};
uint16_t sum_b=19; //sum of b filter
int32_t data[CALIBRATION_TABLE_SIZE];
int32_t table2[CALIBRATION_TABLE_SIZE];
int32_t i;
int32_t offset=0;
int32_t startNum;
//first lets handle the wrap around in the table
for (i=0; i<CALIBRATION_TABLE_SIZE; i++)
{
if (i>0 && offset==0)
{
if(((uint16_t)table[i-1].value-(uint16_t)table[i].value) <-32768)
{
offset=-65536;
}
if (((uint16_t)table[i-1].value-(uint16_t)table[i].value) > 32768)
{
offset=65536;
}
}
table2[i]=(int32_t)((uint16_t)table[i].value)+offset;
}
//Serial.print("after wrap\n\r");
//printData(table2,CALIBRATION_TABLE_SIZE);
//remove the starting offset and compensate table for index
startNum=table2[0];
for (i=0; i<CALIBRATION_TABLE_SIZE; i++)
{
table2[i]=table2[i]-startNum - (i*65536)/CALIBRATION_TABLE_SIZE;
}
//Serial.print("after phase comp\n\r");
//printData(table2,CALIBRATION_TABLE_SIZE);
//filter the data
for (i=0; i<CALIBRATION_TABLE_SIZE; i++)
{
int j,ix,ib;;
int32_t sum=0;
ib=0;
for (j=i-3; j<i+4; j++)
{
ix=j;
if (ix<0)
{
ix=ix+CALIBRATION_TABLE_SIZE;
}
if (ix>=CALIBRATION_TABLE_SIZE)
{
ix=ix-CALIBRATION_TABLE_SIZE;
}
if (i==0)
{
LOG("index %d",ix);
}
sum=sum+table2[ix]*b[ib];
ib++;
}
sum=DIVIDE_WITH_ROUND(sum,sum_b);
data[i]=sum;
}
//Serial.print("after filter\n\r");
//printData(data,CALIBRATION_TABLE_SIZE);
//add in offset and the phase compenstation
for (i=0; i<CALIBRATION_TABLE_SIZE; i++)
{
data[i]=data[i]+startNum + (i*65536)/CALIBRATION_TABLE_SIZE;
}
//Serial.print("after phase comp added\n\r");
//printData(data,CALIBRATION_TABLE_SIZE);
//remove the uint16_t wrap
for (i=0; i<CALIBRATION_TABLE_SIZE; i++)
{
if (data[i]>=65536)
{
data[i]=data[i]-65536;
}
}
//Serial.print("after wrap added\n\r");
//printData(data,CALIBRATION_TABLE_SIZE);
//save new table
for (i=0; i<CALIBRATION_TABLE_SIZE; i++)
{
table[i].value=data[i];
}
}
void CalibrationTable::saveToFlash(void)
{
FlashCalData_t data;
int i;
for (i=0; i<CALIBRATION_TABLE_SIZE; i++ )
{
data.table[i]=(uint16_t)table[i].value;
}
data.status=true;
LOG("Writting Calbiration to Flash");
nvmWriteCalTable(&data,sizeof(data));
memset(&data,0,sizeof(data));
memcpy(&data, &NVM->CalibrationTable,sizeof(data));
createFastCal();
LOG("after writting status is %d",data.status);
loadFromFlash();
}
void CalibrationTable::loadFromFlash(void)
{
FlashCalData_t data;
int i;
LOG("Reading Calbiration to Flash");
memcpy(&data, &NVM->CalibrationTable,sizeof(data));
for (i=0; i<CALIBRATION_TABLE_SIZE; i++ )
{
table[i].value=Angle(data.table[i]);
table[i].error=CALIBRATION_MIN_ERROR;
}
data.status=true;
}
bool CalibrationTable::flashGood(void)
{
LOG("calibration table status is: %d",NVM->CalibrationTable.status);
return NVM->CalibrationTable.status;
}
void CalibrationTable::createFastCal(void)
{
#ifdef NZS_FAST_CAL
int32_t i;
uint16_t cs=0;
uint16_t data[256];
int32_t j;
j=0;
cs=0;
LOG("setting fast calibration");
for (i=0; i<16384; i++)
{
uint16_t x;
x=reverseLookup(i*4);
data[j]=x;
j++;
if (j>=256)
{
flashWrite(&NVM->FastCal.angle[i-255],data,256*sizeof(uint16_t));
//LOG("Wrote fastcal at index %d-%d", i-255, i);
j=0;
}
cs+=x;
}
//update the checksum
flashWrite(&NVM->FastCal.checkSum,&cs,sizeof(uint16_t));
fastCalVaild=true;
//this is a quick test
/*
for (i=0; i<16384; i++)
{
LOG("fast Cal %d,%d,%d",i,NVM->FastCal.angle[i],(uint32_t)reverseLookup(i*4));
}
*/
#endif
}
void CalibrationTable::updateFastCal(void)
{
#ifdef NZS_FAST_CAL
int32_t i;
uint16_t cs=0;
uint16_t data[256];
int32_t j;
bool NonZero=false;
for (i=0; i<16384; i++)
{
cs+=NVM->FastCal.angle[i];
if (cs != 0)
{
NonZero=true;
}
}
if (cs!=NVM->FastCal.checkSum || NonZero==false)
{
createFastCal();
}
else
{
LOG("fast cal is valid");
fastCalVaild=true;
}
#endif
}
void CalibrationTable::init(void)
{
int i;
if (true == flashGood())
{
loadFromFlash();
updateFastCal();
}else
{
for (i=0; i<CALIBRATION_TABLE_SIZE; i++)
{
table[i].value=0;
table[i].error=CALIBRATION_ERROR_NOT_SET;
}
}
return;
}
#if 0
//This code was removed because with micro stepping we can not assume
// our actualAngle is correct.
void CalibrationTable::updateTable(Angle actualAngle, Angle encoderValue);
{
static int32_t lastAngle=-1;
static uint16_t lastEncoderValue=0;
if (last != -1)
{
int32_t dist;
//hopefull we can use the current point and last point to interpolate and set a value or two in table.
dist=abs((int32_t)actualAngle-(int32_t)lastAngle); //distance between the two angles
//since our angles wrap the shortest distance will be one less than 32768
if (dist>CALIBRATION_STEPS/2)
{
dist=dist-CALIBRATION_STEPS;
}
//if our distance is larger than size between calibration points in table we will ignore this sample
if (dist>CALIBRATION_STEPS/CALIBRATION_TABLE_SIZE)
{
//spans two or more table calibration points for this implementation we will not use
lastIndex=(int32_t)index;
lastValue=value;
return;
}
//now lets see if the values are above and below a table calibration point
dist= abs(getTableIndex(lastAngle)-getTableIndex(actualAngle));
if (dist != 0) //if the two indexs into table are not the same it spans a calibration point in table.
{
//the two span a set calibation table point.
uint16_t newValue;
newValue=interp(lastAngle, lastEncoderValue, actualAngle, encoderValue, getTableIndex(actualAngle)*(CALIBRATION_STEPS/CALIBRATION_TABLE_SIZE))
//this new value is our best guess as to the correct calibration value.
updateTableValue(getTableIndex(actualAngle),newValue);
} else
{
//we should calibate the table value for the point the closest
}
}
lastAngle=(int32_t)actualAngle;
lastEncoderValue=encoderValue;
}
#endif
//when we are microstepping and are in between steps the probability the stepper motor did not move
// is high. That is the actualAngle will be correct but the encoderValue will be behind due to not having enough torque to move motor.
// Therefore we only want to update the calibration on whole steps where we have highest probability of things being correct.
void CalibrationTable::updateTable(Angle actualAngle, Angle encoderValue)
{
int32_t dist, index;
Angle tableAngle;
index = getTableIndex((uint32_t)actualAngle+CALIBRATION_STEPS/CALIBRATION_TABLE_SIZE/2); //add half of distance to next entry to round to closest table index
tableAngle=(index*CALIBRATION_STEPS)/CALIBRATION_TABLE_SIZE; //calculate the angle for this index
dist=tableAngle-actualAngle; //distance to calibration table angle
//LOG("Dist is %d",dist);
if (abs(dist)<CALIBRATION_MIN_ERROR) //if we are with in our minimal error we can calibrate
{
updateTableValue(index,(int32_t)encoderValue);
}
}
bool CalibrationTable::calValid(void)
{
uint32_t i;
for (i=0; i<CALIBRATION_TABLE_SIZE; i++)
{
if (table[i].error == CALIBRATION_ERROR_NOT_SET)
{
return false;
}
}
if (false == flashGood())
{
saveToFlash();
}
return true;
}
//We want to linearly interpolate between calibration table angle
int CalibrationTable::getValue(Angle actualAngle, CalData_t *ptrData)
{
int32_t indexLow,indexHigh;
int32_t angleLow,angleHigh;
uint16_t value;
int32_t y1,y2;
int16_t err;
indexLow=getTableIndex((uint16_t)actualAngle);
// LOG("index %d, actual %u",indexLow, (uint16_t)actualAngle);
indexHigh=indexLow+1;
angleLow=(indexLow*CALIBRATION_STEPS)/CALIBRATION_TABLE_SIZE;
angleHigh=(indexHigh*CALIBRATION_STEPS)/CALIBRATION_TABLE_SIZE;
if (indexHigh>=CALIBRATION_TABLE_SIZE)
{
indexHigh -= CALIBRATION_TABLE_SIZE;
}
//LOG("AngleLow %d, AngleHigh %d",angleLow,angleHigh);
//LOG("TableLow %u, TableHigh %d",(uint16_t)table[indexLow].value,(uint16_t)table[indexHigh].value);
y1=table[indexLow].value;
y2=table[indexHigh].value;
//handle the wrap condition
if (abs(y2-y1)>CALIBRATION_STEPS/2)
{
if (y2<y1)
{
y2=y2+CALIBRATION_STEPS;
}else
{
y1=y1+CALIBRATION_STEPS;
}
}
value=interp(angleLow, y1, angleHigh, y2,actualAngle);
//handle the wrap condition
if (value>=CALIBRATION_STEPS)
{
value=value-CALIBRATION_STEPS;
}
err=table[indexLow].error;
if (table[indexHigh].error > err)
{
err=table[indexHigh].error;
}
if (table[indexLow].error == CALIBRATION_ERROR_NOT_SET ||
table[indexHigh].error == CALIBRATION_ERROR_NOT_SET)
{
err=CALIBRATION_ERROR_NOT_SET;
}
ptrData->value=value;
ptrData->error=err;
return 0;
}
Angle CalibrationTable::getCal(Angle actualAngle)
{
CalData_t data;
getValue(actualAngle, &data);
return data.value;
}