Ben Katz
auto-measurements
Dependencies: FastPWM3 mbed-dev
Fork of
Hobbyking_Cheetah_Compact
by 
Ben Katz
Diff: Calibration/calibration.cpp
- Revision:
- 34:47a55f96fbc4
- Parent:
- 28:8c7e29f719c5
--- a/Calibration/calibration.cpp Wed Aug 30 18:10:27 2017 +0000
+++ b/Calibration/calibration.cpp Mon Oct 02 00:55:39 2017 +0000
@@ -6,6 +6,13 @@
#include "foc.h"
#include "PreferenceWriter.h"
#include "user_config.h"
+#include "math.h"
+#include "math_ops.h"
+
+void measure_rl(int n, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs){
+
+
+ }
void order_phases(PositionSensor *ps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs){
@@ -14,7 +21,7 @@
printf("\n\r Checking phase ordering\n\r");
float theta_ref = 0;
float theta_actual = 0;
- float v_d = .25; //Put all volts on the D-Axis
+ float v_d = 2.0; //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;
@@ -22,33 +29,33 @@
///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
+ svm(V_BUS, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); //space vector modulation
+ TIM1->CCR3 = (PWM_ARR)*(0.5f); // Set duty cycles
+ TIM1->CCR2 = (PWM_ARR)*(0.5f);
+ TIM1->CCR1 = (PWM_ARR)*(0.5f);
+ wait_us(100);
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);
+ wait_us(50);
+ TIM1->CCR3 = (PWM_ARR)*(1.0f-dtc_u); // Set duty cycles
+ TIM1->CCR2 = (PWM_ARR)*(1.0f-dtc_v);
+ TIM1->CCR1 = (PWM_ARR)*(1.0f-dtc_w);
}
+
+
//ps->ZeroPosition();
ps->Sample();
wait_us(1000);
//float theta_start = ps->GetMechPosition(); //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
+ while(theta_ref < 4.0f*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
+ svm(V_BUS, 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);
+ TIM1->CCR3 = (PWM_ARR)*(1.0f-dtc_u); //Set duty cycles
+ TIM1->CCR2 = (PWM_ARR)*(1.0f-dtc_v);
+ TIM1->CCR1 = (PWM_ARR)*(1.0f-dtc_w);
ps->Sample(); //sample position sensor
theta_actual = ps->GetMechPosition();
if(theta_ref==0){theta_start = theta_actual;}
@@ -61,6 +68,9 @@
}
float theta_end = ps->GetMechPosition();
int direction = (theta_end - theta_start)>0;
+ float ratio = abs(4.0f*PI/(theta_end-theta_start));
+ int pole_pairs = (int) roundf(ratio);
+
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");}
@@ -86,7 +96,7 @@
int raw_b[n] = {0};
float theta_ref = 0;
float theta_actual = 0;
- float v_d = .25; // Put volts on the D-Axis
+ float v_d = 2.0f; // 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;
@@ -94,39 +104,34 @@
///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
+ svm(V_BUS, 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
+ TIM1->CCR3 = (PWM_ARR)*(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);
+ TIM1->CCR2 = (PWM_ARR)*(1.0f-dtc_v);
+ TIM1->CCR1 = (PWM_ARR)*(1.0f-dtc_w);
}
else{
- TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);
- TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
+ TIM1->CCR1 = (PWM_ARR)*(1.0f-dtc_v);
+ TIM1->CCR2 = (PWM_ARR)*(1.0f-dtc_w);
}
wait_us(100);
}
ps->Sample();
- 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);
+ svm(V_BUS, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation
+ TIM1->CCR3 = (PWM_ARR)*(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);
+ TIM1->CCR2 = (PWM_ARR)*(1.0f-dtc_v); // Set duty cycles
+ TIM1->CCR1 = (PWM_ARR)*(1.0f-dtc_w);
}
else{
- TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);
- TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
+ TIM1->CCR1 = (PWM_ARR)*(1.0f-dtc_v);
+ TIM1->CCR2 = (PWM_ARR)*(1.0f-dtc_w);
}
wait_us(100);
ps->Sample();
@@ -143,15 +148,15 @@
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);
+ svm(V_BUS, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation
+ TIM1->CCR3 = (PWM_ARR)*(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);
+ TIM1->CCR2 = (PWM_ARR)*(1.0f-dtc_v);
+ TIM1->CCR1 = (PWM_ARR)*(1.0f-dtc_w);
}
else{
- TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);
- TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
+ TIM1->CCR1 = (PWM_ARR)*(1.0f-dtc_v);
+ TIM1->CCR2 = (PWM_ARR)*(1.0f-dtc_w);
}
wait_us(100);
ps->Sample();
@@ -183,7 +188,6 @@
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]);
@@ -197,9 +201,7 @@
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;
}
@@ -207,19 +209,18 @@
printf("\n\r Encoder non-linearity compensation table\n\r");
- printf(" Sample Number : Lookup Index : Lookup Value : Cogging Current Lookup\n\r\n\r");
+ printf(" Sample Number : Lookup Index : Lookup Value\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]);
+ printf("%d %d %d\n\r", i, ind, lut[ind]);
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);