Shao Rui
1
Dependencies: mbed-dev-f303 FastPWM3
Diff: Joint_Calibration/joint_calibration.cpp
- Revision:
- 48:1b51771c3647
- Child:
- 49:7eac11914980
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Joint_Calibration/joint_calibration.cpp Fri Feb 07 11:31:37 2020 +0000
@@ -0,0 +1,201 @@
+/// Calibration procedures for determining position sensor offset,
+/// phase ordering, and position sensor linearization
+///
+
+#include "joint_calibration.h"
+#include "foc.h"
+#include "PreferenceWriter.h"
+#include "user_config.h"
+#include "motor_config.h"
+#include "current_controller_config.h"
+#include "MA700Sensor.h"
+
+
+void joint_calibrate(PositionSensorMA700 *jps,PositionSensorAM5147 *ps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs){
+
+
+ printf("Starting joint calibration procedure !\n\r");
+ const int n = 120*NPP; // number of positions to be sampled per mechanical rotation. Multiple of NPP for filtering reasons (see later)
+ const int n2 = 50*GR; // increments between saved samples (for smoothing motion)
+ float delta = 2*PI*NPP*GR/(n*n2); // change in angle between samples
+ float error_f[n] = {0}; // error vector rotating forwards(error between motor and joint)
+ float error_b[n] = {0}; // error vector rotating backwards(error between motor and joint)
+ const int n_joint = 120;
+ int joint[n_joint]= {0}; // clear any old lookup table before starting.
+ jps->WriteLUT(joint);
+ int raw_f[n] = {0};
+ int raw_b[n] = {0};
+ float theta_ref = 0;
+ float theta_actual = 0;
+ float joint_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);
+ 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(" Joint offset starting !\n\r\n\r");
+
+ /*************同时设置转子和关节零位置同步****************/
+ ps->SetMechOffset(0);
+ jps->SetMechOffset(0);
+ ps->Sample(DT);
+ jps->Sample(DT);
+ wait_us(20);
+ M_OFFSET = ps->GetMechPosition();
+ JOINT_M_OFFSET =jps->GetMechPosition();
+ if (!prefs->ready()) prefs->open();
+ prefs->flush(); // Write new prefs to flash
+ prefs->close();
+ prefs->load();
+ ps->SetMechOffset(M_OFFSET);
+ jps->SetMechOffset(JOINT_M_OFFSET );
+ printf("\n\r Saved new zero position: %.4f\n\r\n\r", M_OFFSET);
+ printf("\n\r Saved new zero position1: %.4f\n\r\n\r",JOINT_M_OFFSET );
+
+ /*************同时设置转子和关节零位置同步****************/
+ 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);
+ jps->Sample(DT);
+ }
+ ps->Sample(DT);
+ jps->Sample(DT);
+ theta_actual =(1.0f/GR)* ps->GetMechPosition();
+ joint_theta_actual=jps->GetMechPosition();
+ error_f[i] = theta_actual-joint_theta_actual;
+ raw_f[i] = jps->GetRawPosition();
+ printf("%.4f %.4f %d\n\r", theta_actual, joint_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);
+ jps->Sample(DT);
+ }
+ ps->Sample(DT);
+ jps->Sample(DT);
+
+ theta_actual =(1.0f/GR)* ps->GetMechPosition();
+ joint_theta_actual=jps->GetMechPosition();
+ error_b[i] = theta_actual-joint_theta_actual;
+ raw_b[i] = jps->GetRawPosition();
+ printf("%.4f %.4f %d\n\r", theta_actual, joint_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 joint position sensor offset
+ __float_reg[8]= offset;
+ //JOINT_OFFSET = offset;
+
+ /// Perform filtering to linearize joint position sensor eccentricity
+ /// FIR n-sample average, where n = number of samples in one 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 = 120;
+ 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_joint; i++){ // build lookup table
+ int ind = (raw_offset>>7) + i;
+ if(ind > (n_joint-1)){
+ ind -= n_joint;
+ }
+ joint[ind] = (int) ((error_filt[i*NPP] - mean)*(float)(jps->GetCPR())/(2.0f*PI));
+ printf("%d %d %d %f\n\r", i, ind, joint[ind], cogging_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)); // copy the lookup table to the flash array
+ printf("\n\rEncoder Joint Offset (rad) %f\n\r", offset);
+
+ if (!prefs->ready()) prefs->open();
+ prefs->flush(); // write offset and lookup table to flash
+ prefs->close();
+
+
+ }
\ No newline at end of file