Aditya Mehrotra
Modified Motor Driver Firmware to include Flash + Thermal
Dependencies: FastPWM3 mbed-dev-STM-lean
Diff: Calibration/calibration.cpp
- Revision:
- 22:60276ba87ac6
- Child:
- 23:2adf23ee0305
diff -r 7d1f0a206668 -r 60276ba87ac6 Calibration/calibration.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Calibration/calibration.cpp Fri Mar 31 18:24:46 2017 +0000
@@ -0,0 +1,201 @@
+/// Calibration procedures for determining position sensor offset,
+/// phase ordering, and position sensor linearization
+///
+
+#include "calibration.h"
+
+
+void order_phases(PositionSensor *ps, GPIOStruct *gpio){
+ ///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 = .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
+ for(int i = 0; i<20000; i++){
+ 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);
+ }
+ //ps->ZeroPosition();
+ ps->Sample();
+ float theta_start = ps->GetMechPosition(); //get initial rotor position
+
+ /// 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 = 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
+ theta_actual = ps->GetMechPosition();
+ 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->GetMechPosition();
+ 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("Phaseing correct\n\r");}
+ else if(!direction){printf("Phasing incorrect. Swapping phases V and W\n\r");}
+ gpio->phasing = direction;
+
+ }
+
+
+
+void calibrate(PositionSensor *ps, GPIOStruct *gpio){
+ /// 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 = 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
+ 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_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
+ for(int i = 0; i<40000; i++){
+ TIM1->CCR3 = 0x708*(1.0f-dtc_u); // Set duty cycles
+ if(gpio->phasing){
+ TIM1->CCR2 = 0x708*(1.0f-dtc_v);
+ TIM1->CCR1 = 0x708*(1.0f-dtc_w);
+ }
+ else{
+ TIM1->CCR1 = 0x708*(1.0f-dtc_v);
+ TIM1->CCR2 = 0x708*(1.0f-dtc_w);
+ }
+ wait_us(100);
+ }
+ ps->Sample();
+
+ 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 = 0x708*(1.0f-dtc_u);
+ if(gpio->phasing){
+ TIM1->CCR2 = 0x708*(1.0f-dtc_v);
+ TIM1->CCR1 = 0x708*(1.0f-dtc_w);
+ }
+ else{
+ TIM1->CCR1 = 0x708*(1.0f-dtc_v);
+ TIM1->CCR2 = 0x708*(1.0f-dtc_w);
+ }
+ wait_us(100);
+ ps->Sample();
+ }
+ ps->Sample();
+ theta_actual = ps->GetMechPosition();
+ error_f[i] = theta_ref/NPP - theta_actual;
+ raw_f[i] = ps->GetRawPosition();
+ printf("%.4f %.4f %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 = 0x708*(1.0f-dtc_u);
+ if(gpio->phasing){
+ TIM1->CCR2 = 0x708*(1.0f-dtc_v);
+ TIM1->CCR1 = 0x708*(1.0f-dtc_w);
+ }
+ else{
+ TIM1->CCR1 = 0x708*(1.0f-dtc_v);
+ TIM1->CCR2 = 0x708*(1.0f-dtc_w);
+ }
+ wait_us(100);
+ ps->Sample();
+ }
+ 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]);
+ //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
+ printf("Encoder Electrical Offset (rad) %f\n\r", offset);
+
+ ps->SetElecOffset(offset); // Set position sensor 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 also be completely filtered out.
+
+ float error[n] = {0};
+ int window = 128;
+ float error_filt[n] = {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;
+ }
+ //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
+ const int n_lut = 128;
+ int lut[n_lut];
+ 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 %d\n\r", raw_offset>>7, i, ind, lut[ind]);
+ }
+ ps->WriteLUT(lut); // write lookup table to position sensor object
+
+
+
+
+
+
+ }
\ No newline at end of file