Ben Katz
Mini Cheetah Actuator Branch Superseded by: https://github.com/bgkatz/motorcontrol
Dependencies: mbed-dev-f303 FastPWM3
Superseded by: https://github.com/bgkatz/motorcontrol
Diff: Calibration/calibration.cpp
- Revision:
- 25:f5741040c4bb
- Parent:
- 24:58c2d7571207
- Child:
- 26:2b865c00d7e9
--- a/Calibration/calibration.cpp Fri Apr 07 16:23:39 2017 +0000
+++ b/Calibration/calibration.cpp Sun Apr 09 03:05:52 2017 +0000
@@ -13,17 +13,17 @@
printf("\n\r Checking phase ordering\n\r");
float theta_ref = 0;
float theta_actual = 0;
- float v_d = .2; //Put all volts on the D-Axis
+ float v_d = .2; //Put all volts on the D-Axis
float v_q = 0.0;
float v_u, v_v, v_w = 0;
float dtc_u, dtc_v, dtc_w = .5;
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
+ 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 = 0x708*(1.0f-dtc_u); // Set duty cycles
+ TIM1->CCR3 = 0x708*(1.0f-dtc_u); // Set duty cycles
TIM1->CCR2 = 0x708*(1.0f-dtc_v);
TIM1->CCR1 = 0x708*(1.0f-dtc_w);
wait_us(100);
@@ -31,7 +31,7 @@
//ps->ZeroPosition();
ps->Sample();
wait_us(1000);
- float theta_start = ps->GetMechPosition(); //get initial rotor position
+ float theta_start = ps->GetMechPosition(); //get initial rotor position
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;
@@ -40,14 +40,14 @@
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
+ 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 = 0x708*(1.0f-dtc_u); //Set duty cycles
+ TIM1->CCR3 = 0x708*(1.0f-dtc_u); //Set duty cycles
TIM1->CCR2 = 0x708*(1.0f-dtc_v);
TIM1->CCR1 = 0x708*(1.0f-dtc_w);
- ps->Sample(); //sample position sensor
+ ps->Sample(); //sample position sensor
theta_actual = ps->GetMechPosition();
if(sample_counter > 200){
sample_counter = 0 ;
@@ -71,26 +71,26 @@
/// 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 = 10; // 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 = 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
int raw_f[n] = {0};
int raw_b[n] = {0};
float theta_ref = 0;
float theta_actual = 0;
- float v_d = .2; // Put volts on the D-Axis
+ float v_d = .2; // Put volts on the D-Axis
float v_q = 0.0;
float v_u, v_v, v_w = 0;
float dtc_u, dtc_v, dtc_w = .5;
///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
+ 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 = 0x708*(1.0f-dtc_u); // Set duty cycles
+ TIM1->CCR3 = 0x708*(1.0f-dtc_u); // Set duty cycles
if(PHASE_ORDER){
TIM1->CCR2 = 0x708*(1.0f-dtc_v);
TIM1->CCR1 = 0x708*(1.0f-dtc_w);
@@ -107,14 +107,14 @@
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));
- for(int i = 0; i<n; i++){ // rotate forwards
+ 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
+ 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 = 0x708*(1.0f-dtc_u);
- if(PHASE_ORDER){
- TIM1->CCR2 = 0x708*(1.0f-dtc_v);
+ if(PHASE_ORDER){ // Check phase ordering
+ TIM1->CCR2 = 0x708*(1.0f-dtc_v); // Set duty cycles
TIM1->CCR1 = 0x708*(1.0f-dtc_w);
}
else{
@@ -132,11 +132,11 @@
//theta_ref += delta;
}
printf(" Current Angle : Rotor Angle : Raw Encoder \n\r\n\r");
- for(int i = 0; i<n; i++){ // rotate backwards
+ 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
+ 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 = 0x708*(1.0f-dtc_u);
if(PHASE_ORDER){
TIM1->CCR2 = 0x708*(1.0f-dtc_v);
@@ -149,8 +149,8 @@
wait_us(100);
ps->Sample();
}
- ps->Sample(); // sample position sensor
- theta_actual = ps->GetMechPosition(); // get mechanical position
+ ps->Sample(); // sample position sensor
+ theta_actual = ps->GetMechPosition(); // get mechanical position
error_b[i] = theta_ref/NPP - theta_actual;
raw_b[i] = ps->GetRawPosition();
printf("%.4f %.4f %d\n\r", theta_ref/(NPP), theta_actual, raw_b[i]);
@@ -159,33 +159,33 @@
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 += (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
+ offset = fmod(offset*NPP, 2*PI); // convert mechanical angle to electrical angle
- ps->SetElecOffset(offset); // Set position sensor offset
+ 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 should be completely filtered out.
+ /// 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
+ 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
+ int ind = -window/2 + j + i; // Indexes from -window/2 to + window/2
if(ind<0){
- ind += n;} // Moving average wraps around
+ ind += n;} // Moving average wraps around
else if(ind > n-1) {
ind -= n;}
error_filt[i] += error[ind]/(float)window;
@@ -196,12 +196,12 @@
//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
+ int raw_offset = (raw_f[0] + raw_b[n-1])/2; //Insensitive to errors in this direction, so 2 points is plenty
const int n_lut = 128;
int lut[n_lut];
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
+ 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;
@@ -210,14 +210,14 @@
printf("%d %d %d %f\n\r", i, ind, lut[ind], cogging_current[i]);
}
- ps->WriteLUT(lut); // write lookup table to position sensor object
+ 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));
+ 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();
- //prefs->close();
+ prefs->flush(); // write offset and lookup table to flash
+ prefs->close();
}
\ No newline at end of file