Shao Rui
1
Dependencies: mbed-dev-f303 FastPWM3
Calibration/calibration.cpp
- Committer:
- shaorui
- Date:
- 2020-02-07
- Revision:
- 48:1b51771c3647
- Parent:
- 45:aadebe074af6
- Child:
- 49:7eac11914980
File content as of revision 48:1b51771c3647:
/// Calibration procedures for determining position sensor offset,
/// phase ordering, and position sensor linearization
///
#include "calibration.h"
#include "foc.h"
#include "PreferenceWriter.h"
#include "user_config.h"
#include "motor_config.h"
#include "current_controller_config.h"
void order_phases(PositionSensor *ps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs){
///Checks phase order, to ensure that positive Q current produces
///torque in the positive direction wrt the position sensor.
printf("\n\r Checking phase ordering\n\r");
float theta_ref = 0;
float theta_actual = 0;
//float v_d = .15f;
float v_d = .23f; //Put all volts on the D-Axis
float v_q = 0.0f;
float v_u, v_v, v_w = 0;
float dtc_u, dtc_v, dtc_w = .5f;
int sample_counter = 0;
///Set voltage angle to zero, wait for rotor position to settle
abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); //inverse dq0 transform on voltages
svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); //space vector modulation
for(int i = 0; i<20000; i++){
TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u); // Set duty cycles
TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);
TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
wait_us(100);
}
//ps->ZeroPosition();
ps->Sample(DT);
wait_us(1000);
//float theta_start = ps->GetMechPositionFixed(); //get initial rotor position
float theta_start;
controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset); //Calculate phase currents from ADC readings
controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
controller->i_a = -controller->i_b - controller->i_c;
dq0(controller->theta_elec, controller->i_a, controller->i_b, controller->i_c, &controller->i_d, &controller->i_q); //dq0 transform on currents
float current = sqrt(pow(controller->i_d, 2) + pow(controller->i_q, 2));
printf("\n\rCurrent\n\r");
printf("%f %f %f\n\r\n\r", controller->i_d, controller->i_q, current);
/// Rotate voltage angle
while(theta_ref < 4*PI){ //rotate for 2 electrical cycles
abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); //inverse dq0 transform on voltages
svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); //space vector modulation
wait_us(100);
TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u); //Set duty cycles
TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);
TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
ps->Sample(DT); //sample position sensor
theta_actual = ps->GetMechPositionFixed();
if(theta_ref==0){theta_start = theta_actual;}
if(sample_counter > 200){
sample_counter = 0 ;
printf("%.4f %.4f\n\r", theta_ref/(NPP), theta_actual);
}
sample_counter++;
theta_ref += 0.001f;
}
float theta_end = ps->GetMechPositionFixed();
int direction = (theta_end - theta_start)>0;
printf("Theta Start: %f Theta End: %f\n\r", theta_start, theta_end);
printf("Direction: %d\n\r", direction);
if(direction){printf("Phasing correct\n\r");}
else if(!direction){printf("Phasing incorrect. Swapping phases V and W\n\r");}
PHASE_ORDER = direction;
}
void calibrate(PositionSensor *ps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs){
/// Measures the electrical angle offset of the position sensor
/// and (in the future) corrects nonlinearity due to position sensor eccentricity
printf("Starting calibration procedure\n\r");
const int n = 128*NPP; // number of positions to be sampled per mechanical rotation. Multiple of NPP for filtering reasons (see later)
const int n2 = 50; // increments between saved samples (for smoothing motion)
float delta = 2*PI*NPP/(n*n2); // change in angle between samples
float error_f[n] = {0}; // error vector rotating forwards
float error_b[n] = {0}; // error vector rotating backwards
const int n_lut = 128;
int lut[n_lut]= {0}; // clear any old lookup table before starting.
ps->WriteLUT(lut);
int raw_f[n] = {0};
int raw_b[n] = {0};
float theta_ref = 0;
float theta_actual = 0;
//float v_d = .15f;
float v_d = .2f; // Put volts on the D-Axis
float v_q = 0.0f;
float v_u, v_v, v_w = 0;
float dtc_u, dtc_v, dtc_w = .5f;
///Set voltage angle to zero, wait for rotor position to settle
abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages
svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation
for(int i = 0; i<40000; i++){
TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u); // Set duty cycles
if(PHASE_ORDER){
TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);
TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
}
else{
TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);
TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
}
wait_us(100);
}
ps->Sample(DT);
controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset); //Calculate phase currents from ADC readings
controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
controller->i_a = -controller->i_b - controller->i_c;
dq0(controller->theta_elec, controller->i_a, controller->i_b, controller->i_c, &controller->i_d, &controller->i_q); //dq0 transform on currents
float current = sqrt(pow(controller->i_d, 2) + pow(controller->i_q, 2));
printf(" Current Angle : Rotor Angle : Raw Encoder \n\r\n\r");
for(int i = 0; i<n; i++){ // rotate forwards
for(int j = 0; j<n2; j++){
theta_ref += delta;
abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages
svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation
TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u);
if(PHASE_ORDER){ // Check phase ordering
TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v); // Set duty cycles
TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
}
else{
TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);
TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
}
wait_us(100);
ps->Sample(DT);
}
ps->Sample(DT);
theta_actual = ps->GetMechPositionFixed();
error_f[i] = theta_ref/NPP - theta_actual;
raw_f[i] = ps->GetRawPosition();
printf("ref %.4f\n\r actual %.4f\n\r raw %d\n\r", theta_ref/(NPP), theta_actual, raw_f[i]);
//theta_ref += delta;
}
for(int i = 0; i<n; i++){ // rotate backwards
for(int j = 0; j<n2; j++){
theta_ref -= delta;
abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages
svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation
TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u);
if(PHASE_ORDER){
TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);
TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
}
else{
TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);
TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
}
wait_us(100);
ps->Sample(DT);
}
ps->Sample(DT); // sample position sensor
theta_actual = ps->GetMechPositionFixed(); // get mechanical position
error_b[i] = theta_ref/NPP - theta_actual;
raw_b[i] = ps->GetRawPosition();
printf("ref %.4f\n\r actual %.4f\n\r raw %d\n\r", theta_ref/(NPP), theta_actual, raw_b[i]);
//theta_ref -= delta;
}
float offset = 0;
for(int i = 0; i<n; i++){
offset += (error_f[i] + error_b[n-1-i])/(2.0f*n); // calclate average position sensor offset
}
offset = fmod(offset*NPP, 2*PI); // convert mechanical angle to electrical angle
ps->SetElecOffset(offset); // Set position sensor offset
__float_reg[0] = offset;
E_OFFSET = offset;
/// Perform filtering to linearize position sensor eccentricity
/// FIR n-sample average, where n = number of samples in one electrical cycle
/// This filter has zero gain at electrical frequency and all integer multiples
/// So cogging effects should be completely filtered out.
float error[n] = {0};
const int window = 128;
float error_filt[n] = {0};
float cogging_current[window] = {0};
float mean = 0;
for (int i = 0; i<n; i++){ //Average the forward and back directions
error[i] = 0.5f*(error_f[i] + error_b[n-i-1]);
}
for (int i = 0; i<n; i++){
for(int j = 0; j<window; j++){
int ind = -window/2 + j + i; // Indexes from -window/2 to + window/2
if(ind<0){
ind += n;} // Moving average wraps around
else if(ind > n-1) {
ind -= n;}
error_filt[i] += error[ind]/(float)window;
}
if(i<window){
cogging_current[i] = current*sinf((error[i] - error_filt[i])*NPP);
}
//printf("%.4f %4f %.4f %.4f\n\r", error[i], error_filt[i], error_f[i], error_b[i]);
mean += error_filt[i]/n;
}
int raw_offset = (raw_f[0] + raw_b[n-1])/2; //Insensitive to errors in this direction, so 2 points is plenty
printf("\n\r Encoder non-linearity compensation table\n\r");
printf(" Sample Number : Lookup Index : Lookup Value : Cogging Current Lookup\n\r\n\r");
for (int i = 0; i<n_lut; i++){ // build lookup table
int ind = (raw_offset>>7) + i;
if(ind > (n_lut-1)){
ind -= n_lut;
}
lut[ind] = (int) ((error_filt[i*NPP] - mean)*(float)(ps->GetCPR())/(2.0f*PI));
printf("%d %d %d %f\n\r", i, ind, lut[ind], cogging_current[i]);
wait(.001);
}
ps->WriteLUT(lut); // write lookup table to position sensor object
//memcpy(controller->cogging, cogging_current, sizeof(controller->cogging)); //compensation doesn't actually work yet....
memcpy(&ENCODER_LUT, lut, sizeof(lut)); // copy the lookup table to the flash array
printf("\n\rEncoder Electrical Offset (rad) %f\n\r", offset);
if (!prefs->ready()) prefs->open();
prefs->flush(); // write offset and lookup table to flash
prefs->close();
}
/*
void j_calibrate(PositionSensorMA700 *jps,PositionSensorAM5147 *ps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs)
{
/// Measures the electrical angle offset of the position sensor
/// and (in the future) corrects nonlinearity due to position sensor eccentricity
//printf("Starting calibration procedure\n\r");
printf("s\n\r");
const int n = 128*NPP; // number of positions to be sampled per mechanical rotation. Multiple of NPP for filtering reasons (see later)
const int n2 = 50; // increments between saved samples (for smoothing motion)
float delta = 2*PI*NPP/(n*n2); // change in angle between samples
float error_f[n] = {0}; // error vector rotating forwards
float error_b[n] = {0}; // error vector rotating backwards
float error_joint_f[n] = {0}; // error vector rotating forwards
float error_joint_b[n] = {0}; // error vector rotating backwards
const int n_lut = 128;
const int n_joint = 128;
int lut[n_lut]= {0}; // clear any old lookup table before starting.
int joint[n_joint]= {0}; // clear any old lookup table before starting.
ps->WriteLUT(lut);
jps->WriteLUT(joint);
int raw_f[n] = {0};
int raw_b[n] = {0};
int joint_raw_f[n] = {0};
int joint_raw_b[n] = {0};
float theta_ref = 0;
float theta_actual = 0;
float theta_joint_ref = 0;
float theta_joint_actual = 0;
//float v_d = .15f;
float v_d = .2f; // Put volts on the D-Axis
float v_q = 0.0f;
float v_u, v_v, v_w = 0;
float dtc_u, dtc_v, dtc_w = .5f;
///Set voltage angle to zero, wait for rotor position to settle
abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages
svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation
for(int i = 0; i<40000; i++){
TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u); // Set duty cycles
if(PHASE_ORDER){
TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);
TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
}
else{
TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);
TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
}
wait_us(100);
}
ps->Sample(DT);
controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset); //Calculate phase currents from ADC readings
controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
controller->i_a = -controller->i_b - controller->i_c;
dq0(controller->theta_elec, controller->i_a, controller->i_b, controller->i_c, &controller->i_d, &controller->i_q); //dq0 transform on currents
float current = sqrt(pow(controller->i_d, 2) + pow(controller->i_q, 2));
printf(" Current Angle : Rotor Angle : Raw Encoder \n\r\n\r");
for(int i = 0; i<n; i++){ // rotate forwards
for(int j = 0; j<n2; j++){
theta_ref += delta;
//theta_joint_ref=theta_joint_ref- delta-theta_ref/GR;
theta_joint_ref-=delta;
abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages
svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation
TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u);
if(PHASE_ORDER){ // Check phase ordering
TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v); // Set duty cycles
TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
}
else{
TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);
TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
}
wait_us(100);
ps->Sample(DT);
jps->Sample(DT);
}
ps->Sample(DT);
jps->Sample(DT);
theta_actual = ps->GetMechPositionFixed();
error_f[i] = theta_ref/NPP - theta_actual;
raw_f[i] = ps->GetRawPosition();
theta_joint_actual = jps->GetMechPositionFixed()+(1/GR)*theta_actual;
error_joint_f[i] = theta_joint_ref/NPP - theta_joint_actual;
joint_raw_f[i] = jps->GetRawPosition();
printf("%.4f %.4f %d-------%.4f %.4f %d\n\r", theta_ref/(NPP), theta_actual, raw_f[i],theta_joint_ref/(NPP), theta_joint_actual, joint_raw_f[i]);
//theta_ref += delta;
}
for(int i = 0; i<n; i++){ // rotate backwards
for(int j = 0; j<n2; j++){
theta_ref -= delta;
//theta_joint_ref=theta_joint_ref+delta+theta_ref/GR;
theta_joint_ref+=delta;
abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages
svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation
TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u);
if(PHASE_ORDER){
TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);
TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
}
else{
TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);
TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
}
wait_us(100);
ps->Sample(DT);
jps->Sample(DT);
}
ps->Sample(DT); // sample position sensor
jps->Sample(DT);
theta_actual = ps->GetMechPositionFixed(); // get mechanical position
error_b[i] = theta_ref/NPP - theta_actual;
raw_b[i] = ps->GetRawPosition();
theta_joint_actual = jps->GetMechPositionFixed()+(1/GR)*theta_actual;
error_joint_b[i] = theta_joint_ref/NPP - theta_joint_actual;
joint_raw_b[i] = jps->GetRawPosition()-raw_b[i]/GR;
//printf("%.4f %.4f %d\n\r", theta_ref/(NPP), theta_actual, raw_b[i]);
printf("%.4f %.4f %d-------%.4f %.4f %d\n\r", theta_ref/(NPP), theta_actual, raw_b[i],theta_joint_ref/(NPP), theta_joint_actual, joint_raw_b[i]);
//theta_ref -= delta;
}
float offset = 0;
float joint_offset=0;
for(int i = 0; i<n; i++){
offset += (error_f[i] + error_b[n-1-i])/(2.0f*n); // calclate average position sensor offset
joint_offset += (error_joint_f[i] + error_joint_b[n-1-i])/(2.0f*n);
}
offset = fmod(offset*NPP, 2*PI);
joint_offset = fmod( joint_offset*NPP, 2*PI); // convert mechanical angle to electrical angle
ps->SetElecOffset(offset); // Set position sensor offset
__float_reg[0] = offset;
E_OFFSET = offset;
jps->SetElecOffset(joint_offset); // Set position sensor offset
__float_reg[8] = joint_offset;
JOINT_E_OFFSET = joint_offset;
/// Perform filtering to linearize position sensor eccentricity
/// FIR n-sample average, where n = number of samples in one electrical cycle
/// This filter has zero gain at electrical frequency and all integer multiples
/// So cogging effects should be completely filtered out.
float error[n] = {0};
float joint_error[n]={0};
const int window = 128;
float error_filt[n] = {0};
float error_joint_filt[n] = {0};
float cogging_current[window] = {0};
float cogging_joint_current[window] = {0};
float mean = 0;
float joint_mean=0;
for (int i = 0; i<n; i++){ //Average the forward and back directions
error[i] = 0.5f*(error_f[i] + error_b[n-i-1]);
joint_error[i]= 0.5f*(error_joint_f[i] + error_joint_b[n-i-1]);
}
for (int i = 0; i<n; i++){
for(int j = 0; j<window; j++){
int ind = -window/2 + j + i; // Indexes from -window/2 to + window/2
if(ind<0){
ind += n;
} // Moving average wraps around
else if(ind > n-1) {
ind -= n;
}
error_filt[i] += error[ind]/(float)window;
error_joint_filt[i] += joint_error[ind]/(float)window;
}
if(i<window){
cogging_current[i] = current*sinf((error[i] - error_filt[i])*NPP);
cogging_joint_current[i] = current*sinf((joint_error[i] - error_joint_filt[i])*NPP);
}
//printf("%.4f %4f %.4f %.4f\n\r", error[i], error_filt[i], error_f[i], error_b[i]);
mean += error_filt[i]/n;
joint_mean += error_joint_filt[i]/n;
}
int raw_offset = (raw_f[0] + raw_b[n-1])/2; //Insensitive to errors in this direction, so 2 points is plenty
int joint_raw_offset =(joint_raw_f[0] + joint_raw_b[n-1])/2;
printf("\n\r Encoder non-linearity compensation table\n\r");
printf(" Sample Number : Lookup Index : Lookup Value : Cogging Current Lookup\n\r\n\r");
for (int i = 0; i<n_lut; i++){ // build lookup table
int ind = (raw_offset>>7) + i;
if(ind > (n_lut-1)){
ind -= n_lut;
}
lut[ind] = (int) ((error_filt[i*NPP] - mean)*(float)(ps->GetCPR())/(2.0f*PI));
printf("%d %d %d %f\n\r", i, ind, lut[ind], cogging_current[i]);
wait(.001);
}
for (int i = 0; i<n_joint; i++){ // build lookup table
int joint_ind = (joint_raw_offset>>7) + i;
if(joint_ind > (n_joint-1)){
joint_ind -= n_joint;
}
joint[joint_ind] = (int) ((error_joint_filt[i*NPP] - joint_mean)*(float)(jps->GetCPR())/(2.0f*PI));
printf("%d %d %d %f\n\r", i, joint_ind, joint[joint_ind], cogging_joint_current[i]);
wait(.001);
}
ps->WriteLUT(lut);
jps->WriteLUT(joint); // write lookup table to position sensor object
//memcpy(controller->cogging, cogging_current, sizeof(controller->cogging)); //compensation doesn't actually work yet....
memcpy(&ENCODER_LUT, lut, sizeof(lut)); // copy the lookup table to the flash array
memcpy(&ENCODER_JOINT, joint, sizeof(joint));
printf("\n\rEncoder Electrical Offset (rad) %f\n\r", offset);
printf("\n\rJoint Encoder Electrical Offset (rad) %f\n\r", joint_offset);
if (!prefs->ready()) prefs->open();
prefs->flush(); // write offset and lookup table to flash
prefs->close();
}
*/
void j_calibrate(PositionSensorMA700 *jps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs)
{
/// Measures the electrical angle offset of the position sensor
/// and (in the future) corrects nonlinearity due to position sensor eccentricity
printf("Starting joint calibration procedure\n\r");
const int n = 128*NPP; // number of positions to be sampled per mechanical rotation. Multiple of NPP for filtering reasons (see later)
const int n2 = 50; // increments between saved samples (for smoothing motion)
float delta = 2*PI*NPP/(n*n2); // change in angle between samples
float error_joint_f[n] = {0}; // error vector rotating forwards
float error_joint_b[n] = {0}; // error vector rotating backwards
const int n_joint = 128;
int joint[n_joint]= {0}; // clear any old lookup table before starting.
jps->WriteLUT(joint);
jps->ReadLUT();
int joint_raw_f[n] = {0};
int joint_raw_b[n] = {0};
float theta_ref = 0;
float theta_joint_ref = 0;
float theta_joint_actual = 0;
//float v_d = .15f;
float v_d = .2f; // Put volts on the D-Axis
float v_q = 0.0f;
float v_u, v_v, v_w = 0;
float dtc_u, dtc_v, dtc_w = .5f;
///Set voltage angle to zero, wait for rotor position to settle
abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages
svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation
for(int i = 0; i<40000; i++){
TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u); // Set duty cycles
if(PHASE_ORDER){
TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);
TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
}
else{
TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);
TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
}
wait_us(100);
}
jps->Sample(DT);
controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset); //Calculate phase currents from ADC readings
controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
controller->i_a = -controller->i_b - controller->i_c;
dq0(controller->theta_elec, controller->i_a, controller->i_b, controller->i_c, &controller->i_d, &controller->i_q); //dq0 transform on currents
float current = sqrt(pow(controller->i_d, 2) + pow(controller->i_q, 2));
printf(" Current Angle : Rotor Angle : Raw Encoder \n\r\n\r");
for(int i = 0; i<n; i++){ // rotate forwards
for(int j = 0; j<n2; j++){
theta_ref += delta;
theta_joint_ref-=delta;
// theta_joint_ref+=delta;
abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages
svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation
TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u);
if(PHASE_ORDER){ // Check phase ordering
TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v); // Set duty cycles
TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
}
else{
TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);
TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
}
wait_us(100);
jps->Sample(DT);
}
jps->Sample(DT);
theta_joint_actual = jps->GetMechPositionFixed();
//error_joint_f[i] = theta_joint_ref*(1+1/GR)/NPP -theta_joint_actual;
error_joint_f[i] = (theta_joint_ref-(1/GR)*theta_ref)/NPP -theta_joint_actual;
joint_raw_f[i] = jps->GetRawPosition();
//printf("%.4f %.4f %d\n\r", theta_joint_ref*(1+1/GR)/(NPP), theta_joint_actual, joint_raw_f[i]);
printf("ref %.4f\n\r actual %.4f\n\r raw %d\n\r", (theta_joint_ref-(1/GR)*theta_ref)/NPP, theta_joint_actual, joint_raw_f[i]);
//theta_ref += delta;
}
for(int i = 0; i<n; i++){ // rotate backwards
for(int j = 0; j<n2; j++){
theta_ref -= delta;
//theta_joint_ref-=delta;
theta_joint_ref+=delta;
abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages
svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation
TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u);
if(PHASE_ORDER){
TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);
TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
}
else{
TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);
TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
}
wait_us(100);
jps->Sample(DT);
}
jps->Sample(DT);
theta_joint_actual = jps->GetMechPositionFixed();
//error_joint_b[i] = theta_joint_ref*(1+1/GR)/NPP - theta_joint_actual;
error_joint_b[i] = (theta_joint_ref-(1/GR)*theta_ref)/NPP -theta_joint_actual;
joint_raw_b[i] = jps->GetRawPosition();//-raw_b[i]/GR;
//printf("%.4f %.4f %d-\n\r", theta_ref*(1+1/GR)/(NPP), theta_joint_actual, joint_raw_b[i]);
printf("ref %.4f\n\r actual %.4f\n\r raw %d\n\r", (theta_joint_ref-(1/GR)*theta_ref)/NPP, theta_joint_actual, joint_raw_b[i]);
//theta_ref -= delta;
}
float joint_offset=0;
for(int i = 0; i<n; i++){
joint_offset += (error_joint_f[i] + error_joint_b[n-1-i])/(2.0f*n); // calclate average position sensor joint offset
}
joint_offset = fmod( joint_offset*NPP, 2*PI); // convert mechanical angle to electrical angle
jps->SetElecOffset(joint_offset); // Set position joint sensor offset
__float_reg[8] = joint_offset;
JOINT_E_OFFSET = joint_offset;
/// Perform filtering to linearize position sensor eccentricity
/// FIR n-sample average, where n = number of samples in one electrical cycle
/// This filter has zero gain at electrical frequency and all integer multiples
/// So cogging effects should be completely filtered out.
float joint_error[n]={0};
const int window = 128;
float error_joint_filt[n] = {0};
float cogging_joint_current[window] = {0};
float joint_mean=0;
for (int i = 0; i<n; i++){ //Average the forward and back directions
joint_error[i]= 0.5f*(error_joint_f[i] + error_joint_b[n-i-1]);
}
for (int i = 0; i<n; i++){
for(int j = 0; j<window; j++){
int ind = -window/2 + j + i; // Indexes from -window/2 to + window/2
if(ind<0){
ind += n;
} // Moving average wraps around
else if(ind > n-1) {
ind -= n;
}
error_joint_filt[i] += joint_error[ind]/(float)window;
}
if(i<window){
cogging_joint_current[i] = current*sinf((joint_error[i] - error_joint_filt[i])*NPP);
}
//printf("%.4f %4f %.4f %.4f\n\r", error[i], error_filt[i], error_f[i], error_b[i]);
joint_mean += error_joint_filt[i]/n;
}
int joint_raw_offset =(joint_raw_f[0] + joint_raw_b[n-1])/2; //Insensitive to errors in this direction, so 2 points is plenty
printf("\n\r Encoder non-linearity compensation table\n\r");
printf(" Sample Number : Lookup Index : Lookup Value : Cogging Current Lookup\n\r\n\r");
for (int i = 0; i<n_joint; i++){ // build lookup table
int joint_ind = (joint_raw_offset>>7) + i;
if(joint_ind > (n_joint-1)){
joint_ind -= n_joint;
}
joint[joint_ind] = (int) ((error_joint_filt[i*NPP] - joint_mean)*(float)(jps->GetCPR())/(2.0f*PI));
printf("%d %d %d %f\n\r", i, joint_ind, joint[joint_ind], cogging_joint_current[i]);
wait(.001);
}
jps->WriteLUT(joint); // write lookup table to position sensor object
//memcpy(controller->cogging, cogging_current, sizeof(controller->cogging)); //compensation doesn't actually work yet....
memcpy(&ENCODER_JOINT, joint, sizeof(joint));
printf("\n\rJoint Encoder Electrical Offset (rad) %f\n\r", joint_offset);
if (!prefs->ready()) prefs->open();
prefs->flush(); // write offset and lookup table to flash
prefs->close();
}
/*
void j_calibrate(PositionSensorMA700 *jps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs)
{
/// Measures the electrical angle offset of the position sensor
/// and (in the future) corrects nonlinearity due to position sensor eccentricity
printf("Starting joint calibration procedure\n\r");
const int n = 128*NPP; // number of positions to be sampled per mechanical rotation. Multiple of NPP for filtering reasons (see later)
const int n2 = 50; // increments between saved samples (for smoothing motion)
float delta = 2*PI*NPP/(n*n2); // change in angle between samples
float error_joint_f[n] = {0}; // error vector rotating forwards
float error_joint_b[n] = {0}; // error vector rotating backwards
const int n_joint = 128;
int joint[n_joint]= {0}; // clear any old lookup table before starting.
jps->WriteLUT(joint);
int joint_raw_f[n] = {0};
int joint_raw_b[n] = {0};
float theta_ref = 0;
float theta_joint_ref = 0;
float theta_joint_actual = 0;
//float v_d = .15f;
float v_d = .2f; // Put volts on the D-Axis
float v_q = 0.0f;
float v_u, v_v, v_w = 0;
float dtc_u, dtc_v, dtc_w = .5f;
///Set voltage angle to zero, wait for rotor position to settle
abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages
svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation
for(int i = 0; i<40000; i++){
TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u); // Set duty cycles
if(PHASE_ORDER){
TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);
TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
}
else{
TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);
TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
}
wait_us(100);
}
jps->Sample(DT);
controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset); //Calculate phase currents from ADC readings
controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
controller->i_a = -controller->i_b - controller->i_c;
dq0(controller->theta_elec, controller->i_a, controller->i_b, controller->i_c, &controller->i_d, &controller->i_q); //dq0 transform on currents
float current = sqrt(pow(controller->i_d, 2) + pow(controller->i_q, 2));
printf(" Current Angle : Rotor Angle : Raw Encoder \n\r\n\r");
for(int i = 0; i<n; i++){ // rotate forwards
for(int j = 0; j<n2; j++){
//theta_ref += delta;
//theta_joint_ref+=delta;
theta_joint_ref+=delta;
abc(theta_joint_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages
svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation
TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u);
if(PHASE_ORDER){ // Check phase ordering
TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v); // Set duty cycles
TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
}
else{
TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);
TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
}
wait_us(100);
jps->Sample(DT);
}
jps->Sample(DT);
theta_joint_actual = jps->GetMechPositionFixed();
error_joint_f[i] = -theta_joint_ref*(1+1/GR)/NPP -theta_joint_actual;
// error_joint_f[i] = (theta_joint_ref-(1/GR)*theta_ref)/NPP -theta_joint_actual;
joint_raw_f[i] = jps->GetRawPosition();
printf("%.4f %.4f %d\n\r", theta_joint_ref*(1+1/GR)/(NPP), theta_joint_actual, joint_raw_f[i]);
// printf("%.4f %.4f %d\n\r", (theta_joint_ref-(1/GR)*theta_ref)/NPP, theta_joint_actual, joint_raw_f[i]);
//theta_ref += delta;
}
for(int i = 0; i<n; i++){ // rotate backwards
for(int j = 0; j<n2; j++){
//theta_ref -= delta;
theta_joint_ref-=delta;
//theta_joint_ref+=delta;
abc( theta_joint_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages
svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation
TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u);
if(PHASE_ORDER){
TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);
TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
}
else{
TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);
TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
}
wait_us(100);
jps->Sample(DT);
}
jps->Sample(DT);
theta_joint_actual = jps->GetMechPositionFixed();
error_joint_b[i] = -theta_joint_ref*(1+1/GR)/NPP - theta_joint_actual;
//error_joint_b[i] = (theta_joint_ref-(1/GR)*theta_ref)/NPP -theta_joint_actual;
joint_raw_b[i] = jps->GetRawPosition();//-raw_b[i]/GR;
printf("%.4f %.4f %d-\n\r", theta_ref*(1+1/GR)/(NPP), theta_joint_actual, joint_raw_b[i]);
// printf("%.4f %.4f %d\n\r", (theta_joint_ref-(1/GR)*theta_ref)/NPP, theta_joint_actual, joint_raw_b[i]);
//theta_ref -= delta;
}
float joint_offset=0;
for(int i = 0; i<n; i++){
joint_offset += (error_joint_f[i] + error_joint_b[n-1-i])/(2.0f*n); // calclate average position sensor joint offset
}
joint_offset = fmod( joint_offset*NPP, 2*PI); // convert mechanical angle to electrical angle
jps->SetElecOffset(joint_offset); // Set position joint sensor offset
__float_reg[8] = joint_offset;
JOINT_E_OFFSET = joint_offset;
/// Perform filtering to linearize position sensor eccentricity
/// FIR n-sample average, where n = number of samples in one electrical cycle
/// This filter has zero gain at electrical frequency and all integer multiples
/// So cogging effects should be completely filtered out.
float joint_error[n]={0};
const int window = 128;
float error_joint_filt[n] = {0};
float cogging_joint_current[window] = {0};
float joint_mean=0;
for (int i = 0; i<n; i++){ //Average the forward and back directions
joint_error[i]= 0.5f*(error_joint_f[i] + error_joint_b[n-i-1]);
}
for (int i = 0; i<n; i++){
for(int j = 0; j<window; j++){
int ind = -window/2 + j + i; // Indexes from -window/2 to + window/2
if(ind<0){
ind += n;
} // Moving average wraps around
else if(ind > n-1) {
ind -= n;
}
error_joint_filt[i] += joint_error[ind]/(float)window;
}
if(i<window){
cogging_joint_current[i] = current*sinf((joint_error[i] - error_joint_filt[i])*NPP);
}
//printf("%.4f %4f %.4f %.4f\n\r", error[i], error_filt[i], error_f[i], error_b[i]);
joint_mean += error_joint_filt[i]/n;
}
int joint_raw_offset =(joint_raw_f[0] + joint_raw_b[n-1])/2; //Insensitive to errors in this direction, so 2 points is plenty
printf("\n\r Encoder non-linearity compensation table\n\r");
printf(" Sample Number : Lookup Index : Lookup Value : Cogging Current Lookup\n\r\n\r");
for (int i = 0; i<n_joint; i++){ // build lookup table
int joint_ind = (joint_raw_offset>>7) + i;
if(joint_ind > (n_joint-1)){
joint_ind -= n_joint;
}
joint[joint_ind] = (int) ((error_joint_filt[i*NPP] - joint_mean)*(float)(jps->GetCPR())/(2.0f*PI));
printf("%d %d %d %f\n\r", i, joint_ind, joint[joint_ind], cogging_joint_current[i]);
wait(.001);
}
jps->WriteLUT(joint); // write lookup table to position sensor object
//memcpy(controller->cogging, cogging_current, sizeof(controller->cogging)); //compensation doesn't actually work yet....
memcpy(&ENCODER_JOINT, joint, sizeof(joint));
printf("\n\rJoint Encoder Electrical Offset (rad) %f\n\r", joint_offset);
if (!prefs->ready()) prefs->open();
prefs->flush(); // write offset and lookup table to flash
prefs->close();
}*/